nav2 Controller server not receiving poses sent through `FollowPath` action
I am trying to send paths, i.e. nav_msgs::msg::Path, to a nav2 controller server through the FollowPath plugin.
As a stepping stone, I've made a "demo" node which should just send a Path consisting of a single pose. I've based the node on the ros2 action client tutorial, so I figure this should be fine.
When I run the client
$ ros2 run tractor_navigation send_goal_demo
[INFO] [1638967653.880364807] [send_goal_demo]: Sending goal, compose of 1 poses
[INFO] [1638967653.880845873] [send_goal_demo]: Goal was accepted by server, waiting for result.
[ERROR] [1638967653.881508408] [send_goal_demo]: Goal was aborted
The navigation server responds with
[lifecycle_manager-8] [INFO] [1638967358.922339620] [lifecycle_manager_navigation]: Managed nodes are active
[DEBUG] [launch]: processing event: '<launch.events.process.process_stderr.ProcessStderr object at 0x7fa5364a57c0>'
[DEBUG] [launch]: processing event: '<launch.events.process.process_stderr.ProcessStderr object at 0x7fa5364a57c0>' ✓ '<launch.event_handlers.on_process_io.OnProcessIO object at 0x7fa536429c40>'
[waypoint_follower-7] [INFO] [1638967358.921651396] [waypoint_follower]: Activating
[DEBUG] [launch.launch_context]: emitting event synchronously: 'launch.events.process.ProcessStderr'
[DEBUG] [launch]: processing event: '<launch.events.process.process_stderr.ProcessStderr object at 0x7fa53650c2e0>'
[DEBUG] [launch]: processing event: '<launch.events.process.process_stderr.ProcessStderr object at 0x7fa53650c2e0>' ✓ '<launch.event_handlers.on_process_io.OnProcessIO object at 0x7fa53643c520>'
[controller_server-3] [INFO] [1638967653.880905178] [controller_server]: Received a goal, begin computing control effort.
[DEBUG] [launch.launch_context]: emitting event synchronously: 'launch.events.process.ProcessStderr'
[DEBUG] [launch]: processing event: '<launch.events.process.process_stderr.ProcessStderr object at 0x7fa53794e220>'
[DEBUG] [launch]: processing event: '<launch.events.process.process_stderr.ProcessStderr object at 0x7fa53794e220>' ✓ '<launch.event_handlers.on_process_io.OnProcessIO object at 0x7fa53643c520>'
[controller_server-3] [ERROR] [1638967653.881288925] [controller_server]: Resulting plan has 0 poses in it.
[controller_server-3] [WARN] [1638967653.881346700] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
The controller server says it receives a path with 0 poses, [controller_server-3] [ERROR] [1638967653.881288925] [controller_server]: Resulting plan has 0 poses in it., despite the client saying it send one. I'm not quite sure how this disjointedness comes to happen. For reference I've attached the entire package, including my startup of the navigation function in addition to the cpp node.
Here is my nav2 config file
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.5 # 0.25
yaw_goal_tolerance: 1.0 # 0.25
stateful: True
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 1.0 # 0.26
max_vel_y: 0.0
max_vel_theta: 5.0
min_speed_xy: 0.0
max_speed_xy: 5.0 # 0.26
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 1.0 # 0.25
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] # ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
plugins: ["inflation_layer"] # ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
# voxel_layer:
# plugin: "nav2_costmap_2d::VoxelLayer"
# enabled: True
# publish_voxel_map: True
# origin_z: 0.0
# z_resolution: 0.05
# z_voxels: 16
# max_obstacle_height: 2.0
# mark_threshold: 0
# observation_sources: scan
# scan:
# topic: /scan
# max_obstacle_height: 2.0
# clearing: True
# marking: True
# data_type: "LaserScan"
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "inflation_layer"] # ["static_layer", "obstacle_layer", "inflation_layer"]
# obstacle_layer:
# plugin: "nav2_costmap_2d::ObstacleLayer"
# enabled: True
# observation_sources: scan
# scan:
# topic: /scan
# max_obstacle_height: 2.0
# clearing: True
# marking: True
# data_type: "LaserScan"
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
map_server:
ros__parameters:
use_sim_time: True
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5000
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: False
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
# plugin: "nav2_navfn_planner/NavfnPlanner"
# tolerance: 0.5
# use_astar: false
# allow_unknown: true
plugin: "smac_planner/SmacPlanner"
tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node
downsample_costmap: false # whether or not to downsample the map
downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
allow_unknown: true # allow traveling in unknown space
max_iterations: -1 # maximum total iterations to search for before failing
max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
max_planning_time_ms: 2000.0 # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
smooth_path: false # Whether to smooth searched path
motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp
angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search)
minimum_turning_radius: 0.20 # For SE2 node & smoother: minimum turning radius in m of path / vehicle
reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0
non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1
cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones
smoother:
smoother:
w_curve: 30.0 # weight to minimize curvature of path
w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight
w_smooth: 30000.0 # weight to maximize smoothness of path
w_cost: 0.025 # weight to steer robot away from collision and cost
cost_scaling_factor: 10.0 # this should match the inflation layer's parameter
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: True
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
back_up:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
Asked by morten on 2021-12-08 08:02:43 UTC
Answers
This is pretty simple I think if looking at the code https://github.com/ros-planning/navigation2/blob/30b405c58e6d53ba8c96381416bc4679d35a1483/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp#L541-L543
My guess is that you've put in some bogus value of the pose of that point, so it gets thrown out from the transformed path because its well outside the actual location of the robot to be included. And/or you don't have TF setup so the transformation is failing and unable to transform it from map to odom frames. But I don't see any errors, so it probably isn't that unless you're seeing other logging.
Asked by stevemacenski on 2021-12-08 15:49:21 UTC
Comments
I've tried setting navigation targets through rviz, and this works fine. The point I'm sending through FollowPath is more or less the same as a target I've given through rviz. The point itself is just (10,0,0) with quaternion transformed from rpy(0,0,0) using tf2::toMsg. So I figure there's nothing so crazy here, note path is given in map frame. I figure this is correct as well.
Asked by morten on 2021-12-09 04:48:10 UTC
Comments