search

ArduPilot / ardupilot_ros

ArduPilot ROS integration
GNU General Public License v3.0
15 stars 9 forks source link

WIP: Add integration with nav2 #11

Closed pedro-fuoco closed 1 month ago

pedro-fuoco commented 1 year ago

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

Solves https://github.com/ArduPilot/ardupilot_gz/issues/5

pedro-fuoco commented 1 year ago

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 commented 1 year ago

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. nav2 I will focus on fixing the parameters

pedro-fuoco commented 1 year ago

https://github.com/ArduPilot/ardupilot_ros/assets/62964137/c38c65b3-a489-41d8-8d17-26785f5a73f2

It works now!

pedro-fuoco commented 1 year ago

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

pedro-fuoco commented 1 year ago

@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!

Screenshot from 2023-09-02 00-23-05 Tests are looking good

Ryanf55 commented 1 year ago

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 commented 1 year ago

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

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 commented 11 months ago

@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 commented 8 months ago

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 commented 7 months ago

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

Changes needed

ZihuiGao commented 4 months ago

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: Screenshot from 2024-04-18 12-48-58 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 commented 4 months ago

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 commented 4 months ago

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 https://github.com/ArduPilot/ardupilot_ros/pull/11#issuecomment-2063175578, or unsubscribe https://github.com/notifications/unsubscribe-auth/BH5ERRHRIEV4SESSVYSZGXTY55WWDAVCNFSM6AAAAAA32NXEY2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDANRTGE3TKNJXHA . You are receiving this because you commented.Message ID: @.***>

srmainwaring commented 1 month ago

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