AP_DDS: Design the ROS2 control interface

[Ryanf55]

For a ROS2 offboard computer looking control the autopilot, there are a variety of methods.

From a high level, it could be waypoint-based, feeding waypoints and letting the autopilot figure out how to get to them.

Other control methods (acceleration, rate) control are lower level, and require more real time behavior offboard. Pending some performance metrics that need to be completed, design the generic API for ROS2 to control ardupilot.

ROS REP-147 exposes a good set of messages, but they don't have any feedback on success of commands, or progress indications.

Since the DDS interface doesn't support services or actions (yet), this ticket can be scoped on the open loop control.

Leonard's talk on guided mode is an excellent primer: https://www.youtube.com/live/4G1sxQ_CVng?feature=share&t=11204

He proposes the ideal method is to control position, velocity, and acceleration at the same time. Since ardupilot has some sense of the vehicle limits with respect to WP_NAV params, and these limits are fixed at runtime on copter usually, they can be sent to the companion computer once (on boot). Later, when dynamics are more variable, such as accounting for drag with respect to velocity, or throttling the climb rate based on battery sag, the dynamics can be shared dynamically. For some more info on how to apply the acceleration limits, see this: https://github.com/ros-planning/navigation2/issues/2633

Once the companion computer has the vehicle limits, it will need to take its global plan, and smooth it. For a 2D smoother, see here: https://navigation.ros.org/configuration/packages/configuring-constrained-smoother.html

The companion computer should publish PVA commands as fast as reasonably possible, even 10Hz, with the current requested command (timestamped). There is not currently any look ahead support. Since ArduCopter supports S-Curves, there is currently no need to send an entire trajectory. S-curves attempt to minimize cross track error in 3D.

[Ryanf55]

Starting to refine this. I'll enclose a proposal below. Here's the rough plan of action:

1. Define the name for control library, apparently Randy is good at naming it. For now: AP_ExternalControl
2. Define the coordinate systems and messages for control
   • Mavlink messages will be unchanged, but must be compatable
   • ROS2 can follow REP 147
   • Internal library should not use centidegrees or cm/s, prefer SI
   • It is ok to work in degrees or radians internally, just pick one
3. Draft up the header file for AP_ExternalControl within scope for PVA control
   • Waypoints - out of scope
4. Define how each vehicle type will subclass the control library
5. Implement the interface with unit tests (no mavlink or DDS)
6. Add the DDS messages for PVA control and hook them up to AP_ExternalControl
7. Create backends for the control, such as AP_ExternalControlGCS and AP_ExternalControlDDS and AP_ExternalControlScripting
8. Replace mavlink direct control to go through AP_ExternalControl, perhaps called AP_ExternalControlGCS

[slim71]

Some (hopefully useful) feedback.

I've reviewed REP147 with control algorithms in mind, not only for the project I'll build but also from a general point of view. I'd say the most important stuff, which should take higher priority, is

1. the pose interface, using messages of type geometry_msgs/PoseStamped on topic command_pose ⇨ this would allow the simplest control (which I guess is maybe the most desirable to the average user): provide a pose to the vehicle and let its autopilot reach it autonomously
2. the rate interface, with messages geometry_msgs/TwistStamped on topic command_velocity ⇨ as REP147 states for ROS, this is one standard way to control robots (and so vehicles too). For example, it could be extremely useful for "manual" controls (i.e. as if a joystick is used)
3. the odometry info, using messages nav_msgs/Odometry on topic odom or the proposed and more complete nav_msgs/OdometryWithAcceleration on topic odometry ⇨ data estimation is crucial for any real application
   

The following would instead add information that is not strictly crucial, but really useful to add more functionalities

• messages nav_msgs/GlobalPosition on topic command_gps_pose ⇨ I can see many applications using this to specify a mission consisting of many goals. This is also a more granulated way of giving desired poses as goal, with relaxed constraints
• datatypes mav_msgs/FlightMode and mav_msgs/SetFlightMode for the set_flight_mode service

