基于ros超声波避障仿真
经过我大一半年学习ROS，我也初步了解了ROS，并且学会了机器人模型的制造、激光雷达的仿真环境的实现、并且了解了使用激光雷达slam建图、路径规划等，由于疫情原因我们学长自己买的带激光雷达的小车没有放在实验室，并且由于激光雷达不便宜，使用我突发奇想使用使用超声波来进行避障。
首先我们需要搭建一个仿真机器人----这里我使用的是我学习视频的仿真机器人（【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili）在这里我将 激光雷达换成了超声波雷达。
Gazebo/Yellow
Gazebo/Red
Gazebo/Red
然后我们创建两个小型雷达
Gazebo/Black
接下来我们需要在gazebo中仿真超声波
0 0 0 0 0 0
true
10
8
1
-0.14
0.14
10
1
-0.14
0.14
0.05
3
0.1
0.01
true
10
ultrasonic_sensor
ultrasonic_sensor
0.1
ultrasound
-0.1308
0.1308
这里需要创建三个超声波雷达仿真文件（其中两边的雷达需要调角度）
然后将他们集成进launch文件
然后再创建一个功能包：ultrasonic_obstacle_avoidance 添加依赖：
geometry_msgs
roscpp
sensor_msgs
修改package.xaml文件
ultrasonic_obstacle_avoidance
0.0.0
The ultrasonic_obstacle_avoidance package
rosnoetic
TODO
catkin
geometry_msgs
roscpp
sensor_msgs
geometry_msgs
roscpp
sensor_msgs
roscpp
sensor_msgs
geometry_msgs
sensor_msgs
geometry_msgs
roscpp
sensor_msgs
然后再src下面创建文件：main.cpp（这里可以参考一下Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程）2.5章
#include "ros/ros.h"
#include "sensor_msgs/Range.h"
#include "geometry_msgs/Twist.h"
#define setbit(x, y) x|=(1<range);
range_array[1] = msg->range;
}
void sonar1_callback(const sensor_msgs::Range::ConstPtr& msg)
{
ROS_INFO("left Sonar1 range:[%f]", msg->range);
range_array[0] = msg->range;
}
void sonar2_callback(const sensor_msgs::Range::ConstPtr& msg)
{
ROS_INFO("right Sonar2 range:[%f]", msg->range);
range_array[2] = msg->range;
}
void publishTwistCmd(double linear_x, double angular_z)
{
twist_cmd.linear.x = linear_x;
twist_cmd.linear.y = 0.0;
twist_cmd.linear.z = 0.0;
twist_cmd.angular.x = 0.0;
twist_cmd.angular.y = 0.0;
twist_cmd.angular.z = angular_z;
twist_pub.publish(twist_cmd);
}
void checkSonarRange(double ultrasonic_sensor, double ultrasonic_xiaosensor, double ultrasonic_xiaoxiaosensor)
{
unsigned char flag = 0;
if(ultrasonic_sensor < warn_range)
{
setbit(flag, 2);
}
else
{
clrbit(flag, 2);
}
if(ultrasonic_xiaosensor < warn_range)
{
setbit(flag, 1);
}
else
{
clrbit(flag, 1);
}
if(ultrasonic_xiaoxiaosensor < warn_range)
{
setbit(flag, 0);
}
else
{
clrbit(flag, 0);
}
ROS_INFO("CheckSonarRange get status:0x%x", flag);
switch(flag)
{
case STATUS_A: //右转 0x04
ROS_WARN("left warn,turn right");
publishTwistCmd(0, -default_yaw_rate);
break;
case STATUS_B: // 0x02
ROS_WARN("front warn, left and right ok, compare left and right value to turn");
if(ultrasonic_sensor > ultrasonic_xiaosensor)
{
ROS_WARN("turn left");
publishTwistCmd(0, default_yaw_rate);
}
else
{
ROS_WARN("turn right");
publishTwistCmd(0, -default_yaw_rate);
}
break;
case STATUS_C: //turn left
ROS_WARN("left ok, front ok, right warn, turn left");
publishTwistCmd(0, default_yaw_rate);
break;
case STATUS_D:
ROS_WARN("left,front,right all warn, turn back");
publishTwistCmd(0, 10*default_yaw_rate);
break;
case STATUS_E:
ROS_WARN("left warn, front warn, right ok, turn right");
publishTwistCmd(0, (-default_yaw_rate*2));
break;
case STATUS_F:
ROS_WARN("left ok, front warn, right warn, turn left");
publishTwistCmd(0, (default_yaw_rate*2));
break;
case STATUS_G:
ROS_WARN("left and right warn, front ok, speed up");
publishTwistCmd(2*default_linear_x, 0);
break;
default: //直线前进
publishTwistCmd(default_linear_x, 0);
break;
}
}
int main(int argc, char **argv)
{
setlocale(LC_ALL,"");
//初始化ROS节点
ros::init(argc, argv, "ultrasonic_obstacle_avoidance_node");
//创建ROS句柄
ros::NodeHandle handle;
ros::Rate loop_rate = default_period_hz;
ros::Subscriber sub_sonar0 = handle.subscribe("/ultrasonic_sensor", 100, sonar0_callback);
ros::Subscriber sub_sonar1 = handle.subscribe("/ultrasonic_xiaosensor", 100, sonar1_callback);
ros::Subscriber sub_sonar2 = handle.subscribe("/ultrasonic_xiaoxiaosensor", 100, sonar2_callback);
twist_pub = handle.advertise("/cmd_vel", 10);
while(ros::ok())
{
checkSonarRange(range_array[0], range_array[1], range_array[2]);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
} 然后确定launch文件及rosrun ultrasonic_obstacle_avoidance ultrasonic_obstacle_avoidance_node
2022-04-04-15-41-05