在使用ROS的时候,我们会用到很多节点,例如之前的gazebo仿真、hector建图、键盘控制等,这些节点的消息传递主要靠的是话题与订阅。在很多程序中,我们可能需要订阅一些数据,同时我们处理完这些数据后还要再发布出去,这时我们该如何写这个节点:
比如说这里我们接受scan的数据,然后让其数据以scan3的topic重新发送出去,那么可以通过下面的这个程序实现:
#include "sensor_msgs/LaserScan.h"
#include <iostream>
#include <fstream>
#include <test_msgs/Test.h>
#define RAD2DEG(x) ((x)*180./M_PI)
ros::Publisher scan_pub;
//using namespace std;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan2)
{
//ros:Publisher pub_f64;
//std::vector<float>;
//std::vector<float> data=scan2->ranges;
//std::cout<< ranges.size()<<" ";
//int count = scan2->scan_time / scan2->time_increment;
ROS_INFO("I heard a laser scan %s[%d]:", scan2->header.frame_id.c_str(), count);
ROS_INFO("range, %f", scan2->ranges[12]);
unsigned int num_readings = 100;
double laser_frequency = 40;
double ranges[num_readings];
double intensities[num_readings];
// int count = 0;
// ros::Rate r(1.0);
ros::Time scan3_time = ros::Time::now();
sensor_msgs::LaserScan scan3;
scan3.header.frame_id = "laser_frame";
scan3.angle_min = -1.57;
scan3.angle_max = 1.57;
scan3.angle_increment = 3.14 / num_readings;
scan3.time_increment = (1 / laser_frequency) / (num_readings);
//scan3.scan.time=scan2->scan.time;
scan3.range_min = 0.0;
scan3.range_max = 100.0;
scan3.ranges=scan2->ranges;
scan_pub.publish(scan3);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rplidar_node_client");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);
//ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan3", 50);
scan_pub=n.advertise<sensor_msgs::LaserScan>("scan3", 50);
ros::spin();
return 0;
}
这个程序部分来自于:
https://blog.csdn.net/geerniya/article/details/84853077
以及
https://www.codetd.com/article/1836116
剩下的是参考一些零零碎碎的文章搭建而成。这里需要注意到main()中注释掉的一行:
//ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan3", 50);
scan_pub=n.advertise<sensor_msgs::LaserScan>("scan3", 50);
这两者是有区别的,一开始我使用上面一种语法,编译通过但是节点打开后报错:
后来我在这个论坛里找到了一个解答:
https://answers.ros.org/question/321440/error-call-to-publish-on-an-invalid-publisher-using-approximatetime-policy/
其中似乎提到了一个变量的初始化问题,上面的写法新建了一个变量,而下面的写法使用了全局变量。可能如果要使用上面的语法的话publisher函数处理要放到main主函数中来。