基于ros超声波避障仿真

经过我大一半年学习ROS，我也初步了解了ROS，并且学会了机器人模型的制造、激光雷达的仿真环境的实现、并且了解了使用激光雷达slam建图、路径规划等，由于疫情原因我们学长自己买的带激光雷达的小车没有放在实验室，并且由于激光雷达不便宜，使用我突发奇想使用使用超声波来进行避障。

   首先我们需要搭建一个仿真机器人----这里我使用的是我学习视频的仿真机器人（【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili）在这里我将 激光雷达换成了超声波雷达。

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
   <!-- xacro 用于封装urdf以及调用 -->
   <!-- <xacro:property name="xxxx" value="yyyy" />   属性的定义-->
   <xacro:property name="footprit_radius" value="0.001"/>
   <!-- 1.添加base_footprint -->
   <link name="base_footprint">
      <!--描述外观 -->
      <visual>
      <!-- 设置连杆的形状 -->
           <geometry>
              <sphere radius="${footprit_radius}" />
           </geometry>
      </visual>
   </link>

    
     <!-- 底盘搭建
        参数
            形状:圆柱 
            半径:10     cm 
            高度:8      cm 
            离地:1.5    cm
    -->
    <xacro:property name="base_radius" value="0.1" />
    <xacro:property name="base_length" value="0.08" />
    <xacro:property name="base_mass" value="2" />
    <xacro:property name="lidi" value="0.015" />
    <xacro:property name="base_joint_z" value="${base_length / 2 +lidi}" />

    <link name="base_link">
       <visual>
         <geometry>
             <cylinder radius="0.1" length="0.08" />
         </geometry>
         <!-- 设置偏移量和倾斜弧度 xyz= x偏移 y便宜 z偏移     rpy=x翻滚 y俯仰 z偏航 (单位是弧度)-->
         <origin xyz="0 0 0" rpy="0 0 0" />

         <material name="baselink_color">
            <color rgba="1.0 0.5 0.2 0.5"/>
         </material>   
       </visual>
       <collision>
        <geometry>
            <cylinder radius="0.1" length="0.08" />
        </geometry>
         <!-- 设置偏移量和倾斜弧度 xyz= x偏移 y便宜 z偏移     rpy=x翻滚 y俯仰 z偏航 (单位是弧度)-->
        <origin xyz="0 0 0" rpy="0 0 0" />
       </collision>
       <!-- 调用惯性矩阵 -->
       <xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
       </link>
       <gazebo reference="base_link">
          <material>Gazebo/Yellow</material>
       </gazebo>

       <!-- joint 描述机器人关节的运动学和动力学属性  type关节运动形式-->
    <joint name="link2footprint" type="fixed">
       <!-- parent link的名字是一个强制的属性：link:父级连杆的名字，是这个link在机器人结构树中的名字。 -->
           <parent link="base_footprint" />
           <!-- child link的名字是一个强制的属性：link:子级连杆的名字，是这个link在机器人结构树中的名字。 -->
           <child link="base_link" />
           <!-- 在关节z上设置 = 车体高度/2 + 离地高度 -->
           <origin xyz="0 0 0.055" rpy="0 0 0" />
    </joint>

    <!-- 属性 -->
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.015" />
    <xacro:property name="wheel_mass" value="0.05" />
    <xacro:property name="PI" value="3.1415926" />
    <!-- 注意结果是负数 -->
    <xacro:property name="wheel_joint_z" value="${ -base_length / 2 - lidi + wheel_radius}" />
<!-- 
      wheel_name: left 或 right
      flag :1 或 -1
 -->

