diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 39618cb095..bb7dbbe628 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 constexpr uint16_t DELAY_GEO_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"; @@ -338,6 +339,41 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) msg.twist.angular.z = -angular_velocity[2]; } +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 / 100.0; // 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(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]; + } +} + /* start the DDS thread */ @@ -576,6 +612,21 @@ void AP_DDS_Client::write_local_velocity_topic() } } +void AP_DDS_Client::write_geo_pose_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = geographic_msgs_msg_GeoPoseStamped_size_of_topic(&geo_pose_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[6].dw_id,&ub,topic_size); + const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_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() { WITH_SEMAPHORE(csem); @@ -592,8 +643,6 @@ void AP_DDS_Client::update() write_nav_sat_fix_topic(); } - - if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { constexpr uint8_t battery_instance = 0; update_topic(battery_state_topic, battery_instance); @@ -613,6 +662,12 @@ void AP_DDS_Client::update() write_local_velocity_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; + write_geo_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 ecce3df1d2..96eb0ef92c 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -12,6 +12,7 @@ #include "sensor_msgs/msg/BatteryState.h" #include "geometry_msgs/msg/PoseStamped.h" #include "geometry_msgs/msg/TwistStamped.h" +#include "geographic_msgs/msg/GeoPoseStamped.h" #include #include @@ -58,6 +59,7 @@ private: sensor_msgs_msg_BatteryState battery_state_topic; geometry_msgs_msg_PoseStamped local_pose_topic; geometry_msgs_msg_TwistStamped local_velocity_topic; + geographic_msgs_msg_GeoPoseStamped geo_pose_topic; HAL_Semaphore csem; @@ -70,6 +72,7 @@ private: 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); + static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg); // The last ms timestamp AP_DDS wrote a Time message uint64_t last_time_time_ms; @@ -81,6 +84,8 @@ private: 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; + // The last ms timestamp AP_DDS wrote a GeoPose message + uint64_t last_geo_pose_time_ms; // functions for serial transport bool ddsSerialInit(); @@ -135,10 +140,12 @@ 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) + //! @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 Serialize the current geo_pose and publish to the IO stream(s) + void write_geo_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 5922891c33..084363b494 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -2,6 +2,7 @@ #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" +#include "geographic_msgs/msg/GeoPoseStamped.h" #include "AP_DDS_Generic_Fn_T.h" #include "uxr/client/client.h" @@ -71,5 +72,15 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .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), - } + }, + { + .topic_id = 0x07, + .pub_id = 0x07, + .dw_id = uxrObjectId{.id=0x07, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "geopose__t", + .dw_profile_label = "geopose__dw", + .serialize = Generic_serialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_size_of_topic), + }, }; diff --git a/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoint.idl b/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoint.idl new file mode 100644 index 0000000000..e5dbbdf65b --- /dev/null +++ b/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoint.idl @@ -0,0 +1,30 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from geographic_msgs/msg/GeoPoint.msg +// generated code does not contain a copyright notice + + +module geographic_msgs { + module msg { + @verbatim (language="comment", text= + "Geographic point, using the WGS 84 reference ellipsoid.") + struct GeoPoint { + @verbatim (language="comment", text= + "Latitude. Positive is north of equator; negative is south" "\n" + "(-90 <= latitude <= +90).") + @unit (value="degrees") + double latitude; + + @verbatim (language="comment", text= + "Longitude. Positive is east of prime meridian; negative is" "\n" + "west (-180 <= longitude <= +180). At the poles, latitude is -90 or" "\n" + "+90, and longitude is irrelevant, but must be in range.") + @unit (value="degrees") + double longitude; + + @verbatim (language="comment", text= + "Altitude. Positive is above the WGS 84 ellipsoid (NaN if unspecified).") + @unit (value="m") + double altitude; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPose.idl b/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPose.idl new file mode 100644 index 0000000000..169dd375eb --- /dev/null +++ b/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPose.idl @@ -0,0 +1,21 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from geographic_msgs/msg/GeoPose.msg +// generated code does not contain a copyright notice + +#include "geometry_msgs/msg/Quaternion.idl" +#include "geographic_msgs/msg/GeoPoint.idl" + +module geographic_msgs { + module msg { + @verbatim (language="comment", text= + "Geographic pose, using the WGS 84 reference ellipsoid." "\n" + "" "\n" + "Orientation uses the East-North-Up (ENU) frame of reference." "\n" + "(But, what about singularities at the poles?)") + struct GeoPose { + geographic_msgs::msg::GeoPoint position; + + geometry_msgs::msg::Quaternion orientation; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoseStamped.idl b/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoseStamped.idl new file mode 100644 index 0000000000..526458f097 --- /dev/null +++ b/libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoseStamped.idl @@ -0,0 +1,16 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from geographic_msgs/msg/GeoPoseStamped.msg +// generated code does not contain a copyright notice + +#include "geographic_msgs/msg/GeoPose.idl" +#include "std_msgs/msg/Header.idl" + +module geographic_msgs { + module msg { + struct GeoPoseStamped { + std_msgs::msg::Header header; + + geographic_msgs::msg::GeoPose pose; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 3752760914..a9bbbe9061 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -105,6 +105,11 @@ Follow the steps to use the microROS Agent - https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html +- Install geographic_msgs + ```bash + sudo apt install ros-humble-geographic-msgs + ``` + - Install and run the microROS agent (as descibed here). Make sure to use the `humble` branch. - Follow [the instructions](https://micro.ros.org/docs/tutorials/core/first_application_linux/) for the following: diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 58a38ecab1..7f9747992a 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -170,4 +170,32 @@ + + rt/ap/geopose/filtered + geographic_msgs::msg::dds_::GeoPoseStamped_ + + KEEP_LAST + 5 + + + + PREALLOCATED_WITH_REALLOC + + + BEST_EFFORT + + + VOLATILE + + + + NO_KEY + rt/ap/geopose/filtered + geographic_msgs::msg::dds_::GeoPoseStamped_ + + KEEP_LAST + 5 + + +