#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>
ros::Publisher vel_pub;
void lidar_callback(const sensor_msgs::LaserScan msg){
float fMidDist= msg.ranges[180];
ROS_INFO("Mid Distance: %f",fMidDist);
geometry_msgs::Twist cmd_vel_msg;
if(fMidDist<1.5){
// cmd_vel_msg.linear.x=0.5;
cmd_vel_msg.angular.z=5.0;
}else{
cmd_vel_msg.linear.x=0.05;
// cmd_vel_msg.angular.z=0.5;
}
vel_pub.publish(cmd_vel_msg);
}
int main(int argc,char *argv[]){
setlocale(LC_ALL,"");//解决中文乱码问题
ros::init(argc,argv,"lidar_node");
ros::NodeHandle nh;
ros::Subscriber lidar_sub=nh.subscribe("/scan",10,&lidar_callback);
ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
ros::spin();
return 0;
}
这段ROS避障代码无法运行,代码执行逻辑如下:
向 Master 注册节点
ros::init(argc, argv, "lidar_node");
-
把节点名字登记到
roscore;之后 Master 才知道有“lidar_node”这号人。 -
同时解析命令行里的
__name:=等 ROS 参数。
拿到“句柄”——跟 Master 保持一条 TCP 长连接
ros::NodeHandle nh;
-
所有话题、参数、服务都通过
nh去申请。 -
析构时会自动断开连接,所以不用手动
shutdown()。
-
告诉 Master “我要订
/scan”
ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidar_callback);
-
10 是消息队列长度——如果处理不过来,ROS 会缓存最多 10 帧激光数据,旧帧丢弃。
-
把函数指针
lidar_callback登记给 Master;收到数据时 ROS 内部线程会调用它(回调队列机制)。
告诉 Master “我还要发 cmd_vel”
ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
-
此时还没有真正发消息,只是“占坑”。
-
10 同样是输出队列长度,后面
publish()太快而网络阻塞时,旧消息会被丢掉。 -
注意:这里变量名也叫
vel_pub,把全局变量vel_pub隐藏了(后面会说坑)。
把主线程“让”给 ROS —— 无限等人发数据
ros::spin();
-
相当于
while(ros::ok()) { ros::getGlobalCallbackQueue()->callAvailable(); sleep(); },阻塞执行while循环 -
收到
/scan后,spin 内部会调用lidar_callback。
回调里真正干活:看中间那条激光值 → 决定怎么走 → 发出去
void lidar_callback(const sensor_msgs::LaserScan msg)
{
float fMidDist = msg.ranges[180]; // 取正前方那条射线
ROS_INFO("Mid Distance: %f", fMidDist); // 打印到屏幕 /rosout
geometry_msgs::Twist cmd_vel_msg; // 造一个速度消息
if (fMidDist < 1.5) { // 离墙太近
cmd_vel_msg.angular.z = 5.0; // 纯原地猛转
} else { // 前方空旷
cmd_vel_msg.linear.x = 0.05; // 慢速前进
}
vel_pub.publish(cmd_vel_msg); // ← 这里用到了全局变量
}
-
回调里访问的是全局
vel_pub; -
但
main里又定义了一个局部vel_pub,把全局的遮住了 → 全局变量其实一直是空对象,第一次回调就崩溃(ROS_ASSERT触发)。
改正代码如下:
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>
ros::Publisher vel_pub;
void lidar_callback(const sensor_msgs::LaserScan msg){
float fMidDist= msg.ranges[180];
ROS_INFO("Mid Distance: %f",fMidDist);
geometry_msgs::Twist cmd_vel_msg;
if(fMidDist<1.5){
// cmd_vel_msg.linear.x=0.5;
cmd_vel_msg.angular.z=5.0;
}else{
cmd_vel_msg.linear.x=0.05;
// cmd_vel_msg.angular.z=0.5;
}
vel_pub.publish(cmd_vel_msg);
}
int main(int argc,char *argv[]){
setlocale(LC_ALL,"");//解决中文乱码问题
ros::init(argc,argv,"lidar_node");
ros::NodeHandle nh;
ros::Subscriber lidar_sub=nh.subscribe("/scan",10,&lidar_callback);
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
ros::spin();
return 0;
}
你能发现有什么区别🏇
1235

被折叠的 条评论
为什么被折叠?



