From 171e09d28cdf381283b1c56b985f4c69d0682801 Mon Sep 17 00:00:00 2001 From: pedro-fuoco Date: Thu, 13 Apr 2023 22:51:35 -0300 Subject: [PATCH] AP_DDS: Add local pose publisher --- libraries/AP_DDS/AP_DDS_Client.cpp | 71 +++++++++++++++++++++++++++ libraries/AP_DDS/AP_DDS_Client.h | 7 +++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 10 ++++ libraries/AP_DDS/dds_xrce_profile.xml | 28 +++++++++++ 4 files changed, 116 insertions(+) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 15631bd20a..8e7b50a630 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -9,11 +9,13 @@ #include #include #include +#include #include "AP_DDS_Client.h" static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; 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"; @@ -244,6 +246,55 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_ } } +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 (sqrt(2)/2,0,0,sqrt(2)/2); // 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]; + } +} + /* class constructor */ @@ -436,6 +487,20 @@ void AP_DDS_Client::write_battery_state_topic() } } } +void AP_DDS_Client::write_local_pose_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = geometry_msgs_msg_PoseStamped_size_of_topic(&local_pose_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[4].dw_id,&ub,topic_size); + const bool success = geometry_msgs_msg_PoseStamped_serialize_topic(&ub, &local_pose_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} void AP_DDS_Client::update() { @@ -462,6 +527,12 @@ void AP_DDS_Client::update() write_battery_state_topic(); } + if (cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) { + update_topic(local_pose_topic); + last_local_pose_time_ms = cur_time_ms; + write_local_pose_topic(); + } + connected = uxr_run_session_time(&session, 1); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 37985ed20d..c33febcb3c 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -10,6 +10,7 @@ #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" +#include "geometry_msgs/msg/PoseStamped.h" #include #include @@ -46,6 +47,7 @@ private: sensor_msgs_msg_NavSatFix nav_sat_fix_topic; tf2_msgs_msg_TFMessage static_transforms_topic; sensor_msgs_msg_BatteryState battery_state_topic; + geometry_msgs_msg_PoseStamped local_pose_topic; HAL_Semaphore csem; @@ -56,6 +58,7 @@ private: bool update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) WARN_IF_UNUSED; static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg); static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance); + static void update_topic(geometry_msgs_msg_PoseStamped& msg); // The last ms timestamp AP_DDS wrote a Time message uint64_t last_time_time_ms; @@ -63,6 +66,8 @@ private: uint64_t last_nav_sat_fix_time_ms; // The last ms timestamp AP_DDS wrote a BatteryState message uint64_t last_battery_state_time_ms; + // The last ms timestamp AP_DDS wrote a Local Pose message + uint64_t last_local_pose_time_ms; public: @@ -88,6 +93,8 @@ public: void write_static_transforms(); //! @brief Serialize the current nav_sat_fix state and publish it to the IO stream(s) void write_battery_state_topic(); + //! @brief Serialize the current local pose and publish to the IO stream(s) + void write_local_pose_topic(); //! @brief Update the internally stored DDS messages with latest data void update(); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index ec65c110d6..4c9c31e121 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -52,4 +52,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_BatteryState_deserialize_topic), .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_BatteryState_size_of_topic), }, + { + .topic_id = 0x05, + .pub_id = 0x05, + .dw_id = uxrObjectId{.id=0x05, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "localpose__t", + .dw_profile_label = "localpose__dw", + .serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_PoseStamped_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_PoseStamped_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_PoseStamped_size_of_topic), + }, }; diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index c013cc996a..a1dfec0092 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -114,4 +114,32 @@ + + rt/ROS2_LocalPose + geometry_msgs::msg::dds_::PoseStamped_ + + KEEP_LAST + 5 + + + + PREALLOCATED_WITH_REALLOC + + + BEST_EFFORT + + + VOLATILE + + + + NO_KEY + rt/ROS2_LocalPose + geometry_msgs::msg::dds_::PoseStamped_ + + KEEP_LAST + 5 + + +