ROS 之利用超声波障碍物层紧急避障

朴灿烈づ我的快乐病毒、 2022-12-05 05:21 339阅读 0赞

#### 0 安装编译 ####

git clone https://github.com/DLu/navigation_layers.git   // 在src目录下
    cd ..
    catkin_make 
    rospack plugins --attrib=plugin costmap_2d				 // 如果能检测到range_sensor_layer则说明安装成功

#### 1 配置costmap参数文件 ####

加入sonar层， 根据具体情况在costmap\_common\_params.yaml和global\_costmap\_params.yaml中添加

sonar_layer:
      topics : (Array of strings, default : ['/sonar'] )  // 订阅超声波数据的发布的topic
      no_readings_timeout : ( double , default : 0.0 ) 如果是0，该参数不起作用，否则如果层在该参数指定时间内没有收到传感器任何数据，层会给出warning并被标记为没有数据流
      clear_threshold : ( double, default : .2 ) 概率比clear_threshold低的cell在master costmap中被标记为free空间
      mark_threshold : ( double , default : .8 ) 概率比mark_threshold高的cell在master costmap中被标记为lethal obstacles
      clear_on_max_reading : ( bool , default : false ) 是否将超出sonar最大距离清除
    
    plugins: 
       - { name: sonar_layer,   type: "range_sensor_layer::RangeSensorLayer"}

#### 2 发布超声波数据 ####

#include "ros/ros.h"
    #include "sensor_msgs/Range.h"
    #include "std_msgs/Header.h"
    #include <time.h>
    #include <sstream>
    #include <tf/transform_broadcaster.h>
    
    int main(int argc, char **argv)
    { 
      ros::init(argc, argv, "talkerUltraSound");
      ros::NodeHandle n;
      ros::Publisher ultrasound_pub = n.advertise<sensor_msgs::Range>("UltraSoundPublisher", 10);
      ros::Rate loop_rate(10);
      int count = 0;
      while (ros::ok())
      { 
    
        sensor_msgs::Range msg;
        std_msgs::Header header;
        header.stamp = ros::Time::now();
        header.frame_id = "/ultrasound";
        msg.header = header;
        msg.field_of_view = 1;
        msg.min_range = 0;
        msg.max_range = 5;
        msg.range = rand()%3;//rand()%3;
    
        tf::TransformBroadcaster broadcaster;
          broadcaster.sendTransform(
          tf::StampedTransform(
            tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
            ros::Time::now(),"base_link", "ultrasound"));
    
    
        ultrasound_pub.publish(msg);
    
        loop_rate.sleep();
        ++count;
      }
      return 0;
    }

#### 实际效果 ####

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MDYzOTA5NQ_size_16_color_FFFFFF_t_70_pic_center]  
参考：  
https://blog.csdn.net/xiekaikaibing/article/details/80096033  
https://blog.csdn.net/qq\_25241325/article/details/83824504

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl80MDYzOTA5NQ_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/72422bad34de4f7686eb391f8d9977aa.png