Integrating Nav2 with Ardupilot

[pedro-fuoco]

Hi! I'm a GSoC student for Ardupilot. My project involves using Cartographer and Nav2 to control a copter in Gazebo Garden. So far the Gazebo and Cartographer bits have been solved and integrated into Ardupilot's native port for ROS 2, AP_DDS, with the help of @Ryanf55.

• Operating System:
  • Ubuntu 22
• ROS2 Version:
  • Humble
• Version or commit hash:
  • ros-humble-navigation2 1.1.9-1jammy.20230814.171753
• DDS implementation:
  • Micro-XRCE-DDS

I've remmaped /cmd_vel to /ap/cmd_vel, which made it possible to control the copter through the /goal_pose topic (which I tested with rviz). But the costmaps are not being published correctly and, because of that, the copter isn't avoiding obstacles, as you can see in the video below:

This was done launching ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true after launching the Copter simulation in Gazebo Garden and Cartographer.

Additional information:

• Nav2 does subscribe to the map topic provided by cartographer

• Here's the tf tree from robot_state_publisher and cartographer. It follows map -> odom -> base_link

• The laser scan is being published at /scan as the default parameters for nav2 suggest

• The log from the nav2 launch gets stuck as follows:

  
  [INFO] [launch]: All log files can be found below /home/fuoco/.ros/log/2023-08-25-01-38-40-740637-fuoco-desk-116858
  [INFO] [launch]: Default logging verbosity is set to INFO
  [INFO] [controller_server-1]: process started with pid [117384]
  [INFO] [smoother_server-2]: process started with pid [117386]
  [INFO] [planner_server-3]: process started with pid [117388]
  [INFO] [behavior_server-4]: process started with pid [117390]
  [INFO] [bt_navigator-5]: process started with pid [117392]
  [INFO] [waypoint_follower-6]: process started with pid [117394]
  [INFO] [velocity_smoother-7]: process started with pid [117396]
  [INFO] [lifecycle_manager-8]: process started with pid [117398]
  [lifecycle_manager-8] [INFO] [1692938321.549701976] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
  [bt_navigator-5] [INFO] [1692938321.549969914] [bt_navigator]: 
  [bt_navigator-5]    bt_navigator lifecycle node launched. 
  [bt_navigator-5]    Waiting on external lifecycle transitions to activate
  [bt_navigator-5]    See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [controller_server-1] [INFO] [1692938321.550973482] [controller_server]: 
  [controller_server-1]   controller_server lifecycle node launched. 
  [controller_server-1]   Waiting on external lifecycle transitions to activate
  [controller_server-1]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [velocity_smoother-7] [INFO] [1692938321.551451257] [velocity_smoother]: 
  [velocity_smoother-7]   velocity_smoother lifecycle node launched. 
  [velocity_smoother-7]   Waiting on external lifecycle transitions to activate
  [velocity_smoother-7]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [planner_server-3] [INFO] [1692938321.558875814] [planner_server]: 
  [planner_server-3]  planner_server lifecycle node launched. 
  [planner_server-3]  Waiting on external lifecycle transitions to activate
  [planner_server-3]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [controller_server-1] [INFO] [1692938321.561262477] [controller_server]: Creating controller server
  [waypoint_follower-6] [INFO] [1692938321.561540913] [waypoint_follower]: 
  [waypoint_follower-6]   waypoint_follower lifecycle node launched. 
  [waypoint_follower-6]   Waiting on external lifecycle transitions to activate
  [waypoint_follower-6]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [smoother_server-2] [INFO] [1692938321.564302573] [smoother_server]: 
  [smoother_server-2]     smoother_server lifecycle node launched. 
  [smoother_server-2]     Waiting on external lifecycle transitions to activate
  [smoother_server-2]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [smoother_server-2] [INFO] [1692938321.567915433] [smoother_server]: Creating smoother server
  [behavior_server-4] [INFO] [1692938321.574240440] [behavior_server]: 
  [behavior_server-4]     behavior_server lifecycle node launched. 
  [behavior_server-4]     Waiting on external lifecycle transitions to activate
  [behavior_server-4]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [controller_server-1] [INFO] [1692938321.576736224] [local_costmap.local_costmap]: 
  [controller_server-1]   local_costmap lifecycle node launched. 
  [controller_server-1]   Waiting on external lifecycle transitions to activate
  [controller_server-1]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [controller_server-1] [INFO] [1692938321.577831531] [local_costmap.local_costmap]: Creating Costmap
  [planner_server-3] [INFO] [1692938321.580596369] [global_costmap.global_costmap]: 
  [planner_server-3]  global_costmap lifecycle node launched. 
  [planner_server-3]  Waiting on external lifecycle transitions to activate
  [planner_server-3]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
  [planner_server-3] [INFO] [1692938321.581182197] [global_costmap.global_costmap]: Creating Costmap
  [lifecycle_manager-8] [INFO] [1692938321.665575615] [lifecycle_manager_navigation]: Starting managed nodes bringup...
  [lifecycle_manager-8] [INFO] [1692938321.665612421] [lifecycle_manager_navigation]: Configuring controller_server
  [controller_server-1] [INFO] [1692938321.665779997] [controller_server]: Configuring controller interface
  [controller_server-1] [INFO] [1692938321.665870367] [controller_server]: getting goal checker plugins..
  [controller_server-1] [INFO] [1692938321.665953972] [controller_server]: Controller frequency set to 20.0000Hz
  [controller_server-1] [INFO] [1692938321.668736086] [local_costmap.local_costmap]: Using plugin "voxel_layer"
  [controller_server-1] [INFO] [1692938321.671010887] [local_costmap.local_costmap]: Subscribed to Topics: scan
  [controller_server-1] [INFO] [1692938321.674850365] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
  [controller_server-1] [INFO] [1692938321.674871807] [local_costmap.local_costmap]: Using plugin "inflation_layer"
  [controller_server-1] [INFO] [1692938321.675257874] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
  [controller_server-1] [INFO] [1692938321.679918937] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
  [controller_server-1] [INFO] [1692938321.680439679] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
  [controller_server-1] [INFO] [1692938321.680635081] [controller_server]: Controller Server has general_goal_checker  goal checkers available.
  [controller_server-1] [INFO] [1692938321.683901112] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
  [controller_server-1] [INFO] [1692938321.684506646] [controller_server]: Setting transform_tolerance to 0.200000
  [controller_server-1] [INFO] [1692938321.689550880] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
  [controller_server-1] [INFO] [1692938321.689901941] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.689987633] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
  [controller_server-1] [INFO] [1692938321.690257450] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.690326981] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
  [controller_server-1] [INFO] [1692938321.690427382] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.690496822] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
  [controller_server-1] [INFO] [1692938321.690682229] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.690774502] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
  [controller_server-1] [INFO] [1692938321.691018457] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.691096847] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
  [controller_server-1] [INFO] [1692938321.691224054] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.691293624] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
  [controller_server-1] [INFO] [1692938321.691414750] [controller_server]: Critic plugin initialized
  [controller_server-1] [INFO] [1692938321.691431540] [controller_server]: Controller Server has FollowPath  controllers available.
  [lifecycle_manager-8] [INFO] [1692938321.695777644] [lifecycle_manager_navigation]: Configuring smoother_server
  [smoother_server-2] [INFO] [1692938321.695962428] [smoother_server]: Configuring smoother server
  [smoother_server-2] [INFO] [1692938321.700391387] [smoother_server]: Created smoother : simple_smoother of type nav2_smoother::SimpleSmoother
  [smoother_server-2] [INFO] [1692938321.700703524] [smoother_server]: Smoother Server has simple_smoother  smoothers available.
  [lifecycle_manager-8] [INFO] [1692938321.703239233] [lifecycle_manager_navigation]: Configuring planner_server
  [planner_server-3] [INFO] [1692938321.706386685] [global_costmap.global_costmap]: Using plugin "static_layer"
  [planner_server-3] [INFO] [1692938321.708652198] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
  [planner_server-3] [INFO] [1692938321.709713196] [global_costmap.global_costmap]: Initialized plugin "static_layer"
  [planner_server-3] [INFO] [1692938321.709737264] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
  [planner_server-3] [INFO] [1692938321.710248331] [global_costmap.global_costmap]: Subscribed to Topics: scan
  [planner_server-3] [INFO] [1692938321.712629924] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
  [planner_server-3] [INFO] [1692938321.712653131] [global_costmap.global_costmap]: Using plugin "inflation_layer"
  [planner_server-3] [INFO] [1692938321.713131002] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
  [planner_server-3] [INFO] [1692938321.717562311] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
  [planner_server-3] [INFO] [1692938321.717588016] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
  [planner_server-3] [INFO] [1692938321.717827105] [planner_server]: Planner Server has GridBased  planners available.
  [planner_server-3] [INFO] [1692938321.719926584] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 222 X 222 at 0.050000 m/pix
  [lifecycle_manager-8] [INFO] [1692938321.722087834] [lifecycle_manager_navigation]: Configuring behavior_server
  [behavior_server-4] [INFO] [1692938321.727313603] [behavior_server]: Creating behavior plugin spin of type nav2_behaviors/Spin
  [behavior_server-4] [INFO] [1692938321.728018784] [behavior_server]: Configuring spin
  [behavior_server-4] [INFO] [1692938321.730981054] [behavior_server]: Creating behavior plugin backup of type nav2_behaviors/BackUp
  [behavior_server-4] [INFO] [1692938321.731626637] [behavior_server]: Configuring backup
  [behavior_server-4] [INFO] [1692938321.733428428] [behavior_server]: Creating behavior plugin drive_on_heading of type nav2_behaviors/DriveOnHeading
  [behavior_server-4] [INFO] [1692938321.734061609] [behavior_server]: Configuring drive_on_heading
  [behavior_server-4] [INFO] [1692938321.736007064] [behavior_server]: Creating behavior plugin assisted_teleop of type nav2_behaviors/AssistedTeleop
  [behavior_server-4] [INFO] [1692938321.737252742] [behavior_server]: Configuring assisted_teleop
  [behavior_server-4] [INFO] [1692938321.740052891] [behavior_server]: Creating behavior plugin wait of type nav2_behaviors/Wait
  [behavior_server-4] [INFO] [1692938321.740673182] [behavior_server]: Configuring wait
  [lifecycle_manager-8] [INFO] [1692938321.743000940] [lifecycle_manager_navigation]: Configuring bt_navigator
  [lifecycle_manager-8] [INFO] [1692938321.780720077] [lifecycle_manager_navigation]: Configuring waypoint_follower
  [waypoint_follower-6] [INFO] [1692938321.786475495] [waypoint_follower]: Created waypoint_task_executor : wait_at_waypoint of type nav2_waypoint_follower::WaitAtWaypoint
  [lifecycle_manager-8] [INFO] [1692938321.786970736] [lifecycle_manager_navigation]: Configuring velocity_smoother
  [velocity_smoother-7] [INFO] [1692938321.787170924] [velocity_smoother]: Configuring velocity smoother
  [lifecycle_manager-8] [INFO] [1692938321.789314237] [lifecycle_manager_navigation]: Activating controller_server
  [controller_server-1] [INFO] [1692938321.789473409] [local_costmap.local_costmap]: Checking transform
  [controller_server-1] [INFO] [1692938321.989984264] [controller_server]: Creating bond (controller_server) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938322.092407429] [lifecycle_manager_navigation]: Server controller_server connected with bond.
  [lifecycle_manager-8] [INFO] [1692938322.092442503] [lifecycle_manager_navigation]: Activating smoother_server
  [smoother_server-2] [INFO] [1692938322.092640806] [smoother_server]: Creating bond (smoother_server) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938322.194933584] [lifecycle_manager_navigation]: Server smoother_server connected with bond.
  [lifecycle_manager-8] [INFO] [1692938322.194963762] [lifecycle_manager_navigation]: Activating planner_server
  [planner_server-3] [INFO] [1692938322.195159925] [global_costmap.global_costmap]: Checking transform
  [planner_server-3] [INFO] [1692938323.195776167] [planner_server]: Activating plugin GridBased of type NavfnPlanner
  [planner_server-3] [INFO] [1692938323.196398637] [planner_server]: Creating bond (planner_server) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938323.298551835] [lifecycle_manager_navigation]: Server planner_server connected with bond.
  [lifecycle_manager-8] [INFO] [1692938323.298581490] [lifecycle_manager_navigation]: Activating behavior_server
  [behavior_server-4] [INFO] [1692938323.298757467] [behavior_server]: Activating spin
  [behavior_server-4] [INFO] [1692938323.298766257] [behavior_server]: Activating backup
  [behavior_server-4] [INFO] [1692938323.298772321] [behavior_server]: Activating drive_on_heading
  [behavior_server-4] [INFO] [1692938323.298778313] [behavior_server]: Activating assisted_teleop
  [behavior_server-4] [INFO] [1692938323.298783653] [behavior_server]: Activating wait
  [behavior_server-4] [INFO] [1692938323.298792649] [behavior_server]: Creating bond (behavior_server) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938323.400977077] [lifecycle_manager_navigation]: Server behavior_server connected with bond.
  [lifecycle_manager-8] [INFO] [1692938323.401038759] [lifecycle_manager_navigation]: Activating bt_navigator
  [bt_navigator-5] [INFO] [1692938323.422932591] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938323.525302904] [lifecycle_manager_navigation]: Server bt_navigator connected with bond.
  [lifecycle_manager-8] [INFO] [1692938323.525337624] [lifecycle_manager_navigation]: Activating waypoint_follower
  [waypoint_follower-6] [INFO] [1692938323.525565539] [waypoint_follower]: Creating bond (waypoint_follower) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938323.628096670] [lifecycle_manager_navigation]: Server waypoint_follower connected with bond.
  [lifecycle_manager-8] [INFO] [1692938323.628133169] [lifecycle_manager_navigation]: Activating velocity_smoother
  [velocity_smoother-7] [INFO] [1692938323.628403318] [velocity_smoother]: Creating bond (velocity_smoother) to lifecycle manager.
  [lifecycle_manager-8] [INFO] [1692938323.730793485] [lifecycle_manager_navigation]: Server velocity_smoother connected with bond.
  [lifecycle_manager-8] [INFO] [1692938323.730822686] [lifecycle_manager_navigation]: Managed nodes are active
  [lifecycle_manager-8] [INFO] [1692938323.730829778] [lifecycle_manager_navigation]: Creating bond timer...

Please feel free to ask for any additional information.
Thanks in advance!

[mamariomiamo]

Just wondering if UAV would require a 3D costmap?

[Ryanf55]

Yea we will need full 3D support eventually, but the scope right now is using NAV2 on a copter but only moving in 2D.

[SteveMacenski]

Please ask for configuration help on Robotics Stack Exchange.

But generally speaking, I don't see a costmap in your video, so you may want to look into that + configuring it for your application.

[Karthik03B]

It will help alot if you can push this project to your GitHub repositories. Thank you

[Ryanf55]

It will help alot if you can push this project to your GitHub repositories. Thank you

Everything is pushed. https://discuss.ardupilot.org/t/gsoc-2023-gps-denied-autonomous-exploration-with-ros-2/101121/20?u=rfriedman