From 6560d162a5a09d94dc0899115f462ca9387a9cbd Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 3 Oct 2023 17:09:44 +0100 Subject: [PATCH] AP_DDS: use common code from AP_ROS Signed-off-by: Rhys Mainwaring AP_DDS: use common code from AP_ROS for external control Signed-off-by: Rhys Mainwaring AP_DDS: use common code from AP_ROS for time type conversion Signed-off-by: Rhys Mainwaring AP_DDS: add mutable versions of accessor templates Signed-off-by: Rhys Mainwaring AP_DDS: use common code from AP_ROS for published topics Signed-off-by: Rhys Mainwaring AP_DDS: consolidate code for publishing nav sat fix Signed-off-by: Rhys Mainwaring AP_DDS: delete code moved to AP_ROS Signed-off-by: Rhys Mainwaring AP_DDS: rename mutable transforms Signed-off-by: Rhys Mainwaring AP_DDS: consolidate code for publishing static transforms Signed-off-by: Rhys Mainwaring AP_DDS: move template specialisation definitions to cpp to prevent duplicate symbols Signed-off-by: Rhys Mainwaring AP_DDS: fix formatting Signed-off-by: Rhys Mainwaring AP_DDS: add accessor templates for battery state Signed-off-by: Rhys Mainwaring AP_DDS: consolidate code for publishing battery state Signed-off-by: Rhys Mainwaring --- libraries/AP_DDS/AP_DDS_Client.cpp | 316 +------------------ libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 35 +- libraries/AP_DDS/AP_DDS_External_Odom.cpp | 61 +--- libraries/AP_DDS/AP_DDS_Type_Conversions.cpp | 47 ++- libraries/AP_DDS/AP_DDS_Type_Conversions.h | 59 ++++ 5 files changed, 122 insertions(+), 396 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index d5315e829705e..86cef43b11536 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -12,15 +12,19 @@ #include #include #include + +#include + #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" #endif -#include "AP_DDS_Frames.h" +// #include "AP_DDS_Frames.h" #include "AP_DDS_Client.h" #include "AP_DDS_Topic_Table.h" #include "AP_DDS_Service_Table.h" #include "AP_DDS_External_Odom.h" +#include "AP_DDS_Type_Conversions.h" // Enable DDS at runtime by default static constexpr uint8_t ENABLED_BY_DEFAULT = 1; @@ -65,335 +69,42 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) { - uint64_t utc_usec; - if (!AP::rtc().get_utc_usec(utc_usec)) { - utc_usec = AP_HAL::micros64(); - } - msg.sec = utc_usec / 1000000ULL; - msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; - + AP_ROS_Client::update_time(msg); } bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) { - // Add a lambda that takes in navsatfix msg and populates the cov - // Make it constexpr if possible - // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ - // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; - - // assert(instance >= GPS_MAX_RECEIVERS); - if (instance >= GPS_MAX_RECEIVERS) { - return false; - } - - auto &gps = AP::gps(); - WITH_SEMAPHORE(gps.get_semaphore()); - - if (!gps.is_healthy(instance)) { - msg.status.status = -1; // STATUS_NO_FIX - msg.status.service = 0; // No services supported - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return false; - } - - // No update is needed - const auto last_fix_time_ms = gps.last_fix_time_ms(instance); - if (last_nav_sat_fix_time_ms == last_fix_time_ms) { - return false; - } else { - last_nav_sat_fix_time_ms = last_fix_time_ms; - } - - - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, WGS_84_FRAME_ID); - msg.status.service = 0; // SERVICE_GPS - msg.status.status = -1; // STATUS_NO_FIX - - - //! @todo What about glonass, compass, galileo? - //! This will be properly designed and implemented to spec in #23277 - msg.status.service = 1; // SERVICE_GPS - - const auto status = gps.status(instance); - switch (status) { - case AP_GPS::NO_GPS: - case AP_GPS::NO_FIX: - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - case AP_GPS::GPS_OK_FIX_2D: - case AP_GPS::GPS_OK_FIX_3D: - msg.status.status = 0; // STATUS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_DGPS: - msg.status.status = 1; // STATUS_SBAS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: - case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: - msg.status.status = 2; // STATUS_SBAS_FIX - break; - default: - //! @todo Can we not just use an enum class and not worry about this condition? - break; - } - const auto loc = gps.location(instance); - msg.latitude = loc.lat * 1E-7; - msg.longitude = loc.lng * 1E-7; - - int32_t alt_cm; - if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { - // With absolute frame, this condition is unlikely - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - } - msg.altitude = alt_cm * 0.01; - - // ROS allows double precision, ArduPilot exposes float precision today - Matrix3f cov; - msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); - msg.position_covariance[0] = cov[0][0]; - msg.position_covariance[1] = cov[0][1]; - msg.position_covariance[2] = cov[0][2]; - msg.position_covariance[3] = cov[1][0]; - msg.position_covariance[4] = cov[1][1]; - msg.position_covariance[5] = cov[1][2]; - msg.position_covariance[6] = cov[2][0]; - msg.position_covariance[7] = cov[2][1]; - msg.position_covariance[8] = cov[2][2]; - - return true; + return AP_ROS_Client::update_nav_sat_fix(msg, instance, last_nav_sat_fix_time_ms); } void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) { - msg.transforms_size = 0; - - auto &gps = AP::gps(); - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - const auto gps_type = gps.get_type(i); - if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { - continue; - } - update_topic(msg.transforms[i].header.stamp); - char gps_frame_id[16]; - //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? - hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); - strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID); - strcpy(msg.transforms[i].child_frame_id, gps_frame_id); - // The body-frame offsets - // X - Forward - // Y - Right - // Z - Down - // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation - - const auto offset = gps.get_antenna_offset(i); - - // In ROS REP 103, it follows this convention - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - - msg.transforms[i].transform.translation.x = offset[0]; - msg.transforms[i].transform.translation.y = -1 * offset[1]; - msg.transforms[i].transform.translation.z = -1 * offset[2]; - - msg.transforms_size++; - } - + AP_ROS_Client::update_static_transforms(msg); } void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) { - if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { - return; - } - - update_topic(msg.header.stamp); - auto &battery = AP::battery(); - - if (!battery.healthy(instance)) { - msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD - msg.present = false; - return; - } - msg.present = true; - - msg.voltage = battery.voltage(instance); - - float temperature; - msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; - - float current; - msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; - - const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; - msg.design_capacity = design_capacity; - - uint8_t percentage; - if (battery.capacity_remaining_pct(percentage, instance)) { - msg.percentage = percentage * 0.01; - msg.charge = (percentage * design_capacity) * 0.01; - } else { - msg.percentage = NAN; - msg.charge = NAN; - } - - msg.capacity = NAN; - - if (battery.current_amps(current, instance)) { - if (percentage == 100) { - msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL - } else if (current < 0.0) { - msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING - } else if (current > 0.0) { - msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING - } else { - msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING - } - } else { - msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN - } - - msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD - - msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN - - if (battery.has_cell_voltages(instance)) { - const uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells; - std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, msg.cell_voltage); - } + AP_ROS_Client::update_battery_state(msg, instance); } void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - - Vector3f position; - if (ahrs.get_relative_position_NED_home(position)) { - msg.pose.position.x = position[1]; - msg.pose.position.y = position[0]; - msg.pose.position.z = -position[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_pose_stamped(msg); } void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - Vector3f velocity; - if (ahrs.get_velocity_NED(velocity)) { - msg.twist.linear.x = velocity[1]; - msg.twist.linear.y = velocity[0]; - msg.twist.linear.z = -velocity[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // The gyro data is received from AP_AHRS in body-frame - // X - Forward - // Y - Right - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z - Vector3f angular_velocity = ahrs.get_gyro(); - msg.twist.angular.x = angular_velocity[0]; - msg.twist.angular.y = -angular_velocity[1]; - msg.twist.angular.z = -angular_velocity[2]; + AP_ROS_Client::update_twist_stamped(msg); } void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - Location loc; - if (ahrs.get_location(loc)) { - msg.pose.position.latitude = loc.lat * 1E-7; - msg.pose.position.longitude = loc.lng * 1E-7; - msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_geopose_stamped(msg); } void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { - update_topic(msg.clock); + AP_ROS_Client::update_clock(msg); } /* @@ -839,6 +550,7 @@ void AP_DDS_Client::write_battery_state_topic() } } } + void AP_DDS_Client::write_local_pose_topic() { WITH_SEMAPHORE(csem); diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index a72536d6ffde8..f8fbf7a3e6fc2 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -1,44 +1,13 @@ #if AP_DDS_ENABLED #include "AP_DDS_ExternalControl.h" -#include "AP_DDS_Frames.h" #include - #include +#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, BASE_LINK_FRAME_ID) == 0) { - // Convert commands from body frame (x-forward, y-left, z-up) to NED. - Vector3f linear_velocity; - Vector3f linear_velocity_base_link { - float(cmd_vel.twist.linear.x), - float(cmd_vel.twist.linear.y), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - - auto &ahrs = AP::ahrs(); - linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - - else if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) == 0) { - // 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); - } - - return false; + return AP_ROS_External_Control::handle_velocity_control(cmd_vel); } - #endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_External_Odom.cpp b/libraries/AP_DDS/AP_DDS_External_Odom.cpp index cf121f408521a..9694d2add4f1f 100644 --- a/libraries/AP_DDS/AP_DDS_External_Odom.cpp +++ b/libraries/AP_DDS/AP_DDS_External_Odom.cpp @@ -1,76 +1,23 @@ - - #include "AP_DDS_External_Odom.h" #include "AP_DDS_Type_Conversions.h" #if AP_DDS_VISUALODOM_ENABLED -#include -#include +#include void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& msg) { - auto *visual_odom = AP::visualodom(); - if (visual_odom == nullptr) { - return; - } - - for (size_t i = 0; i < msg.transforms_size; i++) { - const auto& ros_transform_stamped = msg.transforms[i]; - if (!is_odometry_frame(ros_transform_stamped)) { - continue; - } - const uint64_t remote_time_us {AP_DDS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)}; - - Vector3f ap_position; - Quaternion ap_rotation; - - convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); - // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. - // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. - // TODO what if the quaternion is NaN? - ap_rotation.normalize(); - - // No error is available in TF, trust the data as-is - const float posErr {0.0}; - const float angErr {0.0}; - // The odom to base_link transform used is locally consistent per ROS REP-105. - // https://www.ros.org/reps/rep-0105.html#id16 - // Thus, there will not be any resets. - const uint8_t reset_counter {0}; - // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); - const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; - visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); - - } + AP_ROS_External_Odom::handle_external_odom(msg); } bool AP_DDS_External_Odom::is_odometry_frame(const geometry_msgs_msg_TransformStamped& msg) { - char odom_parent[] = "odom"; - char odom_child[] = "base_link"; - // Assume the frame ID's are null terminated. - return (strcmp(msg.header.frame_id, odom_parent) == 0) && - (strcmp(msg.child_frame_id, odom_child) == 0); + return AP_ROS_External_Odom::is_odometry_frame(msg); } void AP_DDS_External_Odom::convert_transform(const geometry_msgs_msg_Transform& ros_transform, Vector3f& translation, Quaternion& rotation) { - // convert from x-forward, y-left, z-up to NED - // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 - translation = { - static_cast(ros_transform.translation.x), - static_cast(-ros_transform.translation.y), - static_cast(-ros_transform.translation.z) - }; - - // In AP, q1 is the quaternion's scalar component. - // In ROS, w is the quaternion's scalar component. - // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion - rotation.q1 = ros_transform.rotation.w; - rotation.q2 = ros_transform.rotation.x; - rotation.q3 = -ros_transform.rotation.y; - rotation.q4 = -ros_transform.rotation.z; + AP_ROS_External_Odom::convert_transform(ros_transform, translation, rotation); } #endif // AP_DDS_VISUALODOM_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp index 2aa4caf543f89..7508acec1389a 100644 --- a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp +++ b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp @@ -1,13 +1,52 @@ #include "AP_DDS_Type_Conversions.h" #if AP_DDS_ENABLED -#include "builtin_interfaces/msg/Time.h" - - uint64_t AP_DDS_Type_Conversions::time_u64_micros(const builtin_interfaces_msg_Time& ros_time) { - return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); + return AP_ROS_TypeConversions::time_u64_micros(ros_time); +} + +// string specialisations +template <> +const char* string_data(const char* str) { + return str; +} + +template <> +char* mutable_string_data(char* str) { + return str; +} + +// transform specialisations +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs_msg_TFMessage& msg) { + return msg.transforms_size; } +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs_msg_TFMessage& msg) { + return msg.transforms_size; +} + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs_msg_TFMessage& msg) { + return msg.transforms; +} + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs_msg_TFMessage& msg) { + return msg.transforms; +} + +// cell_voltage specialisations +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs_msg_BatteryState& msg) { + return msg.cell_voltage; +} #endif // AP_DDS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.h b/libraries/AP_DDS/AP_DDS_Type_Conversions.h index 71b2e3182de29..096821b0c51cf 100644 --- a/libraries/AP_DDS/AP_DDS_Type_Conversions.h +++ b/libraries/AP_DDS/AP_DDS_Type_Conversions.h @@ -4,7 +4,12 @@ #if AP_DDS_ENABLED +#include + #include "builtin_interfaces/msg/Time.h" +#include "geometry_msgs/msg/TransformStamped.h" +#include "sensor_msgs/msg/BatteryState.h" +#include "tf2_msgs/msg/TFMessage.h" class AP_DDS_Type_Conversions { @@ -14,4 +19,58 @@ class AP_DDS_Type_Conversions static uint64_t time_u64_micros(const builtin_interfaces_msg_Time& ros_time); }; +// string specialisations +template <> +const char* string_data(const char* str); + +template <> +char* mutable_string_data(char* str); + +// transform specialisations +template <> +struct transforms_size_type { + typedef uint32_t type; +}; + +template <> +struct mutable_transforms_size_type { + typedef uint32_t& type; +}; + +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs_msg_TFMessage& msg); + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs_msg_TFMessage& msg); + +template <> +struct transforms_type { + typedef const geometry_msgs_msg_TransformStamped* type; +}; + +template <> +struct mutable_transforms_type { + typedef geometry_msgs_msg_TransformStamped* type; +}; + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs_msg_TFMessage& msg); + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs_msg_TFMessage& msg); + +// cell_voltage specialisations +template <> +struct mutable_cell_voltage_type { + typedef float* type; +}; + +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs_msg_BatteryState& msg); + #endif // AP_DDS_ENABLED