WIP: Add integration with nav2 #11

pedro-fuoco · 2023-08-22T20:04:48Z

The purpose of this PR is to integrate the navigation2 stack with Ardupilot and Cartographer.

pedro-fuoco · 2023-08-22T21:21:38Z

I've been using this branch to test things out while /ap/cmd_vel still accepts earth frame messages and nav2 only publishes Twist instead of TwistStamped:
https://github.com/pedro-fuoco/ardupilot/tree/body-frame-temporary

pedro-fuoco · 2023-08-22T21:25:41Z

There's currently a problem with the navigation.yaml file, as nav2 isn't creating it's local cost map, nor is it avoiding obstacles.
The good new is that the rest is working, the RVIZ goal pose tool is integrated with nav2, and the messages it publishes are being correctly received by my branch of Ardupilot.

I will focus on fixing the parameters

config/navigation.yaml

pedro-fuoco · 2023-08-30T15:59:25Z

nav2.mp4

It works now!

pedro-fuoco · 2023-08-30T16:28:04Z

This PR requires that ArduPilot/ardupilot_gz#26 and ArduPilot/ardupilot#24734 gets merged beforehand to work correctly

README.md

config/navigation.yaml

launch/navigation.launch.py

pedro-fuoco · 2023-09-02T03:27:49Z

@Ryanf55 just overhauled the config file, a lot of the parameters had default values so I removed most to clean it up. Fixed a lot of things you pointed out as well, thanks for the review!

Tests are looking good

Ryanf55 · 2023-09-04T03:04:29Z

Request: Can you try adjust the default location of the camera in the gazebo world scene so you can see the copter without having to rotate the camera. With the defaults, even at 2.5m altitude, you can't see it.

Ryanf55 · 2023-09-04T03:24:56Z

Tested just now, this is the list of nodes.

ryan@B650-970:~/Dev/ros2_ws$ ros2 node list
/Ardupilot_DDS_XRCE_Client
/behavior_server
/bt_navigator
/bt_navigator_navigate_through_poses_rclcpp_node
/bt_navigator_navigate_to_pose_rclcpp_node
/cartographer_node
/cartographer_occupancy_grid_node
/controller_server
/global_costmap/global_costmap
/lifecycle_manager_navigation
/local_costmap/local_costmap
/planner_server
/robot_state_publisher
/ros_gz_bridge
/rviz
/rviz_navigation_dialog_action_client
/smoother_server
/transform_listener_impl_556a908cac60
/transform_listener_impl_558d515cd620
/transform_listener_impl_55dde47130c0
/transform_listener_impl_55edf7db2940
/transform_listener_impl_56341e499650
/transform_listener_impl_5655558d2f80
/velocity_smoother
/waypoint_follower

We have 73 topics:

ryan@B650-970:~/Dev/ros2_ws$ ros2 topic list
/ap/battery/battery0
/ap/clock
/ap/cmd_vel
/ap/geopose/filtered
/ap/joy
/ap/navsat/navsat0
/ap/pose/filtered
/ap/tf
/ap/tf_static
/ap/time
/ap/twist/filtered
/battery
/behavior_server/transition_event
/behavior_tree_log
/bond
/bt_navigator/transition_event
/clicked_point
/clock
/cmd_vel
/constraint_list
/controller_server/transition_event
/cost_cloud
/diagnostics
/downsampled_costmap
/downsampled_costmap_updates
/evaluation
/global_costmap/costmap
/global_costmap/costmap_raw
/global_costmap/costmap_updates
/global_costmap/footprint
/global_costmap/global_costmap/transition_event
/global_costmap/published_footprint
/global_costmap/voxel_marked_cloud
/goal_pose
/gz/tf
/gz/tf_static
/imu
/initialpose
/joint_states
/landmark_poses_list
/local_costmap/clearing_endpoints
/local_costmap/costmap
/local_costmap/costmap_raw
/local_costmap/costmap_updates
/local_costmap/footprint
/local_costmap/local_costmap/transition_event
/local_costmap/published_footprint
/local_costmap/voxel_marked_cloud
/local_plan
/map
/map_updates
/marker
/odom
/odometry
/parameter_events
/plan
/plan_smoothed
/planner_server/transition_event
/received_global_plan
/robot_description
/rosout
/scan
/scan_matched_points2
/smoother_server/transition_event
/speed_limit
/submap_list
/tf
/tf_static
/trajectory_node_list
/transformed_global_plan
/velocity_smoother/transition_event
/waypoint_follower/transition_event
/waypoints

