[ROS] 自定义消息创建及订阅

 

创建msg消息

参考:
CreatingMsgAndSrv

首先创建一个空的package单独存放msg类型(当然也可以在任意的package中自定义msg类型)
这里为便于说明,建立一个名为test_msgs的包,用于对自定义msg类型的用法举例

$ cd catkin_ws/src
$ catkin_create_pkg test_msgs

然后在test_msgs中创建msg文件夹,在msg文件夹其中新建一个名为Test.msg消息类型文件

$ cd test_msgs
$ mkdir msg

Test.msg的内容如下,
基本类型可参考: std_msgscommon_msgs

float32[] data
float32 vel
geometry_msgs/Pose pose
string name

修改package.xml
接下来需要message_generation生成C++或Python能使用的代码,需要message_runtime提供运行时的支持,所以package.xml中添加以下两句

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

CMakeLists.txt要注意四个地方

(1)首先调用find_package查找依赖的包,必备的有roscpp rospy message_generation,其他根据具体类型添加,比如上面的msg文件中用到了geometry_msgs/Pose pose类型,那么必须查找geometry_msgs

find_package(catkin REQUIRED COMPONENTS roscpp rospy message_generation std_msgs geometry_msgs)

(2)然后是add_message_files,指定msg文件

add_message_files(
  FILES
  Test.msg
  # Message2.msg
)


(3)然后是generate_messages,指定生成消息文件时的依赖项,比如上面嵌套了其他消息类型geometry_msgs,那么必须注明

#generate_messages必须在catkin_package前面
generate_messages(
 DEPENDENCIES
 geometry_msgs
)


(4)然后是catkin_package设置运行依赖

catkin_package(

CATKIN_DEPENDS message_runtime

)

到这里新的msg类型test_msgs/Test就可以使用了,下面编译这个包,然后利用rosmsg show指令查看

$ cd catkin_ws
$ catkin_make
$ rosmsg show test_msgs/Test 
float32[] data
float32 vel
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
string name

编译创建的消息时报错:

ModuleNotFoundError: No module named 'em'

 

其他包调用自定义msg类型

参考:
DefiningCustomMessages
如果是在test_msgs包内的节点中调用test_msgs/Test类型,只需要在.cpp文件中如下调用即可

#include <test_msgs/Test.h>

test_msgs::Test msg;

如果是在其他包调用test_msgs/Test类型则需要修改package.xml和CMakeLists.txt,比如同样在工作空间catkin_ws内有一个名为test的包,我们可以在这个包内写一个节点,使用我们刚才自定义的消息类型test_msgs/Test,如下:

(1)修改package.xml
养成好习惯,维护软件包清单的更新,以便于别人使用你的软件前安装各种依赖项,当然这个文件不影响程序编译

<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>

<build_depend>test_msgs</build_depend>
<run_depend>test_msgs</run_depend>

(2)修改CMakeLists.txt
调用自定义消息类型主要修改两个地方,以下是重点:
一是find_package中需要声明查找包含该消息类型的包;
二是add_dependencies要注明该消息的依赖,其他地方和普通节点一样

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  test_msgs
)
add_dependencies(test1 test_msgs_gencpp)#调用同一工作空间的自定义消息类型时注明依赖关系,防止发生头文件找不到的报错


如果缺少add_dependencies中对test_msgs_gencpp的依赖声明,在编译的时候如果先编译test包再编译test_msgs包则会出现如下报错(ROS工作空间各个软件包的编译顺序是随机的),因为头文件test_msgs/Test.h还未生成

fatal error: test_msgs/Test.h: 没有那个文件或目录
 #include "test_msgs/Test.h"

(3) 对于ORBSLAM扩展的工程只有CMakeLists.txt情况.

ORB-SLAM2的ROS包中CMakeLists.txt布局与一般的不同,同时缺少package.xml,编译也没有使用catkin_make,许多原因造成了ROS和该消息缺少了部分联系。解决办法就是在其它的包中按正常流程编译好消息,然后在ORB-SLAM2的源码中引用该头文件。具体需要修改的文件以及流程如下:

  1. 在工作空间catkin_ws/src下新建一个功能包(或使用已有的功能包),在msg/中定义好需要的消息,按正常流程修改好CMakeLists.txtpackage.xml,编译。
  2. 修改/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,在include_directories中加入1中编译好的消息头文件路径,我这里是catkin_ws/devel/include
  3. 修改ros_rgbd.cc,需要include该消息的头文件,即rosmsg show得到的地址。

最后编译ORB-SLAM,即可使用该消息了。

msg类型数组的使用

需要说明的是std_msgs中有些数组用法和C++标准库中不一样
比如Float32MultiArray,其定义如下:

std_msgs/MultiArrayLayout layout
float32[] data

其中data是一个浮点数组,但是方括号只是一个用来表明它是数组的符号,我们不能在定义的时候在方括号中给定数组长度,实际上ROS中类似float32[]int8[]这样的数组类型都是std::vector,使用方法也和std::vector一样.

