Why is the navigation stack not planning a path?
I'm running ROS Kinetic on a Nvidia Jetson TX1. AMCL works fine. However, when I send a navigation goal through rviz, it is published on /move_base_simple/goal
, but move_base
does not publish anything on /move_base/current_goal
or plan a path. There are no errors or warnings. I am using a 2D 270° Hokuyo laser scanner for observation. The navigation stack creates both local and global costmaps successfully, but no path is planned no matter where I place a goal. I don't think this matters, but I'm using tf2
instead of tf
. Has anyone had a similar issue and/or know why the navigation stack is not planning a path? My parameter files are below:
baselocalplanner_params.yaml
TrajectoryPlannerROS:
max_vel_x: 0.3
min_vel_x: 0.1
max_vel_theta: 0.4
min_in_place_vel_theta: 0.2
acc_lim_theta: 3.2
acc_lim_x: 2.5
acc_lim_y: 2.5
meter_scoring: true
holonomic_robot: false
costmapcommonparams.yaml
obstacle_range: 10.0
raytrace_range: 12.0
footprint: [[-0.22, 0.26], [0.22, 0.26], [0.22, -0.26], [-0.22, -0.26]]
inflation_radius: 0.5
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
globalcostmapparams.yaml
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 2.0
static_map: true
localcostmapparams.yaml
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 2.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.05
controller_frequency: 5.0
move_base.launch
<launch>
<!-- map server -->
<arg name="map" default="$(find solace)/maps/hallway2_cleared.yaml" />
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map)" />
<!-- amcl -->
<arg name="scan_topic" default="scan" />
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<param name="odom_model_type" value="diff" />
<param name="odom_alpha5" value="0.1" />
<param name="gui_publish_rate" value="4.0" />
<param name="laser_max_beams" value="60" />
<param name="laser_max_range" value="12.0" />
<param name="min_particles" value="500" />
<param name="max_particles" value="2000" />
<param name="kld_err" value="0.01" />
<param name="kld_z" value="0.99" />
<param name="odom_alpha1" value="0.3" />
<param name="odom_alpha2" value="0.3" />
<param name="odom_alpha3" value="0.3" />
<param name="odom_alpha4" value="0.3" />
<param name="laser_z_hit" value="0.5" />
<param name="laser_z_short" value="0.05" />
<param name="laser_z_max" value="0.05" />
<param name="laser_z_rand" value="0.5" />
<param name="laser_sigma_hit" value="0.2" />
<param name="laser_lambda_short" value="0.1" />
<param name="laser_model_type" value="likelihood_field" />
<param name="laser_likelihood_max_dist" value="2.0" />
<param name="update_min_d" value="0.15" />
<param name="update_min_a" value="0.2" />
<param name="odom_frame_id" value="gmapping_map" />
<param name="base_frame_id" value="base_link" />
<param name="resample_interval" value="1" />
<param name="transform_tolerance" value="0.5" />
<param name="recovery_alpha_slow" value="0.001" />
<param name="recovery_alpha_fast" value="0.1" />
<remap from="scan" to="$(arg scan_topic)" />
<remap from="map" to="navigation_map" />
</node>
<!-- navigation stack -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find solace)/config/racecar-v2/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find solace)/config/racecar-v2/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find solace)/config/racecar-v2/local_costmap_params.yaml" command="load" />
<rosparam file="$(find solace)/config/racecar-v2/global_costmap_params.yaml" command="load" />
<rosparam file="$(find solace)/config/racecar-v2/base_local_planner_params.yaml" command="load" />
</node>
<!-- twist-to-vesc node -->
<node name="navigation_twist_to_vesc" type="navigation_twist_to_vesc.py" pkg="solace" output="screen" />
</launch>
Output of move_base
[ INFO] [1496327808.360792278]: Requesting the map...
[ INFO] [1496327808.380738390]: Received a 1152 X 672 map @ 0.050 m/pix
[ INFO] [1496327808.452784809]: Initializing likelihood field model; this can take some time on large maps...
Laser Pose= 0.14 -2.9617e-13 -5.71959e-15
[ INFO] [1496327808.752546595]: Done initializing likelihood field model.
[INFO] [1496327808.757862]: Got a gmapping map, but I'm not lost.
[INFO] [1496327809.069413]: number of particles: 2000
[ INFO] [1496327809.270089807]: Loading from pre-hydro parameter style
[ INFO] [1496327809.327750307]: Using plugin "static_layer"
[ INFO] [1496327809.427021918]: Requesting the map...
[ INFO] [1496327809.643918458]: Resizing costmap to 1152 X 672 at 0.050000 m/pix
Laser Pose= 0.14 -5.57412e-14 5.35209e-14
[ INFO] [1496327809.742963978]: Received a 1152 X 672 map at 0.050000 m/pix
[ INFO] [1496327809.759764035]: Using plugin "obstacle_layer"
[ INFO] [1496327809.770782808]: Subscribed to Topics:
[ INFO] [1496327809.810277277]: Using plugin "inflation_layer"
[ INFO] [1496327810.143690585]: Loading from pre-hydro parameter style
[ INFO] [1496327810.190312937]: Using plugin "obstacle_layer"
[ INFO] [1496327810.271559975]: Subscribed to Topics:
[ INFO] [1496327810.315618296]: Using plugin "inflation_layer"
Registering Scans:Done
[ INFO] [1496327810.593326758]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1496327810.627301659]: Sim period is set to 0.20
Laser Pose= 0.14 1.08121e-13 1.61824e-13
Registering Scans:Done
[ INFO] [1496327811.569355261]: Recovery behavior will clear layer obstacles
[ INFO] [1496327811.669390196]: Recovery behavior will clear layer obstacles
Laser Pose= 0.14 -1.50839e-13 1.54216e-13
[ INFO] [1496327811.827558439]: odom received!
Registering Scans:Done
Laser Pose= 0.14 -4.72935e-14 2.38952e-13
Registering Scans:Done
Asked by czhao on 2017-05-31 20:14:52 UTC
Comments
what is the output of move_base? what is the status of your goal:
rostopic echo /move_base/status
?Asked by Procópio on 2017-06-01 01:55:48 UTC
sorry, I made an error in my question, which I've fixed. When I send a navigation goal through rviz, it is published on
/move_base_simple/goal
, butmove_base
does not publish anything on/move_base/current_goal
or plan a path.Asked by czhao on 2017-06-01 09:41:38 UTC
the status of the goal has an empty
frame_id
andstatus_list
is empty.Asked by czhao on 2017-06-01 09:51:00 UTC
Did you forget loading the param file
base_global_planner_params.yaml
which specifies which global planner you would like to use?Asked by DavidN on 2017-06-01 11:56:29 UTC
Hm, the navigation stack robot setup tutorial doesn't say anything about a
base_global_planner_params.yaml
file. Is the file necessary? If so, what should I put in it?Asked by czhao on 2017-06-01 19:36:14 UTC
See section "2.4 Base Local Planner Configuration" of the tutorial and 2.5. for the launch file.
Asked by Humpelstilzchen on 2017-06-02 00:56:01 UTC
Adding a
base_global_planner_params.yaml
file did not fix it.Asked by czhao on 2017-06-02 13:46:14 UTC
You did add a rosparam load tag for global planner params file, right? Can you post the output of move_base launch after adding the tag for loading global planner?
Asked by DavidN on 2017-06-02 21:03:09 UTC
I did add a
rosparam
load tag. The output is the same after adding the tag. My param files are here, and the launch file is here.Asked by czhao on 2017-06-05 12:51:44 UTC
were you using rviz to do this? if so yiu might need to add a remap for the /goal topic. Did you get the global planner to work on the Jetson? I am having similar issus where the costmap in the global planner is not loading any of the layers(static, inflation, nor obstacles)
Asked by Troski on 2018-04-19 19:55:47 UTC