search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.6k stars 1.3k forks source link

NavigateThroughPoses not working when robot frame != 'base_link' #2666

Closed charlielito closed 3 years ago

charlielito commented 3 years ago

Bug report

When using another name for the robot base frame than base_link, the action NavigateThroughPoses seems to be broken.

Required Info:

Steps to reproduce issue

Replace in nav2_bringup/bringup/urdf/turtlebot3_waffle.urdf every base_link to let's say chassis. Also replace in nav2_bringup/bringup/params/nav2_params.yml every base_link to chassis. Then run the tb3 demo

ros2 launch nav2_bringup tb3_simulation_launch.py

Use Rviz to use the Nav Through Poses Mode and set a couple of poses to follow. Activate nav2 stack with navigate through poses action.

Expected behavior

The robot should follow the poses.

Actual behavior

BT never starts and display the following logs

...
[bt_navigator-11] Warning: Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist
[bt_navigator-11]          at line 156 in /tmp/binarydeb/ros-galactic-tf2-0.17.2/src/buffer_core.cpp
[bt_navigator-11] Warning: Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist
[bt_navigator-11]          at line 156 in /tmp/binarydeb/ros-galactic-tf2-0.17.2/src/buffer_core.cpp
[bt_navigator-11] Warning: Invalid frame ID "base_link" passed to canTransform argument source_frame - frame does not exist
[bt_navigator-11]          at line 156 in /tmp/binarydeb/ros-galactic-tf2-0.17.2/src/buffer_core.cpp
...

Note that NavigateToPose and WaypointFollowing work well. I tried to look into the code to see if somewhere was base_link hardcoded but couldn't find anything. Any clue where might be the problem?

My mentioned and modified files

turtlebot3_waffle.urdf ``` ```
nav2_params.yaml ``` amcl: ros__parameters: use_sim_time: True alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 do_beamskip: false global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 2000 min_particles: 500 odom_frame_id: "odom" pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 robot_model_type: "differential" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 1.0 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 scan_topic: scan amcl_map_client: ros__parameters: use_sim_time: True amcl_rclcpp_node: ros__parameters: use_sim_time: True bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: chassis odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_compute_path_through_poses_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 - nav2_single_trigger_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node - nav2_remove_passed_goals_action_bt_node - nav2_planner_selector_bt_node - nav2_controller_selector_bt_node - nav2_goal_checker_selector_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 failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_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 #precise_goal_checker: # plugin: "nav2_controller::SimpleGoalChecker" # xy_goal_tolerance: 0.25 # yaw_goal_tolerance: 0.25 # stateful: True general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 max_speed_xy: 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: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: True stateful: True critics: ["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: chassis use_sim_time: True rolling_window: true width: 3 height: 3 resolution: 0.05 robot_radius: 0.22 plugins: ["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" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 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: chassis use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true plugins: ["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" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 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 yaml_filename: "turtlebot3_world.yaml" map_saver: ros__parameters: use_sim_time: True save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True 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 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", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" global_frame: odom robot_base_frame: chassis 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 waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 ```
SteveMacenski commented 3 years ago

I almost guarantee its because you didn't change the base link to your actual base link in all of the places necessary. Also review the Nav2 documentation https://navigation.ros.org/configuration/index.html to see all the places where you may need to change this name.

In general though, you should use base_link to properly implement REP 105, but you should definitely be able to use another frame instead if you choose.

That warning just indicates to me there are more places you didn't make the change. Though if you actually found somewhere that we hardcoded it without a parameter override, let me know! That should be fixed!

charlielito commented 3 years ago

I almost guarantee its because you didn't change the base link to your actual base link in all of the places necessary. Also review the Nav2 documentation https://navigation.ros.org/configuration/index.html to see all the places where you may need to change this name.

In general though, you should use base_link to properly implement REP 105, but you should definitely be able to use another frame instead if you choose.

That warning just indicates to me there are more places you didn't make the change. Though if you actually found somewhere that we hardcoded it without a parameter override, let me know! That should be fixed!

If that were the case NavigateToPose shouldn't work at all, but it is working. I shared also the files I used, and there is not base_link anywhere.

SteveMacenski commented 3 years ago

You need to look at the configuration guide. Just because you don't specify base_link doesn't mean there isn't a parameter in a server or BT plugin node you're using that has it as a parameterization that defaults to base_link.

I won't go through all of them to enumerate all the places you may have missed, but at least one potential omission is https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp#L36

charlielito commented 3 years ago

Yes, you are right, I missed totally that, I was thinking that the robot_base_frame when defined in the bt_navigator part of the .yaml config file file was going to propagate that to all nodes. After adding robot_base_frame to https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml#L13 it worked as expected.

Just one final question, is there another way of defining the robot_base_frame for the RemovePassedGoals directly in the .yaml config file?

SteveMacenski commented 3 years ago

Nope, since those are independent plugins, they all will have their own configuration setting and ports. This is so they’re independent bc user defined plugins will not have access to those same resources.

Happy to have helped.