Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added Social Navigation Layers Support #59

Open
wants to merge 20 commits into
base: hydro-devel
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 8 additions & 5 deletions launch/move_base.launch
Original file line number Diff line number Diff line change
@@ -1,29 +1,32 @@
<launch>
<arg name="machine" default="localhost" />
<arg name="user" default="" />
<arg name="with_social_layer" default="false" />
<arg name="with_mux" default="false" />

<machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)" default="true" />


<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true" unless="$(arg with_mux)">
<node pkg="move_base" type="move_base" respawn="false" name="move_base" args="with_social_layer" output="screen" clear_params="true" unless="$(arg with_mux)">
<!-- default:20.0. with this value dwa planner fails to find a valid plan a lot more -->
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/move_base_params.yaml" command="load"/>
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/global_costmap_params_social.yaml" if="$(arg with_social_layer)" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/global_costmap_params.yaml" unless="$(arg with_social_layer)" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/local_costmap_params.yaml" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/global_costmap_params.yaml" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/dwa_planner_ros.yaml" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/nav_fn_ros.yaml" command="load" />
</node>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true" if="$(arg with_mux)">
<node pkg="move_base" type="move_base" respawn="flase" name="move_base" args="with_social_layer" output="screen" clear_params="true" if="$(arg with_mux)">
<!-- default:20.0. with this value dwa planner fails to find a valid plan a lot more -->
<remap from="/cmd_vel" to="/cmd_vel_mux/input/navigation"/>
<remap from="/cmd_vel" to="/cmd_vel_mux/input/navigation"/>
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/move_base_params.yaml" command="load"/>
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/global_costmap_params_social.yaml" if="$(arg with_social_layer)" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/global_costmap_params.yaml" unless="$(arg with_social_layer)" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/local_costmap_params.yaml" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/global_costmap_params.yaml" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/dwa_planner_ros.yaml" command="load" />
<rosparam file="$(find scitos_2d_navigation)/scitos_move_base_params/nav_fn_ros.yaml" command="load" />
</node>
Expand Down
4 changes: 3 additions & 1 deletion launch/scitos_2d_nav.launch
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@
<arg name="user" default="" />
<arg name="map"/>
<arg name="with_no_go_map" default="false"/>
<arg name="with_mux" default="false" />
<arg name="with_mux" default="false" />
<arg name="with_social_layer" default="false" />
<arg name="no_go_map"/>

<machine name="$(arg machine)" address="$(arg machine)" env-loader="$(optenv ROS_ENV_LOADER )" user="$(arg user)" default="true"/>
Expand All @@ -30,6 +31,7 @@
<arg name="machine" value="$(arg machine)"/>
<arg name="user" value="$(arg user)"/>
<arg name="with_mux" value="$(arg with_mux)"/>
<arg name="with_social_layer" value="$(arg with_social_layer)"/>
</include>

</launch>
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
<build_depend>move_base</build_depend>
<run_depend>move_base</run_depend>
<run_depend>amcl</run_depend>
<run_depend>social_navigation_layers</run_depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
Expand Down
1 change: 0 additions & 1 deletion scitos_move_base_params/costmap_common_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@ no_go_layer:
inflation_layer:
inflation_radius: 0.8
cost_scaling_factor: 5.0


obstacle_layer:
max_obstacle_height: 2.0
Expand Down
2 changes: 2 additions & 0 deletions scitos_move_base_params/dwa_planner_ros.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -42,3 +42,5 @@ DWAPlannerROS:

holonomic_robot: false



1 change: 0 additions & 1 deletion scitos_move_base_params/global_costmap_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,3 @@ global_costmap:
- {name: no_go_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

18 changes: 18 additions & 0 deletions scitos_move_base_params/global_costmap_params_social.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
global_costmap:
update_frequency: 5.0
publish_frequency: 5.0
static_map: true
rolling_window: false
plugins:
- {name: map_layer, type: "costmap_2d::StaticLayer"}
- {name: no_go_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
- {name: proxemic_layer, type: "social_navigation_layers::ProxemicLayer"}
- {name: passing_layer, type: "social_navigation_layers::PassingLayer"}
proxemic_layer:
amplitude: 150.0
covariance: 0.3
factor: 7.0
passing_layer:
enabled: false
1 change: 0 additions & 1 deletion scitos_move_base_params/local_costmap_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,3 @@ local_costmap:
resolution: 0.05
plugins:
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
#- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
1 change: 0 additions & 1 deletion scitos_move_base_params/move_base_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -14,4 +14,3 @@ conservative_reset:
aggressive_reset:
layer_names: ["obstacle_layer"]
reset_distance: 0.0