说明： 在拿相机玩ORB-SLAM3时涉及到了ROS topic的收发与时间同步问题，而且在得到相机位姿后还需要将位姿信息封装成一定的消息格式发布出去，这就又涉及到自定义消息的使用。以往只是在代码中改改相应的topic的名称就直接运行了，这里参考古月老师的《ROS机器人开发实践》简单学习一下其中的消息收发机制。

1创建个功能包和工作空间

# 创建一个工作空间
mkdir -p ros_test_ws/src
cd ros_test_ws/src/
catkin_init_workspace
cd ../
catkin_make
# 创建一个功能包
cd src/
catkin_create_pkg learining_ros std_msgs rospy roscpp
cd ..
catkin_make
1
2
3
4
5
6
7
8
9
10
11

2 在功能包根目录/msg下，自定义一个消息类型testmsg.msg

msg消息格式

std_msgs/Header header
string name
int32 sex
int32 age
1
2
3
4

在功能包目录下的package.xml文件中设置相关编译和运行的相关依赖

 <build_depend>message_generation</build_depend>
 <run_depend>message_runtime</exec_depend>
1
2

在CMakeLists.txt中添加编译选项message_generation

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

# 下面关于自定义消息的内容要放在catkin_package()之前
add_message_files(
  FILES
  testmsg.msg
)
generate_messages(
  DEPENDENCIES
  std_msgs
)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

3 创建相应的发布者和订阅者cpp文件

源代码见文末

4 编译和运行

catkin_make
# 启动ros节点管理器
roscore
# 启动发布者node
rosrun learining_ros talker
# 新打开一个终端启动订阅者node
rosrun learining_ros listener
1
2
3
4
5
6
7

5 编写launch文件

编写launch文件,通过launch文件同时启动talker和listener两个node

//ros_test.launch
<launch>
  <node name="talker" pkg="learining_ros" type="talker" />
  <node name="listener" pkg="learining_ros" type="listener" />
</launch>
1
2
3
4
5

# 启动launch
roslaunch learining_ros rostest.launch
1
2

# 查看node
rosnode list
1
2

# 查看topic
rostopic list
1
2

# 打印topic信息
rostopic echo /chatter
1
2

6 代码文件

publisher.cpp

//publisher.cpp
#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "learining_ros/testmsg.h"

int main(int argc, char** argv)
{
    //Ros 节点初始化
    ros::init(argc, argv, "talker");

    //创建节点句柄
    ros::NodeHandle n;

    //创建一个publisher，参数依次为chatter->topic名称;1000->消息队列的长度
    // 当发布消息实际速度太慢时，Publisher会将消息储存在一定空间队列中，如果消息数量超过队列大小，ROS会自动删除队列中最早入队的消息
    // 消息类型为std_msgs::String
    ros::Publisher chatter_pub = n.advertise<learining_ros::testmsg>("chatter",1000);

    // 设置循环的频率
    // 单位是hz,此处为10hz，当调用Rate::sleep()时，ROS节点会根据此处设置的频率进行相应的休眠
    ros::Rate loop_rate(10);

    // 初始化消息内容
    std::string name = "Lusx";
    int age = 12;
    int sex = 1;
    
    int count = 0;
    while(ros::ok())
    {
        // 按照自定义的消息类型封装一条topic
        learining_ros::testmsg test_msg;
        test_msg.header.seq = count;
        test_msg.header.stamp = ros::Time::now();
        test_msg.header.frame_id = "test_msg";
        test_msg.sex = sex;
        test_msg.age = age;
        test_msg.name = name;

        // 发布消息
        // 将消息在终端进行打印
        ROS_INFO("---");
        ROS_INFO("  test_msg.header.seq: %d",test_msg.header.seq);
        ROS_INFO("  test_msg.header.stamp.sec: %d",test_msg.header.stamp.sec);
        ROS_INFO("  test_msg.header.stamp.msec: %d",test_msg.header.stamp.nsec);
        ROS_INFO("  test_msg.header.frame_id: %s",test_msg.header.frame_id.c_str());
        ROS_INFO("  test_msg.name = name: %s",test_msg.name.c_str());
        ROS_INFO("  test_msg.sex: %d",test_msg.sex);
        ROS_INFO("  test_msg.age: %d",test_msg.age);
        ROS_INFO("---\n");

        // 将封装好的topic发布出去
        chatter_pub.publish(test_msg);

        //循环等待回调函数
        ros::spinOnce();

        // 按照循环频率延时
        loop_rate.sleep();
        ++count;
    }

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65

listener.cpp

//listener.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "learining_ros/testmsg.h"

// 接受到订阅的消息后会进入消息回调函数
void chatterCallback(const learining_ros::testmsg& test_msg)
{
    // 将接受到的消息打印出来
    ROS_INFO("---");
    ROS_INFO("  test_msg.header.seq: %d",test_msg.header.seq);
    ROS_INFO("  test_msg.header.stamp.sec: %d",test_msg.header.stamp.sec);
    ROS_INFO("  test_msg.header.stamp.msec: %d",test_msg.header.stamp.nsec);
    ROS_INFO("  test_msg.header.frame_id: %s",test_msg.header.frame_id.c_str());
    ROS_INFO("  test_msg.name = name: %s",test_msg.name.c_str());
    ROS_INFO("  test_msg.sex: %d",test_msg.sex);
    ROS_INFO("  test_msg.age: %d",test_msg.age);
    ROS_INFO("---\n");
}

// 初始化ROS节点
// 订阅需要的话题
// 循环等待话题消息，接受到消息后进入回调函数
// 在回调函数中完成消息的处理
int main(int argc, char** argv)
{
    //Ros 节点初始化
    ros::init(argc, argv, "listener");

    //创建节点句柄
    ros::NodeHandle n;

    //创建一个Subscriber，参数依次为chatter->topic名称;1000->消息队列的长度,回调函数的地址
    // 如果回调函数是某个类的成员函数，在回调函数之前还需要加上类对象的地址
    // ros::Subscriber chatter_sub = n.subscriber("chatter",1000,&className::callbackFun, &classobj);
    ros::Subscriber chatter_sub = n.subscribe("chatter",1000, chatterCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

CMakeList.txt文件

cmake_minimum_required(VERSION 3.0.2)
project(learining_ros)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

add_message_files(
  FILES
  testmsg.msg
)
generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
  # INCLUDE_DIRS include
  CATKIN_DEPENDS std_msgs message_runtime
)

include_directories(include/learining_ros ${catkin_INCLUDE_DIRS}) 

add_executable(talker src/publisher.cpp) 
target_link_libraries(talker ${catkin_LIBRARIES}) 

add_executable(listener src/listener.cpp) 
target_link_libraries(listener ${catkin_LIBRARIES}) 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

package.xml文件

<?xml version="1.0"?>
<package format="2">
  <name>learining_ros</name>
  <version>0.0.0</version>
  <description>The learining_ros package</description>
  <maintainer email="lusx@todo.todo">lusx</maintainer>
  <license>TODO</license>
  
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>

</package>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24