<xacro:macro name="wheel_func" params="wheel_name flag">
      <link name="${wheel_name}_wheel">
          <visual>
              <geometry>
                  <cylinder radius="${wheel_radius}" length="${wheel_length}" />
              </geometry>
              <origin xyz="0 0 0" rpy="${PI / 2} 0 0" /> 
              <material name="wheel_color">
                  <color rgba="0 0 0 0.3" />
              </material>
          </visual>
          <collision>
                <geometry>
                  <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>
                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" /> 
          </collision>
          <xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
      </link>
    <gazebo reference="${wheel_name}_wheel">
    <material>Gazebo/Red</material>
    </gazebo>

      <joint name="${wheel_name}2link" type="continuous">
          <parent link="base_link" />
          <child link="${wheel_name}_wheel" />
          <origin xyz="0 ${0.1 * flag} ${wheel_joint_z}" rpy="0 0 0"/>
          <axis xyz="0 1 0" />
      </joint>

  </xacro:macro>

<xacro:wheel_func wheel_name="left" flag="1" />
<xacro:wheel_func wheel_name="right" flag="-1" />


<!-- 添加万向轮(支撑轮) -->
    <!--
        参数
            形状: 球体
            半径: 0.75 cm
            颜色: 黑色

        关节设置:
            x = 自定义(底盘半径 - 万向轮半径) = 0.1 - 0.0075 = 0.0925(cm)
            y = 0
            z = 底盘长度 / 2 + 离地间距 / 2 = 0.08 / 2 + 0.015 / 2 = 0.0475 
            axis= 1 1 1

    -->

    <xacro:property name="small_wheel_radius" value="0.0075" />
    <xacro:property name="small_wheel_mass" value="0.01" />
    <!-- z的偏移量 = 车体高度 / 2  + 离地间距 - 万向轮半径  -->
    <xacro:property name="small_joint_z" value="${(base_length / 2 + lidi - small_wheel_radius) * -1}" />

    <xacro:macro name="small_wheel_func" params="small_wheel_name flag">

    <link name="${small_wheel_name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${small_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="wheel_color">
                <color rgba="0 0 0 0.3" />
            </material>
        </visual>
        <collision>
            <geometry>
             <sphere radius="${small_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />                
        </collision>
        <xacro:sphere_inertial_matrix m="${small_wheel_mass}" r="${small_wheel_radius}" />
    </link>
    <gazebo reference="${small_wheel_name}_wheel">
    <material>Gazebo/Red</material>
    </gazebo>

    <joint name="${small_wheel_name}2link" type="continuous">
        <parent link="base_link" />
        <child link="${small_wheel_name}_wheel" />
        <origin xyz="${0.08 * flag} 0 ${small_joint_z}" rpy="0 0 0"/>
        <axis xyz="0 1 0" />
    </joint>
</xacro:macro>
<xacro:small_wheel_func small_wheel_name="front" flag="1"/>
<xacro:small_wheel_func small_wheel_name="back" flag="-1"/>

</robot>

   然后我们创建两个小型雷达

<robot name="xiao_ultrasonic" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:macro name="ultrasonic" params="prefix:=ultrasonic">
    <!-- 超声波雷达属性 -->
    <xacro:property name="ultrasonic_xiaolength" value="0.05" /> <!-- 超声波雷达长度 -->
    <xacro:property name="ultrasonic_xiaoradius" value="0.01" /> <!-- 超声波雷达半径 -->
    <xacro:property name="ultrasonic_xiaomass" value="0.15" /> <!-- 超声波雷达质量 -->
    <xacro:property name="ultrasonic_xiaoz" value="${base_length / 2 + ultrasonic_xiaolength / 2}" /> <!-- 超声波雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  -->
    <link name="ultrasonic_xiaosensor">
        <visual>
            <geometry>
               <cylinder radius="${ultrasonic_xiaoradius}" length="${ultrasonic_xiaolength}" />
            </geometry>
            <!-- 设置偏移量和倾斜弧度 xyz= x偏移 y便宜 z偏移     rpy=x翻滚 y俯仰 z偏航 (单位是弧度)-->
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${ultrasonic_xiaoradius}" length="${ultrasonic_xiaolength}" />
            </geometry>
        </collision>
        <xacro:cylinder_inertial_matrix m="${ultrasonic_xiaomass}" r="${ultrasonic_xiaoradius}" h="${ultrasonic_xiaolength}"/>
    </link>
    <gazebo reference="ultrasonic_xiaosensor">
        <material>Gazebo/Black</material>
    </gazebo>

    <joint name="ultrasonic_xiao" type="fixed">
        <parent link="base_link" />
        <child link="ultrasonic_xiaosensor" />
        <origin xyz="0 0.08 ${ultrasonic_xiaoz}" />
    </joint>
    </xacro:macro>
    <xacro:ultrasonic prefix="ultrasonic"/> 
