ROS开发之如何在同一个节点订阅、处理、发布消息？

大家好，欢迎来到IT知识分享网。

0.引言

  在ROS应用一般会用到发布者和订阅者，若只接收传感器数据，则只实现订阅者就行，但有时需要订阅者部分做简单数据处理后，再发布出去，供其他订阅者接收。以上场景涉及一个中间文件，即既是订阅者，也是发布者的文件，本文查阅现有网络资源，复现现有实现方法，在树莓派中利用二维雷达传感器数据实现数据的订阅，做三维处理后发布，并在上位机虚拟机显示三维点云。相较于【ROS开发之如何将RPLidar数据在RViz中三维显示？】的修改原有代码实现订阅和发布的方式，本文创建了一个中间文件实现订阅和发布。

1.创建中间特殊文件（含订阅者和发布者）

  （1）在node.cpp同级目录下新建中间特殊文件middle_node.cpp；
  

  （2）输入以下代码。

/* Copyright 2023 cacrle ( cacrle.blog.csdn.net). */ #include "ros/ros.h" #include <ros/package.h> #include "sensor_msgs/LaserScan.h" #include "std_srvs/Empty.h" #include "rplidar.h" #include <pcl_ros/point_cloud.h> #include <pcl/point_types.h> #include <boost/foreach.hpp> //declare point data formate typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; pcl::PointCloud<pcl::PointXYZ> cloud_msg; //a changable height float count_h = 0; //Avoid redefine #ifndef DEG2RAD #define DEG2RAD(x) ((x)*M_PI/180.) #endif //declare a publisher ros::Publisher cloud_pub; //the callback method, will monitor the topic and receive message void scanCallback(const sensor_msgs::LaserScan::ConstPtr&amp; scan_msg) { 
    int count = scan_msg->scan_time / scan_msg->time_increment; cloud_msg.header.frame_id = "laser"; cloud_msg.height = 1; cloud_msg.width = count; cloud_msg.points.resize(cloud_msg.width * cloud_msg.height); for(int i = 0; i < count; i++) { 
    float degree = RAD2DEG(scan_msg->angle_min + scan_msg->angle_increment * i); cloud_msg.points[i].x = scan_msg->ranges[i] * cos(DEG2RAD(degree)); cloud_msg.points[i].y = scan_msg->ranges[i] * sin(DEG2RAD(degree)); cloud_msg.points[i].z = count_h; //print out data to screen ROS_INFO("[%f, %f, %f]", cloud_msg.points[i].x ,cloud_msg.points[i].y, cloud_msg.points[i].z); } count_h += 0.05; if(count_h>100) { 
    count_h = 0; } //converse ros time to pcl time pcl_conversions::toPCL(ros::Time::now(),cloud_msg.header.stamp); cloud_pub.publish(cloud_msg); } int main(int argc, char **argv) { 
    //initial, "middle_node" is the name of cpp(i.e. midlle_node.cpp) ros::init(argc, argv, "middle_node"); //declare a node ros::NodeHandle n; //declare a subscirber ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("scan", 1000, scanCallback); //give publisher a initial value, "point_cloud2" is topic, 1000 is frequency(unit is hz) cloud_pub = n.advertise<PointCloud>("point_cloud2",1000); //program reach here and stop to next, this code may monitor the scanCallback ros::spin(); return 0; } 

2.在CMakeLists.txt添加编译规则

cmake_minimum_required(VERSION 2.8.3) project(rplidar_ros) set(RPLIDAR_SDK_PATH "./sdk/") FILE(GLOB RPLIDAR_SDK_SRC "${RPLIDAR_SDK_PATH}/src/arch/linux/*.cpp" "${RPLIDAR_SDK_PATH}/src/hal/*.cpp" "${RPLIDAR_SDK_PATH}/src/*.cpp" ) find_package(catkin REQUIRED COMPONENTS roscpp rosconsole sensor_msgs ) include_directories( ${ 
   RPLIDAR_SDK_PATH}/include ${ 
   RPLIDAR_SDK_PATH}/src ${ 
   catkin_INCLUDE_DIRS} /usr/include/pcl-1.7 /usr/include/eigen3 ) catkin_package() add_executable(rplidarNode src/node.cpp ${ 
   RPLIDAR_SDK_SRC}) target_link_libraries(rplidarNode ${ 
   catkin_LIBRARIES}) add_executable(rplidarNodeClient src/client.cpp) target_link_libraries(rplidarNodeClient ${ 
   catkin_LIBRARIES}) add_executable(middle_node src/middle_node.cpp) target_link_libraries(middle_node ${ 
   catkin_LIBRARIES}) install(TARGETS rplidarNode rplidarNodeClient ARCHIVE DESTINATION ${ 
   CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${ 
   CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${ 
   CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY launch rviz sdk DESTINATION ${ 
   CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) 

3.在launch添加启动项

<launch> <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen"> <param name="serial_port" type="string" value="/dev/ttyUSB0"/> <param name="serial_baudrate" type="int" value=""/><!--A1/A2 --> <!--param name="serial_baudrate" type="int" value=""--><!--A3 --> <param name="frame_id" type="string" value="laser"/> <param name="inverted" type="bool" value="false"/> <param name="angle_compensate" type="bool" value="true"/> </node> <node name="middle_node" pkg="rplidar_ros" type="middle_node" output="screen"/> </launch> 

4.编译运行

cd ~/catkin_ws catkin_make 或 catkin_make_isolated source devel/setup.bash 或 source devel_isolated/setup.bash roslaunch rplidar_ros rplidar.launch 

5.三维显示

  （1）RViz参数设置；
  

  （2）显示结果。