diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index 31ed1cd4115ca..89495b27b709f 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" "msg/Rc.msg" + "msg/GroundTruth.msg" "msg/Status.msg" "msg/Airspeed.msg" "srv/ArmMotors.srv" diff --git a/Tools/ros2/ardupilot_msgs/msg/GroundTruth.msg b/Tools/ros2/ardupilot_msgs/msg/GroundTruth.msg new file mode 100644 index 0000000000000..6ed733c79181a --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/GroundTruth.msg @@ -0,0 +1,20 @@ +std_msgs/Header header + +# Geographical position +float64 latitude +float64 longitude +float64 altitude + + +# Local ENU position (meters) +geometry_msgs/Vector3 position + + +# Orientation (ENU) +geometry_msgs/Quaternion orientation + +# Linear velocity (ENU, m/s) +geometry_msgs/Vector3 linear_velocity + +# Angular velocity ( body frame, rad/s) +geometry_msgs/Vector3 angular_velocity diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index c695728773ff1..92b6a7fb40175 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -73,6 +73,9 @@ static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_ #if AP_DDS_RC_PUB_ENABLED static constexpr uint16_t DELAY_RC_TOPIC_MS = AP_DDS_DELAY_RC_TOPIC_MS; #endif // AP_DDS_RC_PUB_ENABLED +#ifndef AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS +static constexpr uint16_t DELAY_RC_TOPIC_MS = AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS; +#endif #if AP_DDS_GEOPOSE_PUB_ENABLED static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS; #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -565,6 +568,105 @@ bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Rc& msg) } #endif // AP_DDS_RC_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED +void AP_DDS_Client::update_topic(ardupilot_msgs_msg_GroundTruth& msg) +{ +#if AP_SIM_ENABLED + const SITL::SIM *sitl = AP::sitl(); + if (!sitl) { + return; + } + const auto &s = sitl->state; + + update_topic(msg.header.stamp); + STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID); + + // Geographic position + msg.latitude = s.latitude; + msg.longitude = s.longitude; + msg.altitude = s.altitude; + + // Cartesian position in ENU frame relative to home + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // SITL state 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 + + // Location stores lat/lon in 1E7 degrees, altitude in centimeters, conversion on the go + Location current_loc(s.latitude * 1e7,s.longitude * 1e7,s.altitude * 100,Location::AltFrame::ABSOLUTE); + // get_distance_NED returns vector FROM home TO current location + Vector3f pos_ned = s.home.get_distance_NED(current_loc); + + // Cartesion position in ENU frame + msg.position.x = pos_ned.y; // East + msg.position.y = pos_ned.x; // North + msg.position.z = -pos_ned.z; // Up + + // Orientation quaternion + // 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 + // SITL provides orientation in NED body frame (FRD): + // X - Forward + // Y - Right + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU conversion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation(s.quaternion.q1, s.quaternion.q2, s.quaternion.q3, s.quaternion.q4); + 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.orientation.w = orientation[0]; + msg.orientation.x = orientation[1]; + msg.orientation.y = orientation[2]; + msg.orientation.z = orientation[3]; + + // Linear velocity in ENU frame + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // SITL state uses the NED convention: + // X - North (speedN) + // Y - East (speedE) + // Z - Down (speedD) + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + msg.linear_velocity.x = s.speedE; // East + msg.linear_velocity.y = s.speedN; // North + msg.linear_velocity.z = -s.speedD; // Up + + // Angular velocity in body frame + // 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 angular rate data is received from SITL in body-frame (FRD): + // X - Forward (rollRate) + // Y - Right (pitchRate) + // Z - Down (yawRate) + // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z + // Also convert from degrees/s to radians/s + msg.angular_velocity.x = radians(s.rollRate); // Forward + msg.angular_velocity.y = -radians(s.pitchRate); // Left (inverted from Right) + msg.angular_velocity.z = -radians(s.yawRate); // Up (inverted from Down) + +#endif // AP_SIM_ENABLED +} +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED + #if AP_DDS_GEOPOSE_PUB_ENABLED void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) { @@ -1669,6 +1771,22 @@ void AP_DDS_Client::write_tx_local_rc_topic() } } #endif // AP_DDS_RC_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED +void AP_DDS_Client::write_groundtruth_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub{}; + const uint32_t topic_size = ardupilot_msgs_msg_GroundTruth_size_of_topic(&groundtruth_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[to_underlying(TopicIndex::GROUNDTRUTH_PUB)].dw_id,&ub,topic_size); + const bool success = ardupilot_msgs_msg_GroundTruth_serialize_topic(&ub, &groundtruth_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::write_imu_topic() { @@ -1836,6 +1954,13 @@ void AP_DDS_Client::update() write_imu_topic(); } #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED + if (cur_time_ms - last_groundtruth_time_ms > AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS) { + update_topic(groundtruth_topic); + last_groundtruth_time_ms = cur_time_ms; + write_groundtruth_topic(); + } +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { update_topic(geo_pose_topic); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index df02a262b64ae..6893ec0f3c5da 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -47,6 +47,9 @@ #if AP_DDS_RC_PUB_ENABLED #include "ardupilot_msgs/msg/Rc.h" #endif // AP_DDS_RC_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED +#include "ardupilot_msgs/msg/GroundTruth.h" +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED #include "geographic_msgs/msg/GeoPoseStamped.h" #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -173,6 +176,15 @@ class AP_DDS_Client static bool update_topic(ardupilot_msgs_msg_Rc& msg); #endif //AP_DDS_RC_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED + ardupilot_msgs_msg_GroundTruth groundtruth_topic; + // The last ms timestamp AP_DDS wrote a groundtruth message + uint64_t last_groundtruth_time_ms; + //! @brief Serialize the current ground truth of sim vehicle and publish to the IO stream(s) + void write_groundtruth_topic(); + static void update_topic(ardupilot_msgs_msg_GroundTruth& msg); +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED + #if AP_DDS_BATTERY_STATE_PUB_ENABLED sensor_msgs_msg_BatteryState battery_state_topic; // The last ms timestamp AP_DDS wrote a BatteryState message diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 5c5fc8ee21ed9..15209da321a23 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -42,6 +42,9 @@ enum class TopicIndex: uint8_t { #if AP_DDS_RC_PUB_ENABLED LOCAL_RC_PUB, #endif // AP_DDS_RC_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED + GROUNDTRUTH_PUB, +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED GEOPOSE_PUB, #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -241,6 +244,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif // AP_DDS_RC_PUB_ENABLED +#if AP_DDS_GROUNDTRUTH_PUB_ENABLED + { + .topic_id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), + .pub_id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), + .sub_id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), + .dw_id = uxrObjectId{.id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), .type = UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), .type = UXR_DATAREADER_ID}, + .topic_rw = Topic_rw::DataWriter, + .topic_name = "rt/ap/sim/groundtruth", + .type_name = "ardupilot_msgs::msg::dds_::GroundTruth_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }, + }, +#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLEDs #if AP_DDS_GEOPOSE_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 0a49a4a281229..bf2b30be7810c 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -98,6 +98,14 @@ #define AP_DDS_DELAY_RC_TOPIC_MS 100 #endif +#ifndef AP_DDS_GROUNDTRUTH_PUB_ENABLED +#define AP_DDS_GROUNDTRUTH_PUB_ENABLED AP_SIM_ENABLED +#endif + +#ifndef AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS +#define AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS 20 +#endif + #ifndef AP_DDS_BATTERY_STATE_PUB_ENABLED #define AP_DDS_BATTERY_STATE_PUB_ENABLED 1 #endif diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GroundTruth.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GroundTruth.idl new file mode 100644 index 0000000000000..1c4b7884abf5e --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GroundTruth.idl @@ -0,0 +1,39 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/GroundTruth.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 ardupilot_msgs { + module msg { + struct GroundTruth { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + "Geographical position") + double latitude; + + double longitude; + + double altitude; + + @verbatim (language="comment", text= + "Local ENU position (meters)") + geometry_msgs::msg::Vector3 position; + + @verbatim (language="comment", text= + "Orientation (ENU)") + geometry_msgs::msg::Quaternion orientation; + + @verbatim (language="comment", text= + "Linear velocity (ENU, m/s)") + geometry_msgs::msg::Vector3 linear_velocity; + + @verbatim (language="comment", text= + "Angular velocity ( body frame, rad/s)") + geometry_msgs::msg::Vector3 angular_velocity; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index e2288f75a4721..d073b7a699ad2 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -173,6 +173,7 @@ Published topics: * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher * /ap/rc [ardupilot_msgs/msg/Rc] 1 publisher * /ap/status [ardupilot_msgs/msg/Status] 1 publisher + * /ap/sim/groundtruth [ardupilot_msgs/msg/GroundTruth] 1 publisher * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher * /ap/time [builtin_interfaces/msg/Time] 1 publisher * /ap/twist/filtered [geometry_msgs/msg/TwistStamped] 1 publisher