The costmap is showing up now.

One thing is the velocity smoother is observed to not obey the remaps. I suggest we take it out for now because it doesn't yet support subscribing to TwistStamped.

ryan@B650-970:~/Dev/ros2_ws$ ros2 topic info /cmd_vel -vv
Type: geometry_msgs/msg/Twist

Publisher count: 1

Node name: velocity_smoother
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.05.f7.ed.e1.a5.a1.01.00.00.00.00.00.1e.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

ryan@B650-970:~/Dev/ros2_ws$ ros2 topic info /ap/cmd_vel -vv
Type: ['geometry_msgs/msg/Twist', 'geometry_msgs/msg/TwistStamped']

Publisher count: 5

Node name: controller_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.05.f7.e1.e1.74.99.01.00.00.00.00.00.53.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.05.f7.e7.e1.15.29.01.00.00.00.00.00.2c.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.05.f7.e7.e1.15.29.01.00.00.00.00.00.35.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.05.f7.e7.e1.15.29.01.00.00.00.00.00.3e.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.05.f7.e7.e1.15.29.01.00.00.00.00.00.47.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 2

Node name: Ardupilot_DDS_XRCE_Client
Node namespace: /
Topic type: geometry_msgs/msg/TwistStamped
Endpoint type: SUBSCRIPTION
GID: 01.0f.05.f7.9b.8b.6a.41.01.00.00.00.00.00.0b.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: velocity_smoother
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: SUBSCRIPTION
GID: 01.0f.05.f7.ed.e1.a5.a1.01.00.00.00.00.00.1f.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

srmainwaring · 2023-09-29T21:10:27Z

@pedro-fuoco and @Ryanf55 - I've got this all running on macOS using the custom nav2 branch, and it's looking great.

It would be good to unlink this PR from the upstream dependency on the ongoing work in nav2 to deal with TwistStamped and the discussion on adding extra frame information for general velocity transforms.

Perhaps we could add a message filter node that subscribes to /clock and /cmd_vel (unstamped) and multiplexes them into a /cmd_vel_stamped. It's not nice workaround, or that accurate, but would enable this PR to run and we could drop the extra node from the launch when the upstreams fixes are in place?

Ryanf55 · 2024-01-08T21:41:20Z

Upstream is about complete. It's ready for testing with the changes I pushed.

Test with this backport on humble:
https://github.com/Ryanf55/navigation2/tree/backport-humble-1594

srmainwaring · 2024-02-08T19:06:07Z

As discussed here, support for TwistStamped in navigation2 will not be backported to ROS Humble.

A minor change to this PR allows navigation2 support in ArduPilot 4.5 without having to upgrade to a later version of ROS. The proposal is to add a twist_stamper node to the navigation launch to convert the unstamped cmd_vel from nav2 to a stamped ap/cmd_vel consumed by AP_DDS.

Details

Patch: 916366d
ROS package: https://index.ros.org/p/twist_stamper/
Install in Ubuntu using sudo apt install ros-humble-twist-stamper

Changes needed

Rebase this PR on the humble branch
Cherry pick 916366d

srmainwaring

@pedro-fuoco , this is a great addition. Request that changes proposed in #11 (comment) are made (rebase and cherry-pick) so this works with current nav2 for ROS 2 Humble.

ZihuiGao · 2024-04-18T06:49:20Z