消息发布节点

/************************  
 * @Author: Jinglin Zhang  
 * @DateTime: 2017-06-07 20:03:35  
 * @Description: 节点talker,发布"test_msg"话题,用于测试test_msgs::Test消息类型  
************************/ 
#include <ros/ros.h>
#include <test_msgs/Test.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "msg_talker");
  ros::NodeHandle n;
  ros::Publisher msg_pub = n.advertise<test_msgs::Test>("test_msg", 1000);
  ros::Rate loop_rate(10);
  int count = 0;

  while (ros::ok())
  {
    test_msgs::Test msg;
    std::cout << "msg.data.size=" << msg.data.size() << std::endl;

    //用vector给float32[]数组赋值
    float array[3] = {1.1,1.2,0.3};
    std::vector<float> array1(array,array+3);
    msg.data = array1;
    std::cout << "msg.data3[0]=" << msg.data[0] << std::endl;
    std::cout << "msg.data3.size=" << msg.data.size() << std::endl;

    //下标访问float32[]数组
    msg.data[0] = 0.1;
    std::cout << "msg.data3[0]=" << msg.data[0] << std::endl;


    float array4[4] = {1.0,2.0,0.3,6.6};
    std::vector<float> array41(array4,array4+4);
    msg.data = array41;
    std::cout << "msg.data4.size=" << msg.data.size() << std::endl;
    std::cout << "msg.data4=" << msg.data[0] << " " << msg.data[1] <<  " " << msg.data[2] << " " <<  msg.data[3] << std::endl;

    msg.data.push_back(5.5);
    std::cout << "msg.data[5]=" << msg.data[4] << std::endl;

    //使用迭代器
    msg.data.resize(6);
    std::cout << "msg.data6.size=" << msg.data.size() << std::endl;
    std::cout << "msg.data6=" ;
    for(std::vector<float>::iterator it = msg.data.begin(); it != msg.data.end(); ++it)
    {
      *it = 0.6;
      std::cout << *it << " ";
    }
    std::cout << std::endl;

    msg_pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

  return 0;
}

消息订阅节点

/************************  
 * @Author: Jinglin Zhang  
 * @DateTime: 2017-06-07 19:57:30  
 * @Description: 节点test1,订阅了test_msgs包下talker节点发布的"test_msg"话题,用于测试test_msgs::Test消息类型  
************************/
#include <ros/ros.h>
#include <test_msgs/Test.h>

void msgCallback(const test_msgs::Test::ConstPtr &msg)
{
  //test_msgs::Test类型里的float32[]数据传到vector
  std::vector<float> array = msg->data;

  std::cout << "msg->data[0]=" << msg->data[0] << std::endl;
  std::cout << "msg->data.size=" << msg->data.size() << std::endl;
  std::cout << "msg->data=" << msg->data[0] << ", " << msg->data[1] <<  ", " << msg->data[2] << ", " <<  msg->data[3] << ", " <<  msg->data[4] << ", " <<  msg->data[5] << std::endl;

}

int main(int argc,char ** argv)
{
  ros::init(argc,argv,"test1");
  ros::NodeHandle n;

  ros::Subscriber msg_sub = n.subscribe("test_msg", 100, msgCallback);

  ros::spin();
  return 0;
}

Mat与消息数组(vector)间的转换,参考

(1)将数组内容传递给Mat,示例代码:

 Mat Kframe(fheight, fwidth, CV_8UC1);
	for(int row=0; row < fheight; row++)
	{
	    uchar *ptr = Kframe.ptr<uchar>(row);
	    for(int col=0; col < fwidth; col++)
	    {
		ptr[col]=mfvio.data[row*fwidth+col];
	    }
	}

(2)将Mat中的内容传递给数组,如果Mat中的数据是连续的,那么对于传递到一维vector我们可以这样:

                    pose_img.data.resize(im_gray.rows*im_gray.cols);


                    if (im_gray.isContinuous())
                    {
                        int height = im_gray.rows;
                        int width = im_gray.cols;
                        int sum=0;
                        for(int row=0; row < height; row++)
                        {
                            const uchar *ptr = im_gray.ptr(row);
                            for(int col=0; col < width; col++)
                            {
                                pose_img.data[row*width+col] = ptr[0];
                                ptr += 1;
                            }
                        }
                    }

(3)同样的,传递到一维数组我们可以这样

unsigned char *array=new unsigned char[mat.rows*mat.cols];
 
if (mat.isContinuous())
    array = mat.data;

 

 

 

参考:

https://blog.csdn.net/u013453604/article/details/72903398

https://blog.csdn.net/guyuealian/article/details/80253066

https://blog.csdn.net/weixin_42075471/article/details/85988995

https://alexander-tsui.github.io/ros/2019/07/08/AddROSCustomizedMsgToORBSLAM/

https://blog.csdn.net/qq_39575818/article/details/90272706

 

  • 3
    点赞
  • 16
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值