AP_ExternalControl: initial implementation of external control library

ArduCopter/AP_ExternalControl_Copter.cpp

@@ -0,0 +1,37 @@
+/*
+  external control library for copter
+ */
+
+
+#include "AP_ExternalControl_Copter.h"
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+#include "Copter.h"
+
+/*
+  set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
+  velocity is in earth frame, NED, m/s
+*/
+bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
+{
+    if (!ready_for_external_control()) {
+        return false;
+    }
+    const float yaw_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;
+
+    // Copter velocity is positive if aicraft is moving up which is opposite the incoming NED frame.
+    Vector3f velocity_NEU_ms {
+        linear_velocity.x,
+        linear_velocity.y,
+        -linear_velocity.z };
+    Vector3f velocity_up_cms = velocity_NEU_ms * 100;
+    copter.mode_guided.set_velocity(velocity_up_cms, false, 0, !isnan(yaw_rate_rads), yaw_rate_cds);
+    return true;
+}
+
+bool AP_ExternalControl_Copter::ready_for_external_control()
+{
+    return copter.flightmode->in_guided_mode() && copter.motors->armed();
+}
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

ArduCopter/AP_ExternalControl_Copter.h

@@ -0,0 +1,26 @@
+/*
+  external control library for copter
+ */
+#pragma once
+
+#include <AP_ExternalControl/AP_ExternalControl.h>
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+class AP_ExternalControl_Copter : public AP_ExternalControl {
+public:
+    /*
+      Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw.
+      Velocity is in earth frame, NED [m/s].
+      Yaw is in earth frame, NED [rad/s].
+     */
+    bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override;
+private:
+    /*
+      Return true if Copter is ready to handle external control data.
+      Currently checks mode and arm states.
+    */
+    bool ready_for_external_control();
+};
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

ArduCopter/Copter.h

@@ -100,6 +100,11 @@
 #include "AP_Rally.h"           // Rally point library
 #include "AP_Arming.h"
 
+#include <AP_ExternalControl/AP_ExternalControl_config.h>
+#if AP_EXTERNAL_CONTROL_ENABLED
+#include "AP_ExternalControl_Copter.h"
+#endif
+
 #include <AP_Beacon/AP_Beacon_config.h>
 #if AP_BEACON_ENABLED
  #include <AP_Beacon/AP_Beacon.h>
