Tags: SLAM

本篇博客是慕课《ROS机器人操作系统入门》的第二篇笔记,慕课网址是这里。本篇笔记主要记录Topic在C++和Python下的两种实现方式以及需要注意的问题。

1.任务描述与分析

有一个GPS传感器(模拟)以一定间隔发送位置信息,我们需要获得它发送的位置信息并进行处理,解算当前位置到坐标原点的距离。

在这个场景下,使用自定义消息格式的Topic-Message(publish-subscribe)通信方式比较合适。可以新建一个叫gps_processer的包(package),这个包中有publishersubscriber两个节点(Node),publisher用于获取、发送GPS传感器信息,subscriber用于信息处理。定义Topic的名称是gps_infoMessage的格式是float类型的xy以及string类型的当前工作状态state。模拟数据的生成规则是xy分别从1.0、2.0开始,x=x+1.3,y=y+1.5

2.Topic的C++实现

以下步骤是在创建好了工作空间(本机工作空间路径/root/catkin_ws)的情况下继续的,如果还没有创建好工作空间,可以参考这篇笔记创建工作空间,然后再回来。

(1)创建Package

创建Package有多种方式,这里介绍最原始的手动和使用RoboWare这个IDE的两种不同方式。

a.终端中手动创建

在终端中输入如下内容即可创建一个Package

cd catkin_ws/src/
catkin_create_pkg gps_processer roscpp std_msgs message_generation message_runtime

第一行是切换到src目录下,第二行是在当前目录下创建一个叫gps_processer的包,并且包含roscppstd_msgsmessage_generationmessage_runtime依赖,后两个是用到Message的包都要包含的依赖。

b.RoboWare自动创建

打开RoboWare,在”资源管理器”中找到src文件夹,右键单击选择”新建ROS包”,输入名称即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”,输入roscppstd_msgsmessage_generationmessage_runtime(空格隔开)即可添加依赖。

(2)创建自定义消息文件
a.手动创建文件

gps_processer包的根目录下新建一个msg文件夹,在其中新建一个叫做gps.msg的文件即可。

b.RoboWare创建文件

在”资源管理器”中找到gps_processer文件夹,右键单击选择”新建Msg文件夹”,在新建的msg文件夹上右键选择”新建MSG文件”,输入名称即可创建。

文件创建好后在文件中写入如下内容并保存。

float32 x
float32 y
string state

需要注意的是Message的基本数据类型和常规的C++有些区别。例如在Messageint进一步细分成了int6int32int64float细分成了float32float64。所以在Message文件中如果写int是会报错的。

(3)编译自定义消息文件
a.手动方式

在编译自定义消息文件为头文件之前,需要修改gps_processer包的CMakeLists.txt文件,有以下需要修改的地方。

首先在自动生成的CMakeLists.txt文件中找到被注释掉的add_message_files(FILES message1.msg),将其取消注释,并将自定义的msg文件名称填入,这行命令用于向项目中添加刚刚自定义的消息文件。

然后找到被注释掉的generate_messages(DEPENDENCIES std_msgs)将其取消注释,这行命令用于生成自定义的消息文件。

修改后的文件如下。

b.RoboWare方式

如果是在RoboWare里通过菜单新建的msg文件则无需修改,RoboWare已经自动修改好了CMakeLists.txt中的对应内容。

完成了CMake文件的修改之后即可以调用catkin_make进行编译了。在工作空间的根目录下catkin_make即可。之后我们可以在/root/catkin_ws/devel/include/gps_processer找到生成的gps.h头文件了,后续调用它即可。

(4)创建publisher节点
a.手动方法

在包的根目录下新建一个src文件夹,然后在其中新建一个publisher.cpp文件,并在刚刚修改的CMakeLists.txt文件末尾添加如下内容,用于添加可执行文件:

