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

edit

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 ...