</robot>

接下来我们需要在gazebo中仿真超声波

<?xml version="1.0"?>
<robot name="my_ultrasonic" xmlns:xacro="http://www.ros.org/wiki/xacro">
        <gazebo reference="ultrasonic_sensor">
          <sensor type="ray" name="ultrasonic_sensor">
            <pose>0 0 0 0 0 0</pose>
            <visualize>true</visualize>    <!-- 是否可视化射线 -->
            <update_rate>10</update_rate>     <!-- 测距频率 -->
            <ray>
              <scan>
                <horizontal>    <!-- 水平方向 -->
                  <samples>8</samples>
                  <resolution>1</resolution>
                  <min_angle>-0.14</min_angle>  <!-- 角度范围最小值 -7.5度 -->
                  <max_angle>0.14</max_angle>   <!-- 角度范围最大值 7.5度 -->
                </horizontal>
                <vertical>    <!-- 垂直方向-->
                  <samples>10</samples>
                  <resolution>1</resolution>
                  <min_angle>-0.14</min_angle>
                  <max_angle>0.14</max_angle>
                </vertical>
              </scan>
              <range>
                <min>0.05</min>   <!-- 最小测距范围-->
                <max>3</max>   <!-- 最大测距范围  超声波雷达最大测距4.5m-->
                <resolution>0.1</resolution>
              </range>
            </ray>
            <plugin name="ultrasonic_sensor_controller" filename="libgazebo_ros_range.so">
              <gaussianNoise>0.01</gaussianNoise>
              <alwaysOn>true</alwaysOn>
              <updateRate>10</updateRate>
              <topicName>ultrasonic_sensor</topicName>   <!-- 发布数据的 Topic 名称 -->
              <frameName>ultrasonic_sensor</frameName>   <!-- 对应的frame 名字 -->
              <fov>0.1</fov>
              <radiation>ultrasound</radiation> 
              <min_angle>-0.1308</min_angle> 
              <max_angle>0.1308</max_angle> 
            </plugin>
          </sensor>
        </gazebo>
</robot>

这里需要创建三个超声波雷达仿真文件（其中两边的雷达需要调角度）

然后将他们集成进launch文件

<launch>
<!--  服务器中载入urdf文件-->
   <param name="robot_description" command="$(find xacro)/xacro $(find jqr_ultrasonic)/urdf/car.urdf.xacro" />
<!--  启动gazebo的仿真环境 -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch" >
       <arg name="world_name" value="$(find jqr_ultrasonic)/worlds/box_house.world" />
   </include>
<!--  在gazebo中添加机器人模型-->
   <node pkg="gazebo_ros" type="spawn_model" name="spawn_car" args="-urdf -model mycar -param robot_description"/>
</launch>

然后再创建一个功能包：ultrasonic_obstacle_avoidance  添加依赖：

geometry_msgs

roscpp

sensor_msgs

修改package.xaml文件

<?xml version="1.0"?>
<package format="2">
  <name>ultrasonic_obstacle_avoidance</name>
  <version>0.0.0</version>
  <description>The ultrasonic_obstacle_avoidance package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="rosnoetic@todo.todo">rosnoetic</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/ultrasonic_obstacle_avoidance</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

然后再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<<y)
#define  clrbit(x, y)  x&=~(1<<y)
 
//低三位作为声呐警告装置
//定义运动状态
#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
 
//全局变量
geometry_msgs::Twist twist_cmd;
ros::Publisher twist_pub;
 
const double warn_range = 0.5;  //检测距离警告
 
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]; //使用3个声呐值
 
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 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<geometry_msgs::Twist>("/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