add_executable(publisher
  src/publisher.cpp
)
add_dependencies(publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(publisher
  ${catkin_LIBRARIES}
)
b.RoboWare方法

除了手动添加,也可以在RoboWare中自动添加。具体做法是在RoboWare的资源管理器中找到gps_processer,在src文件夹上右键,然后选择”新建CPP源文件”,输入名称为publisher.cpp并回车,然后会弹出一个选择的对话框如下。 这是让你指定新建的这个cpp文件是作为一个库文件还是作为一个可执行文件,选择不同选项后,程序会自动帮你在CMakeLists.txt文件中填好相关内容。当然,如果不选也可以,自己手动填写就可以了。这里由于我们是希望生成一个可执行文件的,所以选择”加入到新的可执行文件中”即可。

(5)编写publisher节点代码

对于一个Topic的发布者,需要以下几个步骤:初始化ROS(ros::init())、新建NodeHandle(ros::NodeHandle)、构造Publisher(nh.advertise())、循环发布消息(pub.publish())。具体代码如下:

#include<ros/ros.h> // ROS基本头文件
#include<gps_processer/gps.h>   // 引用自定义生成的消息类型

int main(int argc, char *argv[])
{
    // 设置消息初值
    gps_processer::gps msg;
    msg.x=1.0;
    msg.y=2.0;
    msg.state="working";

    // 初始化一个叫做publisher的节点
    ros::init(argc, argv, "publisher");
    ros::NodeHandle nh; // 新建一个NodeHandle用于管理节点
    // 利用NodeHandle构造一个publisher
    ros::Publisher pub = nh.advertise<gps_processer::gps>("gps_info",1);
    // 设置循环迭代间隔并发布消息
    ros::Rate loop_rate(1.0);
    while (ros::ok())
    {
        msg.x += 1.3;
        msg.y += 1.5;
        ROS_INFO("Publisher:GPS:x = %f,y = %f",msg.x,msg.y);
        pub.publish(msg);
        loop_rate.sleep();
    }
    return 0;
}
(6)创建subscriber节点
a.手动方法

src文件夹中新建一个subscriber.cpp文件,并在CMakeLists.txt文件末尾添加如下内容,用于添加可执行文件:

add_executable(subscriber
  src/subscriber.cpp
)
add_dependencies(subscriber ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(subscriber
  ${catkin_LIBRARIES}
)
b.RoboWare方法

除了手动添加,也可以在RoboWare中自动添加。具体做法是在RoboWare的资源管理器中找到gps_processer,在src文件夹上右键,然后选择”新建CPP源文件”,输入名称为subscriber.cpp并回车。选择”加入到新的可执行文件中”即可。

(7)编写subscriber节点代码

对于一个Topic的接收者,需要以下几个步骤:初始化ROS(ros::init())、新建NodeHandle(ros::NodeHandle)、定义回调函数、构造Subscriber(nh.subscribe())、循环接收消息(ros::spin())。具体代码如下:

#include<ros/ros.h>
#include<gps_processer/gps.h>
#include<std_msgs/Float32.h>    //ROS中的Float32类型

void gpsCallback(const gps_processer::gps::ConstPtr &msg)
{
    std_msgs::Float32 distance;
    distance.data = sqrt(pow(msg->x,2)+pow(msg->y,2));
    ROS_INFO("Subscriber:Distance to origin = %f,state = %s",distance.data,msg->state.c_str());

}

int main(int argc, char *argv[])
{   
    ros::init(argc, argv, "subscriber");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("gps_info",1,gpsCallback);
    ros::spin();
    return 0;
}
(8)编译运行测试

最后在Catkin工作空间根目录下打开终端,使用catkin_make进行编译,编译成功后如下: 然后先输入roscore打开Master,然后在新终端中输入rosrun gps_processer publisher启动publisher节点,新终端中输入rosrun gps_processer subscriber启动subscriber节点。如下: 可以看到成功运行了刚刚写的两个节点,并且实现了消息的收发与处理。最后简单说一下,生成的两个节点的可执行文件放在了/root/catkin_ws/devel/lib/gps_processer里,直接在终端中运行这个可执行文件也是可以的(Master已经运行的情况下)。

3.Topic的Python实现

(1)新建Package
a.手动方法

在终端中输入如下内容即可创建一个Package。

cd catkin_ws/src/
catkin_create_pkg gps_processer rospy std_msgs message_generation message_runtime

第一行是切换到src目录下,第二行是在当前目录下创建一个叫gps_processer的包,并且包含rospystd_msgsmessage_generationmessage_runtime依赖,后两个是用到Message的包都要包含的依赖。

b.RoboWare方法

打开RoboWare,在”资源管理器”中找到src文件夹,右键单击选择”新建ROS包”,输入名称即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”,输入rospystd_msgsmessage_generationmessage_runtime(空格隔开)即可添加依赖。

(2)新建消息文件

与前面自定义消息文件内容一模一样。

(3)编译消息文件

与前面编译自定义消息文件内容一模一样。

(4)新建publisher节点文件

根据之前的习惯,src文件夹里一般放C/C++文件,Python代码一般放在scripts文件夹里。

a.手动方法

package的根目录下创建一个scripts文件夹,然后新建一个publisher.py文件。由于Python是脚本,并不需要CMake编译,因此创建好文件就可以写代码了。

b.RoboWare方法

在包名上右键点击,选择”新建文件夹”,输入名称为scripts,在scripts文件夹上右键选择”新建文件”,输入publisher.py即可。

(5)编写publisher代码

参考之前笔记说过的rospy的API,代码如下。

# coding=utf-8
import rospy
from gps_processer.msg import gps


def gps_pub():
    # 数据的初始化
    x = 1.0
    y = 2.0
    state = 'working'

    # 初始化节点
    rospy.init_node('publisher')
    # 创建发布对象
    pub = rospy.Publisher('gps_info', gps, queue_size=1)

    # 设置循环间隔
    rate = rospy.Rate(1.0)
    while not rospy.is_shutdown():
        x += 1.3
        y += 1.5
        rospy.loginfo('Publisher:GPS:x = %f,y = %f', x, y)
        # 发布消息
        pub.publish(gps(x, y, state))
        rate.sleep()


if __name__ == '__main__':
    gps_pub()
(6)新建subscriber节点文件
a.手动方法

scripts文件夹下创建subscriber.py即可。

b.RoboWare方法

scripts文件夹上右键选择”新建文件”,输入subscriber.py即可。

(7)编写subscriber代码

Python代码如下:

# coding=utf-8
import math
import rospy
from gps_processer.msg import gps


def callback(gps):
    distance = math.sqrt(math.pow(gps.x, 2)+math.pow(gps.y, 2))
    rospy.loginfo('Subscriber:Distance to origin = %f,state = %s',
                  distance, gps.state)


def gps_sub():
    # 初始化节点
    rospy.init_node("subscriber")
    # 开始订阅
    rospy.Subscriber('gps_info', gps, callback)
    # 循环
    rospy.spin()


if __name__ == '__main__':
    gps_sub()
(8)运行测试

最后在终端中输入roscore启动Master,在scripts目录下打开新终端然后分别输入python publisher.py启动发布节点,python subscriber.py启动接收节点。运行结果如下:

4.Topic的C++与Python实现比较

通过上面的介绍可以看到,ROS中Topic的使用总体而言分为四步:新建包、自定义Topic消息、编写代码以及编译运行。

对于C++和Python在实现起来整体步骤基本是相同的,只是由于API差异导致在代码编写这一步差别较大。其它步骤相同或有细微差别,如由于Python不需要编译因此没有catkin_make环节。具体对比如下表:

步骤 C++ Python
创建Package 依赖roscpp 依赖rospy
创建消息文件 相同 相同
编译消息文件 步骤相同,结果不同。生成.h文件,放在catkin_ws/devel/include/pkg_name 步骤相同,结果不同。生成.py文件,放在catkin_ws/devel/lib/python2.7/dist-packages/pkg_name
创建publisher节点 src scripts
编写publisher节点代码 使用C++ API 使用Python API
创建subscriber节点 src scripts
编写subscriber节点代码 使用C++ API 使用Python API
运行测试 运行前需要调用catkin_make编译 无需编译直接运行脚本

如果一个项目中既会用到C++也会用到Python,那么在一开始新建Package的时候就同时依赖roscpprospy,这样方便一些。

最后将所有代码上传到了Github,点击查看

本文作者原创,未经许可不得转载,谢谢配合

返回顶部