对python和C++版本的ROS自定义消息调试

Step1：工程构建

先在CMakeLists.txt中要填有这几项

cmake_minimum_required(VERSION 3.0.2)
project(simple_ros_test)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

 add_message_files(
   FILES
   Parent.msg
   Child.msg
 )

 generate_messages(
   DEPENDENCIES
   std_msgs
 )

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES simple_ros_test
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

include_directories(
# include
  ../../devel/include/
  ${catkin_INCLUDE_DIRS}
)

add_executable(rosmsg_test_node src/rosmsg_test_main.cpp)
target_link_libraries(rosmsg_test_node
   ${catkin_LIBRARIES}
)

在package.xml中记得添加

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

Step2：自定义消息

比如：Parent.msg中定义

Child[] childs

在Chlid.msg中定义

string Toys
string Name
uint8 Age

其含义是一个父母有很多孩子，而每个孩子有玩具，名字，年龄等元素。

Step3：Python代码

代码

#!/usr/bin/env python
import rospy
import sys
sys.path.insert(0, "/home/wzy/vins_ws/devel/lib/python2.7/dist-packages") #  这一步要加，不然报错ImportError: No module named relative_pose_backend.msg
from relative_pose_backend.msg import Parent
from relative_pose_backend.msg import Child

P = Parent()
C1 = Child()
C2 = Child()

C1.Toys = 1
C1.Name = "jerry"
C1.Age = 10
P.childs.append(C1)

C2.Toys = 2
C2.Name = "Tom"
C2.Age = 8
P.childs.append(C2)

print(P)

# 对循环添加子消息,这里以新增Parent中childs数量为例
for i in range(2):
    Ci = Child()  # 这里若要新增对象 要在循环内初始化对象Child()，不能在循环外初始化。和C++中vector push_back()不一样
    Ci.Name = "name" + str(i)
    Ci.Toys = "toy_name" + str(i*2)
    Ci.Age = Ci.Age + 1
    P.childs.append(Ci)
print(P)

Ci = Child()  # 这里若要新增对象 但在在循环外初始化对象Child()，append函数会依据字典方式，将Name，Toys，Age视作键key，
              # 所以添加进去的值都被覆盖为循环中最后一次的值
for i in range(2):
    Ci.Name = "name" + str(i)
    Ci.Toys = "toy_name" + str(i*2)
    Ci.Age = Ci.Age + 1
    P.childs.append(Ci)
print(P)

# 对循环添加子消息,这里以修改Parent中childs属性为例
for i in range(len(P.childs)):
    P.childs[i].Name = "name" + str(i)
    P.childs[i].Age = P.childs[i].Age + 2
print(P)

显示结果

childs: 
  - 
childs: 
  - 
    Toys: "Toys1"
    Name: "jerry"
    Age: 10
  - 
    Toys: "Toys2"
    Name: "Tom"
    Age: 8
childs: 
  - 
    Toys: "Toys1"
    Name: "jerry"
    Age: 10
  - 
    Toys: "Toys2"
    Name: "Tom"
    Age: 8
  - 
    Toys: "toy_name0"
    Name: "name0"
    Age: 1
  - 
    Toys: "toy_name2"
    Name: "name1"
    Age: 1
childs: 
  - 
    Toys: "Toys1"
    Name: "jerry"
    Age: 10
  - 
    Toys: "Toys2"
    Name: "Tom"
    Age: 8
  - 
    Toys: "toy_name0"
    Name: "name0"
    Age: 1
  - 
    Toys: "toy_name2"
    Name: "name1"
    Age: 1
  - 
    Toys: "toy_name2"
    Name: "name1"
    Age: 2
  - 
    Toys: "toy_name2"
    Name: "name1"
    Age: 2
childs: 
  - 
    Toys: "Toys1"
    Name: "name0"
    Age: 12
  - 
    Toys: "Toys2"
    Name: "name1"
    Age: 10
  - 
    Toys: "toy_name0"
    Name: "name2"
    Age: 3
  - 
    Toys: "toy_name2"
    Name: "name3"
    Age: 3
  - 
    Toys: "toy_name2"
    Name: "name5"
    Age: 6
  - 
    Toys: "toy_name2"
    Name: "name5"
    Age: 6

Step4：C++代码简要

代码

#include "simple_ros_test/Parent.h"
#include "ros/ros.h"
#include "iostream"
int main(int argc, char **argv) {
    // Launch our ros node
    ros::init(argc, argv, "rosmsg_test_node");
    ros::NodeHandle nh;
//    ros::NodeHandle nh("~");
    ros::Rate rate(10);

    simple_ros_test::Parent parent;
    simple_ros_test::Child child_0;
    simple_ros_test::Child child_1;

    child_0.Age = 8;
    child_0.Name = "Jerry";
    child_0.Toys = "hhh";

    child_1.Age = 10;
    child_1.Name = "Tom";
    child_1.Toys = "kkk";

    parent.childs.emplace_back(child_0);
    parent.childs.emplace_back(child_1);
    
    //在循环中添加，循环外定义,可以。这一点与python不同
    simple_ros_test::Child child_i;
    for (int i = 0; i < 2; ++i) {
        child_i.Toys = "toy" + std::to_string(i);
        child_i.Name = "name" + std::to_string(i);
        child_i.Age = i;
        parent.childs.emplace_back(child_i);
    }

    //在循环中添加，循环内定义，也可以
    for (int i = 0; i < 2; ++i) {
        simple_ros_test::Child child_j;
        child_j.Toys = "toy" + std::to_string(i*2);
        child_j.Name = "name" + std::to_string(i*2);
        child_j.Age = i*2;
        parent.childs.emplace_back(child_j);
    }

    //打印结果
    for (int i = 0; i < parent.childs.size(); ++i) {
        std::cout << "parent  child = " << std::to_string(parent.childs[i].Age) << "\t" << parent.childs[i].Name << "\t" << parent.childs[i].Toys << std::endl;
    }
    return 0;
}

显示结果

parent  child = 8	Jerry	hhh
parent  child = 10	Tom	kkk
parent  child = 0	name0	toy0
parent  child = 1	name1	toy1
parent  child = 0	name0	toy0
parent  child = 2	name2	toy2

Step5：注意事项

5.1 若报错没有可执行文件

把新建的python文件赋权限chmod a+x *.py，对C++文件生成的节点，重新source devel/setup.bash

5.2 如果显示找不到自定义消息

请查看如下目录有没有消息文件生成

python需要查看$HOME/vins_ws/devel/lib/python2.7/dist-packages/relative_pose_backend/msg/有类似目录结构

.
├── __init__.py
└── msg
    ├── _Child.py
    ├── __init__.py
    └── _Parent.py

C++需要查看$HOME/vins_ws/devel/include/relative_pose_backend/有如下目录结构

.
├── Child.h
├── Parent.h