[Bug]: Path planning hangs when given a navigation goal

ben-greenberg commented 8 months ago

What happened?

When given a /goal_pose the robot is motionless, we expected it to move to the specified position.

After some digging around, it seems that the path planning process is hanging without return a path to be executed. We enabled debug messages and see that the planner_server node seems to be stuck.

Full log attached, the goal is sent around line 13951 LOG.txt

Questions:

Has anyone run into this issue and figured out a solution?
Are we giving the robot the motion goal in an acceptable way?
Has anyone successfully achieved this type of basic motion on a system running Humble?

Robot Model

locobot-wx250s

Operating System

Ubuntu 22.04

ROS Distro

ROS 2 Humble

Steps To Reproduce

Following the instructions here we are able to successfully build a map by running slam with the following command and moving the create3 base around with using teleop_twist_keyboard

ros2 launch interbotix_xslocobot_nav xslocobot_rtabmap.launch.py robot_model:=locobot_wx250s use_lidar:=true slam_mode:=mapping rtabmap_args:=-d

However, when we run the localization command:

ros2 launch interbotix_xslocobot_nav xslocobot_rtabmap.launch.py robot_model:=locobot_wx250s use_lidar:=true slam_mode:=localization

the robot does not move to the specified goal when a /goal_pose message is sent using the 2D Goal Pose interface in rviz (or published from the command line).

Relevant log output

[bt_navigator-16] [INFO] [1698083864.871219288] [bt_navigator]: Begin navigating from current location (-0.33, -0.26) to (-3.54, -0.37)
[planner_server-14] [DEBUG] [1698083864.871547510] [rcl]: Service server taking service request
[planner_server-14] [DEBUG] [1698083864.871631263] [rcl]: Service take request succeeded: true
[planner_server-14] [DEBUG] [1698083864.871827103] [planner_server]: [compute_path_to_pose] [ActionServer] Received request for goal acceptance
[planner_server-14] [DEBUG] [1698083864.871925654] [rcl]: Sending service response
[planner_server-14] [DEBUG] [1698083864.872017245] [planner_server.rclcpp_action]: Accepted goal 89adc37a5e5246fa8feb74804a4849
[planner_server-14] [DEBUG] [1698083864.872118818] [planner_server]: [compute_path_to_pose] [ActionServer] Receiving a new goal
[planner_server-14] [DEBUG] [1698083864.872138820] [planner_server]: [compute_path_to_pose] [ActionServer] Executing goal asynchronously.
[planner_server-14] [DEBUG] [1698083864.872938011] [rcl]: Service server taking service request
[planner_server-14] [DEBUG] [1698083864.872981284] [rcl]: Service take request succeeded: true
[planner_server-14] [DEBUG] [1698083864.873955160] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.874002725] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.887123275] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.887176669] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.887199615] [rcl]: Timer canceled
[planner_server-14] [DEBUG] [1698083864.887207049] [rcl]: Timer canceled
[planner_server-14] [DEBUG] [1698083864.887227457] [rcl]: Timer canceled
[planner_server-14] [DEBUG] [1698083864.887232087] [rcl]: Timer canceled
[planner_server-14] [DEBUG] [1698083864.887248172] [tf2_ros_message_filter]: MessageFilter [target=map ]: Message ready in frame locobot/laser_frame_link at time 1698083864.796, count now 0
[planner_server-14] [DEBUG] [1698083864.887436897] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.887506085] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.892559938] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.892608609] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.893214523] [planner_server]: [compute_path_to_pose] [ActionServer] Executing the goal...
[planner_server-14] [DEBUG] [1698083864.906477636] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.906524864] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.926260085] [rcl]: Calling timer
[planner_server-14] [DEBUG] [1698083864.926414375] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.926433309] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.929795203] [rcl]: Subscription taking message
[planner_server-14] [DEBUG] [1698083864.929835328] [rcl]: Subscription take succeeded: true
[planner_server-14] [DEBUG] [1698083864.941326471] [rcl]: Subscription taking message

Additional Info

Our create3 base is still on galactic. I don't think this is the issue, since we've been able to send and receive messages from it, but figure it's worth mentioning

ben-greenberg commented 8 months ago

I see that #35 mentions that it is now "Able to produce global plan given a goal pose"

Is this expected to solve my issue?

I don't see what changes would have this impact, but I'm not very familiar with the nav2 stack and BT interface

ben-greenberg commented 8 months ago

I can confirm that building the code in that #35 did solve this problem.

Interbotix / interbotix_ros_rovers