From 847c7980c7212d1d7235ba666b7e8fd14e5002d2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Aug 2023 10:41:46 +1000 Subject: [PATCH] AP_DDS: use AP_ExternalControl for velocity control Signed-off-by: Ryan Friedman Co-authored-by: Andrew Tridgell --- libraries/AP_DDS/AP_DDS_Client.cpp | 16 +++++++---- libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 32 +++++++++++++++++++++ libraries/AP_DDS/AP_DDS_ExternalControl.h | 11 +++++++ libraries/AP_DDS/AP_DDS_Frames.h | 7 +++++ 4 files changed, 60 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_DDS/AP_DDS_ExternalControl.cpp create mode 100644 libraries/AP_DDS/AP_DDS_ExternalControl.h create mode 100644 libraries/AP_DDS/AP_DDS_Frames.h diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 5c2933f2d5..0d7b26d7b4 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -10,6 +10,11 @@ #include #include #include +#include +#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; } } diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp new file mode 100644 index 0000000000..8d551cac13 --- /dev/null +++ b/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 + +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 \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h new file mode 100644 index 0000000000..dbffafdd81 --- /dev/null +++ b/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 diff --git a/libraries/AP_DDS/AP_DDS_Frames.h b/libraries/AP_DDS/AP_DDS_Frames.h new file mode 100644 index 0000000000..0bdaf354ec --- /dev/null +++ b/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";