AP_DDS: use AP_ExternalControl for velocity control

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Co-authored-by: Andrew Tridgell <tridge60@gmail.com>
This commit is contained in:
Andrew Tridgell 2023-08-09 10:41:46 +10:00
parent 10435ca3c4
commit 847c7980c7
4 changed files with 60 additions and 6 deletions

View File

@ -10,6 +10,11 @@
#include <AP_BattMonitor/AP_BattMonitor.h> #include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Arming/AP_Arming.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_Client.h"
#include "AP_DDS_Topic_Table.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_LOCAL_VELOCITY_TOPIC_MS = 33;
static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; 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. // Define the subscriber data members, which are static class scope.
// If these are created on the stack in the subscriber, // 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++; subscribe_sample_count++;
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received geometry_msgs/TwistStamped"); #if AP_EXTERNAL_CONTROL_ENABLED
// TODO implement the velocity control to AP 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; break;
} }
} }

View File

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

View File

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

View File

@ -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";