2.使用STDR进行ROS小车超声波避障策略仿真实验

转自链接：http://www.corvin.cn/573.html

0x00 什么是仿真实验

在没有真实物理实体实验设备的情况下，通过在计算机上用软件模拟真实设备运行的效果，这就叫做软件仿真实验，用软件仿真是一条可行性高、测试成本低、实验效率高的一条路。就拿ROS小车的避障策略设计来说，如果我们设计了一个避障策略想要实验一下效果如何的话，一般都需要先做一个真实的ROS小车，这样就需要花费不少的钱。当在真实的小车上实验时还会遇到小车电池电量不够、运行时间短、测试环境不够完善等各种问题，有时为了测试一次避障策略程序就需要准备好长时间使小车处于正常工作状态才能开始实验，费时费力。

如果我们使用软件模拟出真实小车的各种参数、各种运动状态的话，我们将自己的程序在仿真中跑起来就方便多了，STDR就是这么一款可以仿真ROS小车在二维平面移动的软件，仿真软件中的小车上传感器发布的数据跟真实的小车发布的话题数据基本上是一样的，那么我们的避障策略程序就可以很好的在仿真软件上进行实验修改完善，当在仿真环境中没有问题的话，最后再在真实的小车上运行，这样就会有事半功倍的效果。

---

0x01 仿真小车介绍

当使用如下命令来启动STDR仿真软件时会自动的加载一个ROS仿真差速驱动小车，我们可以把它当作跟turtlebot一样的小车即可，只有线速度x和角速度z：

roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch

使用如下命令即可在rviz中打开该仿真软件，稍微配置一下即可得到如下图所示的小车超声波传感器探测范围：

roslaunch stdr_launchers rviz.launch

---

0x02 设计超声波避障策略

由于该ROS机器人上面装有５个超声波传感器，为了在本次实验中减轻设计策略的复杂度我们只使用机器人前面的三个传感器，分别是正前方、左侧、右侧传感器，下面的流程图是设计的使用三个超声波传感器来避障的程序设计流程图（该避障策略仅供参考，只是为了实验需要而设计的，在真实的环境中我们可能需要设计更为复杂完善的避障策略），如果下图看不清楚的话可以查看以下附件：

超声波避障策略.jpeg

---

0x03 熟悉超声波话题及其消息格式

首先来认识一下超声波话题和消息格式，在首先启动了仿真器后接下来通过如下图所示命令来操作：

对于sensor_msgs/Range.msg的具体详细描述可以从roswiki的官网获得，网址如下：

http://mirror.umd.edu/roswiki/doc/diamondback/api/sensor_msgs/html/msg/Range.html

输出当前超声波话题内容,可以看到与定义的格式内容是匹配的:

---

0x04 获取一个超声波的测距输出

我们先以正前方的超声波sonar_0来演示如何编写代码获取其测距输出值,我们首先创建自己的超声波避障软件包ultrasonic_obstacle_avoidance,在工作空间的src目录下使用如下命令来创建:

catkin_create_pkg ultrasonic_obstacle_avoidance roscpp sensor_msgs

接下来进入刚才创建的软件包下的src目录下创建main.cpp,输入如下代码,该代码可以读取robot0上的sonar_0的测距值并打印输出:

/**************************************************************
 *Copyright(C): ROS小课堂 www.corvin.cn
 *FileName: main.cpp
 *Author: corvin
 *Date: 20180330
 *Description:在STDR上进行超声波避障策略仿真,实验代码.
 *
 **************************************************************/
#include "ros/ros.h"
#include "sensor_msgs/Range.h"
void sonar0_callback(const sensor_msgs::Range::ConstPtr& msg)
{
    ROS_INFO("Sonar0 range:[%f]", msg->range);
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "ultrasonic_obstacle_avoidance_node");
    ros::NodeHandle handle;
    ros::Subscriber sub_sonar0 = handle.subscribe("/robot0/sonar_0", 100, sonar0_callback);
    ros::spin();
    return 0;
}

然后修改CMakeLists.txt配置文件,按照如下图所示修改:

最后就可以来编译该代码了,在catkin_ws目录下执行catkin_make编译,然后source devel/setup.bash配置好环境变量,如下操作:

corvin@workspace:~/catkin_ws$ catkin_make