Finally, some more stuff for additional debug purposes or even more functionalities:

• battery status
• vehicle status
• IMU data
• GPS data
• position reporting in different frames
• mission management
• collision detection
• connectivity checks

I realize this last list is a bit long, but as I said in my opinion these would have lower priority compared to the ones above.

A couple more things.

• Internal library should not use centidegrees or cm/s, prefer SI
• It is ok to work in degrees or radians internally, just pick one

1. I completely agree on using SI units, since it's universal. That would also settle the degree vs radians argument, but the important stuff is being consistent. The same applies for Euler angles vs quaternions when it comes to rotations, but I guess that's not an issue here.
2. Just as an added info, in my case the auto mode would be crucial, since I plan to control a fleet later on. I think it is also very useful to the general user in both simulations and real applications (just think about "follow me" missions)
3. The update rates would change drastically depending on hardware/simulation, application, vehicle velocity, etc... As a general rule of thumb, average applications should be good with a frequency in the 20Hz-100Hz range (with 20Hz and a max velocity of 0.5m/s, the vehicle would move 0.025m=2.5cm between two updates and I think that's fairly acceptable)

[Ryanf55]

Controls interface

Position

• In Mavlink copter interface, you command a position, and ArduPilot handles the orientation for you. If you don't send velocity or acceleration, it will fly to it, and stop there. The same command handles both position and velocity.
  • https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
  • https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#set-position-target-global-int
• Decision: While Mavlink supports local and global position controls, we decided to expose only the local control method in the DDS interface.

Velocity Controller

• You can do velocity in ArduPilot, using the same mavlink packets above, see vx field for example. The mavlink packet also lets you send a yaw rate, but does not expose roll rates or pitch rates (these are controlled at a different level by ArduPilot itself).
• If a ROS user is sending velocity controls over geometry_msgs/TwistStamped, we should provide documentation on what fields to zero depending on the vehicle type. Randy thinks a level is missing. If we provide a velocity with a rotation target but no angle target, there are an infinite number of solutions

Acceration Controller

• We can also implement this, but it's likely going to be used less than position and velocity, so less of a priority.

Trajectory and Waypoint Control

Let's skip these for now (at least in scope of GSoC)

Interfacing with ArduPilot libraries

If we want to do AP_ExternalControl, it would likely involve making lots of changes to the vehicle code, which could take longer.

Instead, Randy initially proposes we use the utilities the scripting interface, by changing the define to be something like this:

#if AP_SCRIPTING_ENABLED OR AP_DDS_ENABLED

Note, the scripting interface does not currently expose an acceleration control yet, but we could add it for DDS (later).

This provides an advantage also because we don't have to worry about Mavlink at all yet, and all development would be constrained to the AP_DDS library which reduces the risk of breaking unrelated things.

Later the down the line, we can get help integrating AP_ExternalControl and doing it the "right" way.

Handing over controls

In general, a companion computer shouldn't change the flight mode, unless the developer is very sure they know what they are doing. This is dangerous, and should likely be protected.

• For example, if a plane is low on battery, ArduPilot engages RTL, and then the companion computer wants to send guided mode commands because it's still running its algorithm. This should be ignored. Same with "manual" control. This could disabled with a define or parameter or something. Normally, a human will change the flight mode from whatever they were in to guided mode. A user could click on the map, which sends a position command over mavlink, or this is where a companion computer will be allowed to send controls. If the companion computer is sending PVA controls and the vehicle is not in GUIDED mode, there is no current feedback to the mavlink user. AP_Vehicle likely knows, but it's not shared.

If the companion computer wants to request control (switch to guided), should it be allowed? Yes, it will need to be added, and obey the protections. IN mavlink, this is the message, which allows arming, takeoff, but not a command to go into guided. In the auto mode mission list, one item would be this guided enabled command. See this for a nice video. OR this for obstacle avoidance.