ROS2 humble, Ubuntu 22.04
@pedro-fuoco and @Ryanf55 , great work. I just try to follow your navgation branch and copy the launch file and yaml file to my ardupilot_ros package because I already have the humble branch. It seems like every thing works well except that the drone cannot reach the goal point even if the path looks correct in rviz. The drone will only move towards x like this:
Screencast from 04-18-2024 12:46:19 PM.webm
What is wrong with it? I also check the topic /cmd_vel and /ap/cmd_vel, they don't have the y value like this:

Is there something wrong with the nav2 pgk? I think nav2 is automatically installed with ros2 so should I remove them and download it from the source code? Or did I miss a better solution?

Ryanf55 · 2024-04-18T07:10:02Z

Nice, glad to see you are giving this a go.

Yea, we only control x direction, and then rotate around the Z axis because of the way nav2 is configured for ground robots assumes you can't go in the Y direction.

I think it's time we merge in Rhys's patch to this branch, and get all this work merged in. We don't have any indication Nav2 will support TwistStamped on humble.

Please follow this thread, I'll reach back out when it's ready for another round of testing.

ZihuiGao · 2024-04-18T08:00:02Z

Ok I understand now. Thanks.

…

On Thu, Apr 18, 2024 at 3:10 PM Ryan ***@***.***> wrote: Nice, glad to see you are giving this a go. Yea, we only control x direction, and then rotate around the Z axis because of the way nav2 is configured for ground robots assumes you can't go in the Y direction. I think it's time we merge in Rhys's patch to this branch, and get all this work merged in. We don't have any indication Nav2 will support TwistStamped on humble. Please follow this thread, I'll reach back out when it's ready for another round of testing. — Reply to this email directly, view it on GitHub <#11 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/BH5ERRHRIEV4SESSVYSZGXTY55WWDAVCNFSM6AAAAAA32NXEY2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDANRTGE3TKNJXHA> . You are receiving this because you commented.Message ID: ***@***.***>

Signed-off-by: Ryan Friedman <[email protected]>

srmainwaring · 2024-07-21T18:18:17Z

Rebased on humble and update commit messages to use prefix nav2:.

srmainwaring

@pedro-fuoco and @Ryanf55 LGTM. Issues with ROS 2 Humble upstream dependency on TwistStamped to be dealt with in #28

pedro-fuoco mentioned this pull request Aug 22, 2023

ardupilot_ros: turn it into a ros 2 package #6

Closed

pedro-fuoco added the ros2 label Aug 22, 2023

pedro-fuoco changed the title ~~Add integration with nav2~~ WIP: Add integration with nav2 Aug 22, 2023

Ryanf55 assigned pedro-fuoco Aug 23, 2023

Ryanf55 self-requested a review August 23, 2023 00:05

pedro-fuoco force-pushed the ros2-navigation branch 2 times, most recently from c372b66 to 5f1f5e0 Compare August 30, 2023 15:54

pedro-fuoco commented Aug 30, 2023

View reviewed changes

config/navigation.yaml Show resolved Hide resolved

pedro-fuoco mentioned this pull request Aug 30, 2023

Remove remaps from cartographer launch files #14

Merged

pedro-fuoco force-pushed the ros2-navigation branch from 5f1f5e0 to 118f949 Compare August 30, 2023 16:31

Ryanf55 reviewed Aug 30, 2023

View reviewed changes

pedro-fuoco force-pushed the ros2-navigation branch 2 times, most recently from d4dfc31 to 5c7c58d Compare September 2, 2023 03:26

pedro-fuoco mentioned this pull request Sep 4, 2023

ardupilot_ros: Remove workarounds, use packages-up-to in instructions #15

Merged

pedro-fuoco force-pushed the ros2-navigation branch 2 times, most recently from 26b7770 to 5dcedc9 Compare September 4, 2023 03:24

pedro-fuoco mentioned this pull request Sep 18, 2023

Copter: fix odometry transform ArduPilot/ardupilot_gz#32

Merged

srmainwaring mentioned this pull request Oct 2, 2023

AP_UROS: add micro-ROS client ArduPilot/ardupilot#25154

Draft

4 tasks

Ryanf55 force-pushed the ros2-navigation branch from 5dcedc9 to 6d49bdd Compare January 8, 2024 21:35

