基于STDR的ROS小车超声波避障策略仿真实验

一、二维移动机器人仿真器STDR

STDR是Simple Two Dimentional Robot Simulator的缩写，它可以非常容易的对在二维平面移动的机器人进行仿真，STDR的设计目的不是为了像Gazebo那样的大型逼真的机器人仿真或者一个功能最全面、功能强大的仿真器，这款软件的目的是为了尽可能的简单的去模拟单个移动机器人的运动或者多个机器人的协同仿真。

STDR可以完美的与ROS兼容配套，机器人上的传感器数据都是通过ros话题的形式发布。跟ROS运行一样，STDR的运行也可以将图形用户接口和服务器分开在不同的电脑上运行，而且STDR也可以和Rviz一起工作，在Rviz中将STDR中的机器人数据可视化显示。

实验仿真环境：

• ubuntu18.04
• ROS-melodic

二、搭建仿真系统

创建一个catkin_ws/src，然后…

git clone https://gitee.com/zhankun3280/stdr_simulator
cd ..
catkin_make

source devel/setup.bash
# GUI界面  
roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch 
# RIVZ界面  
roslaunch stdr_launchers rviz.launch

此时，可以看到仿真界面。

启动后在地图的左下角会自动的生成一个机器人，其中红色的线是雷达的扫描输出，绿色的部分是超声波的输出。

如果使用键盘控制Robot的运动，这可以使用下面命令：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/robot0/cmd_vel

三、基于STDR的ROS小车超声波避障策略仿真实验

由于该ROS机器人上面装有５个超声波传感器，为了在本次实验中减轻设计策略的复杂度我们只使用机器人前面的三个传感器，分别是正前方、左侧、右侧传感器。

创建一个catkin_ws/src，然后…

catkin_create_pkg ultrasonic_obstacle_avoidance roscpp sensor_msgs geometry_msgs

cd ultrasonic_obstacle_avoidance/src
cd ultrasonic_obstacle_avoidance/src
touch main.cpp

cd ../../..
catkin_make

main.cpp

/**************************************************************
 *Copyright(C): ROS小课堂 www.corvin.cn
 *FileName: main.cpp
 *Author: corvin
 *Date: 20180330
 *Description:在STDR上进行超声波避障策略仿真,实验代码.
 *
 **************************************************************/
#include "ros/ros.h"
#include "sensor_msgs/Range.h"
#include "geometry_msgs/Twist.h"
 
#define  setbit(x, y)  x|=(1<<y)
#define  clrbit(x, y)  x&=~(1<<y)
 
//low three bit as sonar warn flag
//           left  font right
// x x x x  x  0    0    0
#define  STATUS_A   0x04  // v x x
#define  STATUS_B   0x02  // x v x
#define  STATUS_C   0x01  // x x v
#define  STATUS_D   0x07  // v v v
#define  STATUS_E   0x06  // v v x
#define  STATUS_F   0x03  // x v v
#define  STATUS_G   0x05  // v x v
 
//global variable
geometry_msgs::Twist twist_cmd;
ros::Publisher twist_pub;
 
const double warn_range = 0.5;  //warn check distance
 
double default_period_hz = 10;  //hz
double default_linear_x = 0.5;  // (m/s)
double default_yaw_rate = 0.5;  // rad/s
 
double range_array[3]; //save three sonar value
 
void sonar0_callback(const sensor_msgs::Range::ConstPtr& msg)
{
    ROS_INFO("front Sonar0 range:[%f]", msg->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 sonar_l, double sonar_f, double sonar_r)
{
   unsigned char flag = 0;
 
   if(sonar_l < warn_range)
   {
       setbit(flag, 2);
   }
   else
   {
       clrbit(flag, 2);
   }
 
   if(sonar_f < warn_range)
   {
       setbit(flag, 1);
   }
   else
   {
       clrbit(flag, 1);
   }
 
   if(sonar_r < warn_range)
   {
       setbit(flag, 0);
   }
   else
   {
       clrbit(flag, 0);
   }
 
   ROS_INFO("CheckSonarRange get status:0x%x", flag);
   switch(flag)
   {
    case STATUS_A: //turn right 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(sonar_l > sonar_r)
        {
            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: //go forward straight line
        publishTwistCmd(default_linear_x, 0);
        break;
   }
 
}
 
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "ultrasonic_obstacle_avoidance_node");
    ros::NodeHandle handle;
    ros::Rate loop_rate = default_period_hz;
 
    ros::Subscriber sub_sonar0 = handle.subscribe("/robot0/sonar_0", 100, sonar0_callback);
    ros::Subscriber sub_sonar1 = handle.subscribe("/robot0/sonar_1", 100, sonar1_callback);
    ros::Subscriber sub_sonar2 = handle.subscribe("/robot0/sonar_2", 100, sonar2_callback);
 
    twist_pub = handle.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 10);
 
    while(ros::ok())
    {
       checkSonarRange(range_array[0], range_array[1], range_array[2]);
 
       ros::spinOnce();
       loop_rate.sleep();
    }
 
    return 0;
}

修改CMakeLists.txt

add_executable(${PROJECT_NAME}_node src/main.cpp)

add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})

编译完成后。。。

source devel/setup.bash
roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch

source devel/setup.bash
rosrun ultrasonic_obstacle_avoidance ultrasonic_obstacle_avoidance_node

自主避障效果如下图：

参考链接：
[1] 在ROS-melodic中安装stdr-simulator
[2] １.初识简单的二维移动机器人群仿真器STDR
[3] 2.使用STDR进行ROS小车超声波避障策略仿真实验