Enabled sending waypoints to ardupilot for copter and rover

[haarshitgarg]

Way point support for Copter

This is to fix the issue #25381

Implemented set_global_location for Rover and Copter ( taking inspiration from ArduPlane)

Demo

Taking everything from this pull request

In ArduPilot repo:

./sim_vehicle.py -v ArduCopter --map --console --enable-dds

Running MicroROS agent:

cd ~/ros2_ws
. /opt/ros/humble/setup.bash
colcon build --packages-up-to ardupilot_sitl
. install/setup.bash
cd src/ardupilot/libraries/AP_DDS
ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019 -r dds_xrce_profile.xml

Taking off the copter

mode guided
arm throttle
takeoff 50

Now publish a message for drone to go to greyhound track.

cd ~/ros2_ws
. /opt/ros/humble/setup.bash
colcon build --packages-up-to ardupilot_msgs
. install/setup.bash
ros2 topic pub /ap/cmd_gps_pose  ardupilot_msgs/msg/GlobalPosition "{header: {frame_id: map}, latitude: -35.345996, longitude: 149.159017, altitude: 40, coordinate_frame: 6}" --once

Way point support for copter Rover

Follow all the steps as for copter above

Demo Rover

Same as copter (plus altitude is ignored in ros message)
Result Screenshot (Rover going to the same location as copter)

[amilcarlucas]

Can most of this be moved to AP_Vehicle instead?

[haarshitgarg]

Can most of this be moved to AP_Vehicle instead?

Hello @amilcarlucas I am not really sure about that. I am just following the structure already being followed for ArduPlane.

[Ryanf55]

Can most of this be moved to AP_Vehicle instead?

Which parts do you think should be moved? Collapse AP_ExternalControl in AP_Vehicle, or something else?

[amilcarlucas]

Anything that is common and can be reuse is a good candidate.

That way sub and blimp will also get (at least parts) of this feature.

[Ryanf55]

Overall, this is looking great. Sorry for the delay on the review. I forgot to hit submit.
I'll run testing on it shortly. Have you tried using the python script I wrote to send a goal, and try it against copter?

[rmackay9]

This is looking pretty good I think, it would be good to squash some of the commits together and add the regular subsystem prefixes to them. E.g. commits affecting rover should start with "Rover: ", commits affecting Copter should start with "Copter: ". Also we should have any merge commits. Thanks!

[Ryanf55]

This is looking pretty good I think, it would be good to squash some of the commits together and add the regular subsystem prefixes to them. E.g. commits affecting rover should start with "Rover: ", commits affecting Copter should start with "Copter: ". Also we should have any merge commits. Thanks!

Hi @haarshitgarg , Do you mind fixing up the commits? This is looking nice and I'd like to get this in. We had another asking for copter waypoints on the discord thread.

[maximecapelle]

Is there a way to takeoff throught the ros2 topics aswell? I have used the additions you suggested and the waypoint navigation works when it is in the air but not to takeoff. It would be nice if we can set the drone to mode guided, arm and takeoff all through the ros2 topics! :)

[rmackay9]

@maximecapelle,

I'm not really familiar with this area but maybe this PR is what you're looking for? #26911

[maximecapelle]

@maximecapelle,

I'm not really familiar with this area but maybe this PR is what you're looking for? #26911

Thank you! I’ll check it out :)

[maximecapelle]

Also, is there a way to use waypoints in the local coordinate frame? (x, y, z) Using the "/ap/pose/filtered" topic msgs you have your position in the local map coordinates. Could someone help me implement that? Sorry I have little knowledge on Ardupilot source code haha.

[Ryanf55]

Also, is there a way to use waypoints in the local coordinate frame? (x, y, z) Using the "/ap/pose/filtered" topic msgs you have your position in the local map coordinates. Could someone help me implement that? Sorry I have little knowledge on Ardupilot source code haha.

No, that's not part of REP-147. ArduPilot's waypoints are in WGS-84 (global) coordinates. If you want to do planning on local coordinates that's fine, but transform to global before sending to the autopilot.

[Ryanf55]

Aside from the style changes, the functionality is spot-on. Well done!

I just force pushed the style fixes, enforced it in run_astyle.py and finally fixed the commit messages.

[Ryanf55]

@haarshitgarg Would you be interested in a doing a follow-up PR allowing users to set a yaw angle at the waypoint? This would be good for inspection use cases with a fixed camera where you point the drone at a cliff/wall at each waypoint.

[rmackay9]

Sorry to be nitpicky about naming but could we change this to clarify that we're setting a target location? we normally use the word "position" to indicate an offset from the EKF origin and "location" for lat, lon, alt. Not specifying that this is a target (e.g. controller target) rather than an [ekf] estimate is important as well I think.

[Ryanf55]

Sorry to be nitpicky about naming but could we change this to clarify that we're setting a target location? we normally use the word "position" to indicate an offset from the EKF origin and "location" for lat, lon, alt. Not specifying that this is a target (e.g. controller target) rather than an [ekf] estimate is important as well I think.

Do you want the functions renamed in Plane and Copter, or just External control?

[rmackay9]

Hi @Ryanf55,

I think set_global_position() should be renamed to set_target_location(). That will lead to identically named methods in the Copter and Rover vehicle class and in AP_ExternalControl but I think that's OK... maybe other devs will disagree but we can clear that up on the dev call if not before.

[Ryanf55]

Ok. It's currently named based on REP-147: https://ros.org/reps/rep-0147.html#goal-interface