测试social_navigation_layers

目标：测试social_navigation_layers

方法：

使用move_base接口启动costmap_2d

这样就能直接用configure方法来进行测试不用自己写代码

一、启动move_base

　　1、launch file

<launch>
  <!-- Launch move_base and load all navigation parameters -->
  <include file="$(find costmap_test)/config/move_base_config/move_base.xml"/>
  
  <!-- Run the map server with a blank map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find pioneer_xtion)/stage_config/maps/blank_map.yaml" />

  <!--costmap 必须的tf变换 看http://wiki.ros.org/costmap_2d#tf Required tf Transforms-->
  <!--global costmap -->
  <node name="static_tf0" pkg="tf" type="static_transform_publisher" args="2 0 0 0 0 0 /map /base_link 100"/>
  <!--local costmap -->
  <node name="static_tf1" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /odom /map 100"/>
  <!--发布people msgs-->
  <node name = "people" pkg="people_velocity_tracker" type="static.py" args="2 2 1 1"/>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find costmap_test)/rviz/single_robot.rviz" />
</launch>

　　

　　2、movebase config,common costmap config,local costmap config,global costmap config

<launch>
<!--
  Example move_base configuration. Descriptions of parameters, as well as a full list of all amcl parameters, can be found at http://www.ros.org/wiki/move_base.
-->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
    <param name="footprint_padding" value="0.01" />
    <param name="controller_frequency" value="10.0" />
    <param name="controller_patience" value="3.0" />

    <param name="oscillation_timeout" value="30.0" />
    <param name="oscillation_distance" value="0.5" />

    
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
   
　　<!-- ns = global costmap 加载参数到 global costmap这个node里-->
    <rosparam file="$(find costmap_test)/config/move_base_config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find costmap_test)/config/move_base_config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find costmap_test)/config/move_base_config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find costmap_test)/config/move_base_config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find costmap_test)/config/move_base_config/base_local_planner_params.yaml" command="load" />
    
    <rosparam file="$(find costmap_test)/config/move_base_config/dwa_local_planner_params.yaml" command="load" />
    
  </node>
</launch>

common costmap config

#This file contains common configuration options for the two costmaps used in the navigation stack for more details on the parameters in this file, and a full list of the parameters used by the costmaps, please see http://www.ros.org/wiki/costmap_2d

#For this example we'll configure the costmap in voxel-grid mode
map_type: voxel

#Voxel grid specific parameters
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
unknown_threshold: 9
mark_threshold: 0

#Set the tolerance we're willing to have for tf transforms
transform_tolerance: 0.5

#Obstacle marking parameters
obstacle_range: 2.5
max_obstacle_height: 2.0
raytrace_range: 3.0

#The footprint of the robot and associated padding
#footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
robot_radius: 0.2
footprint_padding: 0.01

#Cost function parameters
inflation_radius: 0.5
cost_scaling_factor: 0.1

#The cost at which a cell is considered an obstacle when a map is read from the map_server
lethal_cost_threshold: 100

#Configuration for the sensors that the costmap will use to update a map

#configuration for plugins
plugins:
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: obstacles,        type: "costmap_2d::VoxelLayer"}
    - {name: proxemicLayer,       type: "social_navigation_layers::ProxemicLayer"}
　　

　global costmap config

#Independent settings for the global planner's costmap. Detailed descriptions of these parameters can be found at http://www.ros.org/wiki/costmap_2d
<!--global costmap config 如果global_costmap改成其他名字，则param名字也会改-->
global_costmap:
  #Set the global and robot frames for the costmap
  global_frame: map
  robot_base_frame: base_link

  #Set the update and publish frequency of the costmap
  update_frequency: 2.0
  publish_frequency: 0.0

  #We'll use a map served by the map_server to initialize this costmap
  static_map: true
  rolling_window: false

  footprint_padding: 0.02

local costmap config

#Independent settings for the local planner's costmap. Detailed descriptions of these parameters can be found at http://www.ros.org/wiki/costmap_2d

local_costmap:
  #We'll publish the voxel grid used by this costmap
  publish_voxel_map: true

  #Set the global and robot frames for the costmap
  global_frame: odom
  robot_base_frame: base_link

  #Set the update and publish frequency of the costmap
  update_frequency: 2.0
  publish_frequency: 2.0

  #We'll configure this costmap to be a rolling window... meaning it is always
  #centered at the robot
  static_map: false
  rolling_window: true
   6.0
  height: 6.0
  resolution: 0.01
  origin_x: 0.0
  origin_y: 0.0

　　

　　

　

二、发布people_msgs

使用

http://wiki.ros.org/people_velocity_tracker?distro=kinetic

people_velocity_tracker 

static.py

效果图

往速度方向增长代价