corvin@workspace:~/catkin_ws$ source devel/setup.bash

编译完成后就可以来运行查看效果了,首先启动STDR仿真软件,然后在启动打印超声波数据的节点,分别在两个终端下执行如下两条命令:

corvin@workspace:~/catkin_ws$ roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch

corvin@workspace:~/catkin_ws$ rosrun ultrasonic_obstacle_avoidance ultrasonic_obstacle_avoidance_node

当启动节点后可以发现已经有打印数据,当我们使用键盘来遥控小车四处走动时可以发现该数据也跟着变动,启动键盘遥控节点命令如下:

corvin@workspace:~$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

---

0x05 实现完整避障策略进行仿真

现在已经可以获取单个超声波传感器的输出了,接下来我们就按照前面的程序设计流程图来获取其他传感器输出实现完整的避障代码,完整代码如下所示:

/**************************************************************
 *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;
}

在以上源码中需要注意以下几点,方便大家更容易理解代码:

(1)使用unsigned char的变量的低3bit来记录三个超声波传感器测距值是否到达警戒值,高5bit不使用,如果我们有更多的超声波的话就可以用上了.对于低3bit来说,为了方便大家更容易的与三个位置的超声波传感器相对应,因此就规定3bit中最高位存放左边超声波传感器的报警标志,中间bit放置正前方超声波传感器报警标志,最低bit放置右边超声波传感器报警标志,如下所示:

    (高5bit不存储任何标志)   (左边)  (正前方)  (右边)

X    X    X    X    X    0     0     0

(2)使用一个double数组range_array来存储三个超声波传感器每次回调回来的测距值,同样的为了方便与三个超声波位置匹配,在range_array[0]存储的是左边sonar的测距值,range_array[1]存储的是正前方sonar的测距值,range_array[2]存储的是右边sonar的测距值.  

下面准备编译该代码,由于我们新增了控制小车移动的消息geometry_msgs/Twist.msg格式,因此需要相应的修改CMakeLists.txt和package.xml文件,首先是修改CMakeLists.txt文件,按照如下图修改即可:

修改package.xml文件,按照如下图所示修改:

接下来就可以来重新编译代码了,编译完成后重新启动STDR仿真器,然后在启动我们的避障节点程序来查看效果:

通过上面的仿真效果来看,我们的机器人在三个超声波测距模块下基本上可以进行自由的避障行走,但是仿真中难免也会遇到其他问题,像下图这种振荡判断,会导致机器人一直在一个地方左右摇摆:

其实在上面出现的这个问题是由于我们的代码设计有问题,我们通过仿真就可以模拟出该效果,省了我们在真实的小车上来测试效果了,那下面来说出现该现象的原因:

(1)当小车正前方的超声波测距显示距离太近时,会来判断左右两边的超声波测距值哪个更大一点,哪边大一点就说明那边肯定更空一点.

(2)例如我们判断左侧超声波数据大点,那就往左边转动,但是刚转过去一点点,在ROS的主循环中又开始下一轮判断超声波数据了.

(3)由于刚才是往左边转了一点,这样就导致右边的数据肯定就比左边的又大了,那在本轮的避障中又控制小车往右边转动了.

(4)这样就会出现上面动图出现的现象了,小车在不停的左右摆动.

对于出现这种现象既然我们已经知道了产生的原因,那么就需要来找到解决办法来消灭掉这个bug,大家可以在上面提供的源码基础上来修改完善,算是作为大家的课后习题了,而且第一位小伙伴解决了该问题并在公众号中给我发消息留言的话,就会有奖励啊,大家来开动脑子想想更好的避障策略来遍历整个房间且不会撞到墙上吧,有了这个STDR就可以很容易的验证大家的避障策略是否可行了.

---

0x06 参考资料

[1].STDR在ROS WiKi上主页地址[OL]. http://wiki.ros.org/stdr_simulator/Tutorials

[2].sensor_msgs/Range.msg在ROS WiKi网站上的定义[OL]. http://mirror.umd.edu/roswiki/doc/diamondback/api/sensor_msgs/html/msg/Range.html

[3].evarobot_sonar读取超声波数据的实例教程[OL]. http://wiki.ros.org/evarobot_sonar/Tutorials/indigo/Writing%20a%20Simple%20Subscriber%20for%20Sonar%20Sensor