From 24de88f85cf248eeec868037b5605c41c6788481 Mon Sep 17 00:00:00 2001 From: astik <41.astiksrivastava@gmail.com> Date: Tue, 20 Feb 2024 23:28:48 +0530 Subject: [PATCH] AP_DDS: Add IMU publisher * Using NED frame --- libraries/AP_DDS/AP_DDS_Client.cpp | 60 +++++++++++++++++++- libraries/AP_DDS/AP_DDS_Client.h | 7 +++ libraries/AP_DDS/AP_DDS_Frames.h | 1 + libraries/AP_DDS/AP_DDS_Topic_Table.h | 12 ++++ libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl | 48 ++++++++++++++++ libraries/AP_DDS/dds_xrce_profile.xml | 23 ++++++++ 6 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index b743ed8aa0..871ef0dfaa 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -1,13 +1,13 @@ #include #if AP_DDS_ENABLED - #include #include #include #include #include +#include #include #include #include @@ -32,6 +32,7 @@ static constexpr uint8_t ENABLED_BY_DEFAULT = 1; static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +static constexpr uint16_t DELAY_IMU_TOPIC_MS = 5; 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; @@ -417,6 +418,42 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) } } +void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) +{ + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID); + + auto &imu = AP::ins(); + auto &ahrs = AP::ahrs(); + + WITH_SEMAPHORE(ahrs.get_semaphore()); + + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + msg.orientation.x = orientation[0]; + msg.orientation.y = orientation[1]; + msg.orientation.z = orientation[2]; + msg.orientation.w = orientation[3]; + } + msg.orientation_covariance[0] = -1; + + uint8_t accel_index = ahrs.get_primary_accel_index(); + uint8_t gyro_index = ahrs.get_primary_gyro_index(); + const Vector3f accel_data = imu.get_accel(accel_index); + const Vector3f gyro_data = imu.get_gyro(gyro_index); + + // Populate the message fields + msg.linear_acceleration.x = accel_data.x; + msg.linear_acceleration.y = accel_data.y; + msg.linear_acceleration.z = accel_data.z; + + msg.angular_velocity.x = gyro_data.x; + msg.angular_velocity.y = gyro_data.y; + msg.angular_velocity.z = gyro_data.z; + msg.angular_velocity_covariance[0] = -1; + msg.linear_acceleration_covariance[0] = -1; +} + void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { update_topic(msg.clock); @@ -958,6 +995,21 @@ void AP_DDS_Client::write_tx_local_velocity_topic() } } +void AP_DDS_Client::write_imu_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub {}; + const uint32_t topic_size = sensor_msgs_msg_Imu_size_of_topic(&imu_topic, 0); + uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::IMU_PUB)].dw_id, &ub, topic_size); + const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_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::write_geo_pose_topic() { WITH_SEMAPHORE(csem); @@ -1023,6 +1075,12 @@ void AP_DDS_Client::update() write_tx_local_velocity_topic(); } + if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) { + update_topic(imu_topic); + last_imu_time_ms = cur_time_ms; + write_imu_topic(); + } + if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { update_topic(geo_pose_topic); last_geo_pose_time_ms = cur_time_ms; diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 3751a697e8..65ce8fd512 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -11,6 +11,7 @@ #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" +#include "sensor_msgs/msg/Imu.h" #include "sensor_msgs/msg/Joy.h" #include "geometry_msgs/msg/PoseStamped.h" #include "geometry_msgs/msg/TwistStamped.h" @@ -62,6 +63,7 @@ private: geometry_msgs_msg_TwistStamped tx_local_velocity_topic; sensor_msgs_msg_BatteryState battery_state_topic; sensor_msgs_msg_NavSatFix nav_sat_fix_topic; + sensor_msgs_msg_Imu imu_topic; rosgraph_msgs_msg_Clock clock_topic; // incoming joystick data static sensor_msgs_msg_Joy rx_joy_topic; @@ -88,6 +90,7 @@ private: static void update_topic(geometry_msgs_msg_PoseStamped& msg); static void update_topic(geometry_msgs_msg_TwistStamped& msg); static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg); + static void update_topic(sensor_msgs_msg_Imu& msg); static void update_topic(rosgraph_msgs_msg_Clock& msg); // subscription callback function @@ -112,6 +115,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 an IMU message + uint64_t last_imu_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 @@ -188,6 +193,8 @@ public: void write_tx_local_velocity_topic(); //! @brief Serialize the current geo_pose and publish to the IO stream(s) void write_geo_pose_topic(); + //! @brief Serialize the current IMU data and publish to the IO stream(s) + void write_imu_topic(); //! @brief Serialize the current clock and publish to the IO stream(s) void write_clock_topic(); //! @brief Update the internally stored DDS messages with latest data diff --git a/libraries/AP_DDS/AP_DDS_Frames.h b/libraries/AP_DDS/AP_DDS_Frames.h index 0bdaf354ec..3b6898f62d 100644 --- a/libraries/AP_DDS/AP_DDS_Frames.h +++ b/libraries/AP_DDS/AP_DDS_Frames.h @@ -3,5 +3,6 @@ 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"; +static constexpr char BASE_LINK_NED_FRAME_ID[] = "base_link_ned"; // https://www.ros.org/reps/rep-0105.html#map static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 8df056d484..96dfd75fe9 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -3,6 +3,7 @@ #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" #include "geographic_msgs/msg/GeoPoseStamped.h" +#include "sensor_msgs/msg/Imu.h" #include "uxr/client/client.h" @@ -15,6 +16,7 @@ enum class TopicIndex: uint8_t { NAV_SAT_FIX_PUB, STATIC_TRANSFORMS_PUB, BATTERY_STATE_PUB, + IMU_PUB, LOCAL_POSE_PUB, LOCAL_VELOCITY_PUB, GEOPOSE_PUB, @@ -73,6 +75,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_profile_label = "batterystate0__dw", .dr_profile_label = "", }, + { + .topic_id = to_underlying(TopicIndex::IMU_PUB), + .pub_id = to_underlying(TopicIndex::IMU_PUB), + .sub_id = to_underlying(TopicIndex::IMU_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAREADER_ID}, + .topic_profile_label = "imu__t", + .dw_profile_label = "imu__dw", + .dr_profile_label = "", + }, { .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), diff --git a/libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl b/libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl new file mode 100644 index 0000000000..2770316110 --- /dev/null +++ b/libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl @@ -0,0 +1,48 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from sensor_msgs/msg/Imu.msg +// generated code does not contain a copyright notice + +#include "geometry_msgs/msg/Quaternion.idl" +#include "geometry_msgs/msg/Vector3.idl" +#include "std_msgs/msg/Header.idl" + +module sensor_msgs { + module msg { + typedef double double__9[9]; + @verbatim (language="comment", text= + "This is a message to hold data from an IMU (Inertial Measurement Unit)" "\n" + "" "\n" + "Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec" "\n" + "" "\n" + "If the covariance of the measurement is known, it should be filled in (if all you know is the" "\n" + "variance of each measurement, e.g. from the datasheet, just put those along the diagonal)" "\n" + "A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the" "\n" + "data a covariance will have to be assumed or gotten from some other source" "\n" + "" "\n" + "If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an" "\n" + "orientation estimate), please set element 0 of the associated covariance matrix to -1" "\n" + "If you are interpreting this message, please check for a value of -1 in the first element of each" "\n" + "covariance matrix, and disregard the associated estimate.") + struct Imu { + std_msgs::msg::Header header; + + geometry_msgs::msg::Quaternion orientation; + + @verbatim (language="comment", text= + "Row major about x, y, z axes") + double__9 orientation_covariance; + + geometry_msgs::msg::Vector3 angular_velocity; + + @verbatim (language="comment", text= + "Row major about x, y, z axes") + double__9 angular_velocity_covariance; + + geometry_msgs::msg::Vector3 linear_acceleration; + + @verbatim (language="comment", text= + "Row major x, y z") + double__9 linear_acceleration_covariance; + }; + }; +}; diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index b8e80c6e49..7d4e4c2001 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -114,6 +114,29 @@ + + rt/ap/imu/experimental/data + sensor_msgs::msg::dds_::Imu_ + + KEEP_LAST + 20 + + + + + NO_KEY + rt/ap/imu/experimental/data + sensor_msgs::msg::dds_::Imu_ + + + + RELIABLE + + + TRANSIENT_LOCAL + + + rt/ap/pose/filtered geometry_msgs::msg::dds_::PoseStamped_