获取雷达传感器的数据
首先利用rostopic list查看激光雷达数据的节点
查看雷达数据
rostopic echo /tianracer/scan
这将显示 一个持续的流 ,这个流就是话题上 LaserScan消息,绝大部分是 LaserScan消息 的ranges ,这就是我们要处理的内容
里面有提前设定好的信息
ranges数组包含了 到最近障碍物的距离,从数组元素的序号可以算出这个障碍物的角度。
下面是官方给出的发布的消息类型
订阅 /tianracer/sacn topic
初始化定义节点
ros::init(argc, argv,"sensor_node" );
创建节点句柄
ros::NodeHandle n;
ros::Subscriber test_sub = n.subscribe( "/tianracer/scan", 10, Lidarcallback);
构造回调函数
//回调函数
void Lidarcallback(const sensor_msgs::LaserScan msg)
{
}
在main()函数里加轮询函数
ros::spinOnce();
求前方障碍物的距离
向量第1个元素 就是 前方的距离 179 是 后方的距离
float ahead= msg.ranges[0];
求 ranges 的向量长度
值得一提的是ranges msg里 对的类型是
msg里float32[ ] 变量是 向量 类型,不是数组
所以求长度时不能用sizeof(),而是直接变量.size() 就行
std::vector<float> ranges = msg.ranges;
float len=msg.ranges.size();
求最近和最远障碍物的距离
此问题转化为 求 ros 里面 vector 的 向量里的最小元素与最大元素 及其位置
ROS 可以调用 std 库下的 min_element()函数 ,来求向量里最小元素
min_element()函数 需要输入两个参数 起始地址 和 结束地址
直接用取址 运算符
float* closest_range = std::min_element(&ranges[0], &ranges[len-1]) ;
最远距离同理
float* farthest_range = std::max_element(&ranges[0], &ranges[len-1]) ;
最后输出结果
最后,在编译时要确保修改的CMakeLists文件中的命令顺序正确如下,不然可能会导致编译出错或者编译正确但是找不到节点
find_package()
catkin_package()
include_directories()
add_executable()
target_link_libraries()