Ask Your Question
1

nav2 Controller server not receiving poses sent through `FollowPath` action

asked 2021-12-08 07:02:43 -0500

morten gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2021-12-08 14:49:21 -0500

This is pretty simple I think if looking at the code https://github.com/ros-planning/navig...

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.

edit flag offensive delete link more

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.

morten gravatar image morten  ( 2021-12-09 03:48:10 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2021-12-08 07:02:43 -0500

Seen: 102 times

Last updated: Dec 08 '21