在ROS下,创建range_layer,加入超声波传感器。

 

首先，先发布 超声波话题。 

以大家常用的python为例，

 

rospy.loginfo("r0: " + str(r0)+"r1: " + str(r1)+"r2: " + str(r2)+"r3: " + str(r3)+"r4: " + str(r4))
                    #rospy.loginfo("r0: " + str(r0) + "r3: " + str(r3))
                    sonar0_range=Range()
                    sonar0_range.header.stamp=now
                    sonar0_range.header.frame_id="/sonar0"
                    sonar0_range.radiation_type=Range.ULTRASOUND
                    sonar0_range.min_range=0.05
                    sonar0_range.max_range=4.0
                    sonar0_range.range=r0/100.0
                    self.sonar0_pub.publish(sonar0_range)

 

 

 

    

发布sonar0 超声波话题，并设置 radiation_type参数。 

然后，创建range_layer ,想了解具体过程 可以参考 官网的链接，

http://wiki.ros.org/range_sensor_layer  以及博客 http://blog.csdn.net/x_r_su/article/details/53418044

 http://blog.csdn.net/heyijia0327/article/details/42241831都是很不错的学习资料。  

下载附件代码， https://pan.baidu.com/s/1i53UCj7 链接: 密码: bqmk 放到ros工作区间下，进行编译 catkin_make .  

在终端输入

rospack plugins --attrib=plugin costmap_2d  

         如果产生一个 range_layer 说明 该插件 生成成功。

  最后， 在导航包的配置文件中，添加配置参数。

１）nav/config/odom/costmap_common_params.yaml
增加
sonar_layer:
  enabled:            true
  clear_threshold:    0.46
  mark_threshold:     0.98
  topics: ["/sonar0", "/sonar1", "/sonar2", "/sonar3", "/sonar4"]   ////订阅超神波主题，添加到sonar_layer到 costmap中
  clear_on_max_reading: true

２）nav/config/odom/local_costmap_params.yaml
在 plugins:
    - {name: static_layer,        type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
下面增加
    - {name: sonar_layer,         type: "range_sensor_layer::RangeSensorLayer"}   //加入到局部导航的配置文件

 

最终效果图如上，紫色的为添加上的超生波模型。