学习ROS基础的电子笔记,摘录自各类ROS基础课程,文末会附上本次内容参考的相关课程视频链接。
今天的内容主要围绕着自主实现话题。之前关于publisher和subscriber的例子实现依赖于turtkesim的话题Twist和Pose,但是当我们需要编写满足自己需求的话题时,将需要自定义话题。第十三讲的示例时实现一个新的话题/person_info及一种新的消息类型learning_topic::Person,并编写person_publisher和person_subscriber来完成Person消息类型的通讯。
1. 自定义话题消息
- 定义msg文件;
// 在工作空间特定目录下新建msg文件夹(与message相关的topic)
cd ~/catkin_ws/src/learning_topic
mkdir msg
vim msg/Person.msg
// Person.msg文件中内容
string name
uint8 sex
uint8 age
uint8 unknown = 0
uint8 male = 1
uint8 female = 2
uint可以看作时无符号整形变量类型unsigned int,其缩写就是uint。根据不同的编译规则,uint8会被编译成各种形式。
- 在package.xml 中添加功能包依赖
vim package.xml
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
- 在CMakeLists.txt 中添加编译选项
vim CMakeLists.txt
// 在文件中添加关于新topic的信息即可
find_package(...... message_generation)
add_message_file(FILES Person.msg)
generate_message(DEPENDENCIES std_msgs)
catkin_package(CATKIN_DEPENDS...message_runtime)
还是找到原注释行进行删改。
- 编译生成相关文件
cd ~/catkin_ws
catkin_make
编译完成效果如下:
可以在devel文件下的include文件中看到生成learning_topic文件及其下文件夹。
2.编写person_publisher
// 明确文件路径,可以按照实际情况编程
vim ~/catkin_ws/src/learning_topic/src/person_publisher.cpp
/**
* 该例程将发布/person_info话题,自定义消息类型learning_topic::Person
*/
#include <ros/ros.h>
#include "learning_topic/Person.h"
int main(int argc, char** argv){
// ROS节点初始化
ros :: init (argc, argv, "person_publisher") ;
// 创建句柄
ros :: NodeHandle n ;
// 创建一个Publisher,发布名为/person_info的topic,消息类型为learning_topic :: Person,队列长度为10
ros :: Publisher person_info_pub = n.advertise<learning_topic :: Person>("/person_info",10) ;
// 设置循环的频率
ros :: Rate loop_rate(1) ;
int count = 0 ;
//主要功能实现
while(ros :: ok()){
//初始化learning_topic :: Person类型的消息
learning_topic :: Person person_msg ; // 新建一个learning::Person类型的对象person_msg
person_msg.name = "Alice" ;
person_msg.age = 18 ;
person_msg.sex = learning_topic::Person::female ; // 针对msg文件
内编写female = 2
// 发布消息
person_info_pub.publish(person_msg) ;
ROS_INFO("Publish Person Info: name: %s age: %d, sex: %d", person_msg.name, person_msg.age, person_msg.sex) ;
// 按照循环频率延时
loop_rate.sleep() ;
}
return 0 ;
}
3.编写person_subscriber
// 明确文件路径,可以按照实际情况编程
vim ~/catkin_ws/src/learning_topic/src/person_subscriber.cpp
/**
* 该历程将订阅/person_info话题,自定义消息类型learning_topic:: Person
*/
#include<ros/ros.h>
#include"learning_topic/Person.h"
// 接收订阅的消息后,会进入消息回调函数
void personInfoCallback(const learning_topic :: Person :: ConstPtr& msg){
// 将接收到的消息打印出来
ROS_INFO("Subscribe Person Info: name: %s, age: %d, sex: %d", msg->name.c_str(), msg->age, msg->sex) ;
}
int main(int argc, char** argv){
// 初始化ROS节点
ros :: init(argc, argv, "person_subscriber") ;
// 创建节点句柄
ros :: NodeHandle n ;
//创建一个Subscriber, 订阅名为/person_info的topic, 注册回调函数personInfoCallbacke
ros :: Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback) ;
//循环等待回调函数
ros :: spin() ;
return 0 ;
4.配置CMakeLists.txt中的编译规则
vim ~/catkin_ws/src/learning_topic/CMakeLists.txt
add_executable(person_publisher src/person_publisher.cpp)
add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_publisher
${catkin_LIBRARIES}
)
target_link_libraries(person_subscriber
${catkin_LIBRARIES}
)
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)
cd ~/catkin_ws
catkin_make
5.执行程序
// teminal1
roscore
//terminal2
source ~/catkin_ws/devel/setup.bash
rosrun learning_topic person_subscriber
//terminal3
source ~/catkin_ws/devel/setup.bash
rosrun learning_topic person_publisher
实现效果如下图:
在完成实验后,关掉roscore也不影响两个节点之间的通信,这进一步证明rosmaster只是负责建立各种节点间联系的媒介,当节点联系建立完成,就不会受rosmaster的影响。当然,如果需要对这种联系进行修改,抑或加入新的节点,那么还是需要rosmaster的帮助。
参考课程: ROS入门21讲(古月居-第13讲)