文章目录

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="115200"/><!--A1/A2 -->
<!--param name="serial_baudrate"     type="int"    value="256000"--><!--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）显示结果。

参考资料：
[1] cacrle. ROS如何进行开发？; 2023-04-10 [accessed 2023-04-13].
[2] cacrle. ROS开发之如何使用发布者、订阅者和话题消息？; 2023-04-12 [accessed 2023-04-13].
[3] cacrle. ROS开发之如何制作launch启动文件？; 2023-04-11 [accessed 2023-04-13].
[4] cacrle. ROS开发之如何使用RPLidar A1二维激光雷达？; 2023-04-11 [accessed 2023-04-13].
[5] cacrle. ROS开发之如何将树莓派采集的雷达、IMU数据在虚拟机rviz中显示？; 2023-04-12 [accessed 2023-04-13].
[6] cacrle. ROS开发之如何将RPLidar数据在RViz中三维显示？; 2023-04-12 [accessed 2023-04-13].
[7] cacrle. Windows与Linux 之间如何进行文件共享？; 2023-03-27 [accessed 2023-04-13].
[8] 十年前与现在. ros-同时订阅与发布; 2020-08-07 [accessed 2023-04-13].
[9] 白巧克力亦唯心. ROS 基础: 在同一个节点里订阅和发布消息; 2015-05-07 [accessed 2023-04-13].
[10] cyliujc. ros同时接收多话题并发布; 2017-12-04 [accessed 2023-04-13].
[11] 小萌是个球. ros::spin() 和 ros::spinOnce() 区别及详解 ; 2016-10-01 [accessed 2023-04-13].