@@ -193,6 +198,9 @@ class Copter : public AP_Vehicle {
     friend class AP_AdvancedFailsafe_Copter;
 #endif
     friend class AP_Arming_Copter;
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Copter;
+#endif
     friend class ToyMode;
     friend class RC_Channel_Copter;
     friend class RC_Channels_Copter;
@@ -319,6 +327,12 @@ class Copter : public AP_Vehicle {
     AP_OpticalFlow optflow;
 #endif
 
+    // external control library
+#if AP_EXTERNAL_CONTROL_ENABLED
+    AP_ExternalControl_Copter external_control;
+#endif
+
+
     // system time in milliseconds of last recorded yaw reset from ekf
     uint32_t ekfYawReset_ms;
     int8_t ekf_primary_core;

ArduCopter/mode.h

@@ -2,6 +2,7 @@
 
 #include "Copter.h"
 #include <AP_Math/chirp.h>
+#include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
 class Parameters;
 class ParametersG2;
 
@@ -957,6 +958,10 @@ class ModeFlowHold : public Mode {
 class ModeGuided : public Mode {
 
 public:
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Copter;
+#endif
+
     // inherit constructor
     using Mode::Mode;
     Number mode_number() const override { return Number::GUIDED; }

ArduPlane/Plane.h

@@ -83,6 +83,10 @@
 #include <AP_Landing/AP_Landing.h>
 #include <AP_LandingGear/AP_LandingGear.h>     // Landing Gear library
 #include <AP_Follow/AP_Follow.h>
+#include <AP_ExternalControl/AP_ExternalControl_config.h>
+#if AP_EXTERNAL_CONTROL_ENABLED
+#include <AP_ExternalControl/AP_ExternalControl.h>
+#endif
 
 #include "GCS_Mavlink.h"
 #include "GCS_Plane.h"
@@ -770,6 +774,11 @@ class Plane : public AP_Vehicle {
 
     AP_Param param_loader {var_info};
 
+    // dummy implementation of external control
+#if AP_EXTERNAL_CONTROL_ENABLED
+    AP_ExternalControl external_control;
+#endif
+
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
 

Rover/Rover.h

@@ -44,6 +44,10 @@
 #include <AP_OpticalFlow/AP_OpticalFlow.h>
 #include <AC_PrecLand/AC_PrecLand_config.h>
 #include <AP_Follow/AP_Follow_config.h>
+#include <AP_ExternalControl/AP_ExternalControl_config.h>
+#if AP_EXTERNAL_CONTROL_ENABLED
+#include <AP_ExternalControl/AP_ExternalControl.h>
+#endif
 
 // Configuration
 #include "defines.h"
@@ -143,6 +147,11 @@ class Rover : public AP_Vehicle {
     // Arming/Disarming management class
     AP_Arming_Rover arming;
 
+    // dummy external control implementation
+#if AP_EXTERNAL_CONTROL_ENABLED
+    AP_ExternalControl external_control;
+#endif
+
 #if AP_OPTICALFLOW_ENABLED
     AP_OpticalFlow optflow;
 #endif

Tools/ardupilotwaf/ardupilotwaf.py

@@ -115,6 +115,7 @@
     'AP_AIS',
     'AP_OpenDroneID',
     'AP_CheckFirmware',
+    'AP_ExternalControl',
 ]
 
 def get_legacy_defines(sketch_name, bld):

Tools/scripts/run_astyle.py

@@ -21,6 +21,7 @@ def __init__(self):
         self.retcode = 0
         self.directories_to_check = [
             'libraries/AP_DDS',
+            'libraries/AP_ExternalControl'
         ]
         self.files_to_check = []
 

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -10,6 +10,11 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Arming/AP_Arming.h>
+#include <AP_ExternalControl/AP_ExternalControl_config.h>
+#if AP_EXTERNAL_CONTROL_ENABLED
+#include "AP_DDS_ExternalControl.h"
+#endif
+#include "AP_DDS_Frames.h"
 
 #include "AP_DDS_Client.h"
 #include "AP_DDS_Topic_Table.h"
@@ -24,10 +29,6 @@ static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10;
-static char WGS_84_FRAME_ID[] = "WGS-84";
-
-// https://www.ros.org/reps/rep-0105.html#base-link
-static char BASE_LINK_FRAME_ID[] = "base_link";
 
 // Define the subscriber data members, which are static class scope.
 // If these are created on the stack in the subscriber,
@@ -475,8 +476,11 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
         }
 
         subscribe_sample_count++;
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received geometry_msgs/TwistStamped");
-        // TODO implement the velocity control to AP
+#if AP_EXTERNAL_CONTROL_ENABLED
+        if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) {
+            // TODO #23430 handle velocity control failure through rosout, throttled.
+        }
+#endif // AP_EXTERNAL_CONTROL_ENABLED
         break;
     }
     }

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -0,0 +1,32 @@
+#if AP_DDS_ENABLED
+
+#include "AP_DDS_ExternalControl.h"
+#include "AP_DDS_Frames.h"
+
+#include <AP_ExternalControl/AP_ExternalControl.h>
+
+bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+    if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) != 0) {
+        // Although REP-147 says cmd_vel should be in body frame, all the AP math is done in earth frame.
+        // This is because accounting for the gravity vector.
+        // Although the ROS 2 interface could support body-frame velocity control in the future,
+        // it is currently not supported.
+        return false;
+    }
+
+    // Convert commands from ENU to NED frame
+    Vector3f linear_velocity {
+        float(cmd_vel.twist.linear.y),
+        float(cmd_vel.twist.linear.x),
+        float(-cmd_vel.twist.linear.z) };
+    const float yaw_rate = -cmd_vel.twist.angular.z;
+    return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
+}
+
+
+#endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_ExternalControl.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#if AP_DDS_ENABLED
+#include "geometry_msgs/msg/TwistStamped.h"
+
+class AP_DDS_External_Control
+{
+public:
+    static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+};
+#endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_Frames.h

@@ -0,0 +1,7 @@
+#pragma once
+
+static constexpr char WGS_84_FRAME_ID[] = "WGS-84";
+// https://www.ros.org/reps/rep-0105.html#base-link
+static constexpr char BASE_LINK_FRAME_ID[] = "base_link";
+// https://www.ros.org/reps/rep-0105.html#map
+static constexpr char MAP_FRAME[] = "map";

libraries/AP_ExternalControl/AP_ExternalControl.cpp

@@ -0,0 +1,24 @@
+#include "AP_ExternalControl.h"
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+// singleton instance
+AP_ExternalControl *AP_ExternalControl::singleton;
+
+AP_ExternalControl::AP_ExternalControl()
+{
+    singleton = this;
+}
+
+
+namespace AP
+{
+
+AP_ExternalControl *externalcontrol()
+{
+    return AP_ExternalControl::get_singleton();
+}
+
+};
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

libraries/AP_ExternalControl/AP_ExternalControl.h

@@ -0,0 +1,43 @@
+/*
+  external control library for MAVLink, DDS and scripting
+ */
+
+#pragma once
+
+#include "AP_ExternalControl_config.h"
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+#include <AP_Math/AP_Math.h>
+
+class AP_ExternalControl
+{
+public:
+
+    AP_ExternalControl();
+    /*
+      Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw.
+      Velocity is in earth frame, NED [m/s].
+      Yaw is in earth frame, NED [rad/s].
+     */
+    virtual bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
+    {
+        return false;
+    }
+
+    static AP_ExternalControl *get_singleton(void)
+    {
+        return singleton;
+    }
+
+private:
+    static AP_ExternalControl *singleton;
+};
+
+
+namespace AP
+{
+AP_ExternalControl *externalcontrol();
+};
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

libraries/AP_ExternalControl/AP_ExternalControl_config.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#ifndef AP_EXTERNAL_CONTROL_ENABLED
+#define AP_EXTERNAL_CONTROL_ENABLED 1
+#endif
+
+