diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 8e7b50a630..7833c7b3b6 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -16,6 +16,7 @@ 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 constexpr uint16_t DELAY_LOCAL_VELOCITY_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"; @@ -295,6 +296,49 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& 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]; +} + /* class constructor */ @@ -502,6 +546,21 @@ void AP_DDS_Client::write_local_pose_topic() } } +void AP_DDS_Client::write_local_velocity_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = geometry_msgs_msg_TwistStamped_size_of_topic(&local_velocity_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[5].dw_id,&ub,topic_size); + const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &local_velocity_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() { WITH_SEMAPHORE(csem); @@ -533,6 +592,12 @@ void AP_DDS_Client::update() write_local_pose_topic(); } + if (cur_time_ms - last_local_velocity_time_ms > DELAY_LOCAL_VELOCITY_TOPIC_MS) { + update_topic(local_velocity_topic); + last_local_velocity_time_ms = cur_time_ms; + write_local_velocity_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 c33febcb3c..459cc73ed5 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -11,6 +11,7 @@ #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" #include "geometry_msgs/msg/PoseStamped.h" +#include "geometry_msgs/msg/TwistStamped.h" #include #include @@ -48,6 +49,7 @@ private: tf2_msgs_msg_TFMessage static_transforms_topic; sensor_msgs_msg_BatteryState battery_state_topic; geometry_msgs_msg_PoseStamped local_pose_topic; + geometry_msgs_msg_TwistStamped local_velocity_topic; HAL_Semaphore csem; @@ -59,6 +61,7 @@ private: 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); + static void update_topic(geometry_msgs_msg_TwistStamped& msg); // The last ms timestamp AP_DDS wrote a Time message uint64_t last_time_time_ms; @@ -68,6 +71,8 @@ private: 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; + // The last ms timestamp AP_DDS wrote a Local Velocity message + uint64_t last_local_velocity_time_ms; public: @@ -95,6 +100,8 @@ public: void write_battery_state_topic(); //! @brief Serialize the current local pose and publish to the IO stream(s) void write_local_pose_topic(); + //! @brief Serialize the current local velocity and publish to the IO stream(s) + void write_local_velocity_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 4c9c31e121..5922891c33 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -62,4 +62,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .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), }, + { + .topic_id = 0x06, + .pub_id = 0x06, + .dw_id = uxrObjectId{.id=0x06, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "localvelocity__t", + .dw_profile_label = "localvelocity__dw", + .serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_TwistStamped_size_of_topic), + } }; diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index a1dfec0092..d1e99c2ee6 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -142,4 +142,32 @@ + + rt/ROS2_LocalVelocity + geometry_msgs::msg::dds_::TwistStamped_ + + KEEP_LAST + 5 + + + + PREALLOCATED_WITH_REALLOC + + + BEST_EFFORT + + + VOLATILE + + + + NO_KEY + rt/ROS2_LocalVelocity + geometry_msgs::msg::dds_::TwistStamped_ + + KEEP_LAST + 5 + + +