Ryanf55 force-pushed the ros2-navigation branch from 6d49bdd to 5176a46 Compare January 8, 2024 21:59

srmainwaring requested changes Feb 8, 2024

View reviewed changes

ashBabu mentioned this pull request Feb 29, 2024

Copter not moving to goal with nav2 #25

Closed

Ryanf55 self-requested a review April 18, 2024 07:10

srmainwaring mentioned this pull request Apr 19, 2024

Way Point Following using Cartographer #27

Closed

srmainwaring force-pushed the ros2-navigation branch from 5176a46 to b004009 Compare July 21, 2024 18:12

pedro-fuoco and others added 6 commits July 21, 2024 19:15

nav2: Remove files associated with navigation in ROS 1

586777d

nav2: Add nav2 config file

a46ee33

nav2: Add Navigation2 and Rviz integration

5fdb699

nav2: Add nav2 example on README

1ba418b

nav2: Add nav2 as a package dependency

39edd89

nav2: Add twistStamped params as needed

1cf211d

Signed-off-by: Ryan Friedman <[email protected]>

srmainwaring force-pushed the ros2-navigation branch from b004009 to 1cf211d Compare July 21, 2024 18:17

srmainwaring mentioned this pull request Jul 21, 2024

Add twist-stamper node so nav2 example works with humble #28

Merged

srmainwaring self-requested a review July 22, 2024 07:53

srmainwaring approved these changes Jul 22, 2024

View reviewed changes

srmainwaring merged commit 940efb9 into ArduPilot:humble Jul 22, 2024
2 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

WIP: Add integration with nav2 #11

WIP: Add integration with nav2 #11

pedro-fuoco commented Aug 22, 2023 •

edited by Ryanf55

Loading

pedro-fuoco commented Aug 22, 2023

pedro-fuoco commented Aug 22, 2023 •

edited

Loading

pedro-fuoco commented Aug 30, 2023

pedro-fuoco commented Aug 30, 2023 •

edited

Loading

pedro-fuoco commented Sep 2, 2023

Ryanf55 commented Sep 4, 2023

Ryanf55 commented Sep 4, 2023

srmainwaring commented Sep 29, 2023

Ryanf55 commented Jan 8, 2024

srmainwaring commented Feb 8, 2024 •

edited

Loading

srmainwaring left a comment

ZihuiGao commented Apr 18, 2024

Ryanf55 commented Apr 18, 2024

ZihuiGao commented Apr 18, 2024 via email •

edited

Loading

srmainwaring commented Jul 21, 2024

srmainwaring left a comment

WIP: Add integration with nav2 #11

WIP: Add integration with nav2 #11

Conversation

pedro-fuoco commented Aug 22, 2023 • edited by Ryanf55 Loading

pedro-fuoco commented Aug 22, 2023

pedro-fuoco commented Aug 22, 2023 • edited Loading

pedro-fuoco commented Aug 30, 2023

pedro-fuoco commented Aug 30, 2023 • edited Loading

pedro-fuoco commented Sep 2, 2023

Ryanf55 commented Sep 4, 2023

Ryanf55 commented Sep 4, 2023

srmainwaring commented Sep 29, 2023

Ryanf55 commented Jan 8, 2024

srmainwaring commented Feb 8, 2024 • edited Loading

Details

Changes needed

srmainwaring left a comment

Choose a reason for hiding this comment

ZihuiGao commented Apr 18, 2024

Ryanf55 commented Apr 18, 2024

ZihuiGao commented Apr 18, 2024 via email • edited Loading

srmainwaring commented Jul 21, 2024

srmainwaring left a comment

Choose a reason for hiding this comment

pedro-fuoco commented Aug 22, 2023 •

edited by Ryanf55

Loading

pedro-fuoco commented Aug 22, 2023 •

edited

Loading

pedro-fuoco commented Aug 30, 2023 •

edited

Loading

srmainwaring commented Feb 8, 2024 •

edited

Loading

ZihuiGao commented Apr 18, 2024 via email •

edited

Loading