AP_DDS: Topic to publish current goal

Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py

@@ -33,6 +33,7 @@
 from geopy import point
 from ardupilot_msgs.srv import ArmMotors
 from ardupilot_msgs.srv import ModeSwitch
+from geographic_msgs.msg import GeoPointStamped
 
 
 PLANE_MODE_TAKEOFF = 13
@@ -78,6 +79,15 @@ def __init__(self):
 
         self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
         self._cur_geopose = GeoPoseStamped()
+
+        self.declare_parameter("goal_topic", "/ap/goal_lla")
+        self._goal_topic = self.get_parameter("goal_topic").get_parameter_value().string_value
+        qos = rclpy.qos.QoSProfile(
+            reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth=1
+        )
+
+        self._subscription_goal = self.create_subscription(GeoPointStamped, self._goal_topic, self.goal_cb, qos)
+        self._cur_goal = GeoPointStamped()        
 
     def geopose_cb(self, msg: GeoPoseStamped):
         """Process a GeoPose message."""
@@ -87,6 +97,15 @@ def geopose_cb(self, msg: GeoPoseStamped):
 
             # Store current state
             self._cur_geopose = msg
+
+    def goal_cb(self, msg: GeoPointStamped):
+        """Process a Goal message."""
+        stamp = msg.header.stamp
+        self.get_logger().info("From AP : Goal [sec:{}, nsec: {}, lat:{} lon:{}]"
+                               .format(stamp.sec, stamp.nanosec,msg.position.latitude, msg.position.longitude))
+
+        # Store current state
+        self._cur_goal = msg
 
     def arm(self):
         req = ArmMotors.Request()
@@ -127,6 +146,10 @@ def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Du
     def get_cur_geopose(self):
         """Return latest geopose."""
         return self._cur_geopose
+
+    def get_cur_goal(self):
+        """Return latest goal."""
+        return self._cur_goal 
 
     def send_goal_position(self, goal_global_pos):
         """Send goal position. Must be in guided for this to work."""
@@ -148,6 +171,15 @@ def achieved_goal(goal_global_pos, cur_geopose):
     print(f"Goal is {euclidian_distance} meters away")
     return euclidian_distance < 150
 
+def going_to_goal(goal_global_pos, cur_goal):
+    p1 = (goal_global_pos.latitude, goal_global_pos.longitude, goal_global_pos.altitude)
+    cur_pos_lla = cur_goal.position
+    p2 = (cur_pos_lla.latitude, cur_pos_lla.longitude, cur_pos_lla.altitude)
+
+    flat_distance = distance.distance(p1[:2], p2[:2]).m
+    euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2)
+    print(f"Commanded and received goal are {euclidian_distance} meters away")
+    return euclidian_distance < 1    
 
 def main(args=None):
     """Node entry point."""
@@ -191,11 +223,15 @@ def main(args=None):
 
         start = node.get_clock().now()
         has_achieved_goal = False
+        is_going_to_goal = False
         while not has_achieved_goal and node.get_clock().now() - start < rclpy.duration.Duration(seconds=120):
             rclpy.spin_once(node)
+            is_going_to_goal = going_to_goal(goal_pos, node.get_cur_goal())
             has_achieved_goal = achieved_goal(goal_pos, node.get_cur_geopose())
             time.sleep(1.0)
 
+        if not is_going_to_goal:
+            raise RuntimeError("Unable to go to goal location")
         if not has_achieved_goal:
             raise RuntimeError("Unable to achieve goal location")
 

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -60,6 +60,9 @@ static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_
 #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
+#if AP_DDS_GOAL_PUB_ENABLED
+static constexpr uint16_t DELAY_GOAL_TOPIC_MS = 200;
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
 static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
 #endif // AP_DDS_CLOCK_PUB_ENABLED
@@ -552,6 +555,39 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 }
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+bool AP_DDS_Client::update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg)
+{
+    const auto &vehicle = AP::vehicle();
+    update_topic(msg.header.stamp);
+    Location target_loc;
+    // Exit if no target is available.
+    if (!vehicle->get_target_location(target_loc)) {
+        return false;
+    }
+    target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
+    msg.position.latitude = target_loc.lat * 1e-7;
+    msg.position.longitude = target_loc.lng * 1e-7;
+    msg.position.altitude = target_loc.alt * 1e-2;
+
+    // Check whether the goal has changed or if the topic has never been published.
+    const double tolerance_lat_lon = 1e-8; // One order of magnitude smaller than the target's resolution.
+    const double distance_alt = 1e-3;
+    if (abs(msg.position.latitude - prev_goal_msg.position.latitude) >  tolerance_lat_lon ||
+        abs(msg.position.longitude - prev_goal_msg.position.longitude) >  tolerance_lat_lon ||
+        abs(msg.position.altitude - prev_goal_msg.position.altitude) > distance_alt ||
+        prev_goal_msg.header.stamp.sec == 0 ) {
+        update_topic(prev_goal_msg.header.stamp);
+        prev_goal_msg.position.latitude = msg.position.latitude;
+        prev_goal_msg.position.longitude = msg.position.longitude;
+        prev_goal_msg.position.altitude = msg.position.altitude;
+        return true;
+    } else {
+        return false;
+    }
+}
+#endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_IMU_PUB_ENABLED
 void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
 {
@@ -1516,6 +1552,22 @@ void AP_DDS_Client::write_gps_global_origin_topic()
 }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+void AP_DDS_Client::write_goal_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub {};
+        const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&goal_topic, 0);
+        uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size);
+        const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic);
+        if (!success) {
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+#endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_STATUS_PUB_ENABLED
 void AP_DDS_Client::write_status_topic()
 {
@@ -1612,6 +1664,14 @@ void AP_DDS_Client::update()
         write_gps_global_origin_topic();
     }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+    if (cur_time_ms - last_goal_time_ms > DELAY_GOAL_TOPIC_MS) {
+        if (update_topic_goal(goal_topic)) {
+            write_goal_topic();
+        }
+        last_goal_time_ms = cur_time_ms;
+    }
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_STATUS_PUB_ENABLED
     if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) {
         if (update_topic(status_topic)) {

libraries/AP_DDS/AP_DDS_Client.h

@@ -114,6 +114,16 @@ class AP_DDS_Client
     static void update_topic(geographic_msgs_msg_GeoPointStamped& msg);
 # endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+    geographic_msgs_msg_GeoPointStamped goal_topic;
+    // The last ms timestamp AP_DDS wrote a goal message
+    uint64_t last_goal_time_ms;
+    //! @brief Serialize the current goal and publish to the IO stream(s)
+    void write_goal_topic();
+    bool update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg);
+    geographic_msgs_msg_GeoPointStamped prev_goal_msg;
+# endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
     // The last ms timestamp AP_DDS wrote a GeoPose message

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -42,6 +42,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     GEOPOSE_PUB,
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#ifdef AP_DDS_GOAL_PUB_ENABLED
+    GOAL_PUB,
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
     CLOCK_PUB,
 #endif // AP_DDS_CLOCK_PUB_ENABLED
@@ -235,6 +238,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+    {
+        .topic_id = to_underlying(TopicIndex::GOAL_PUB),
+        .pub_id = to_underlying(TopicIndex::GOAL_PUB),
+        .sub_id = to_underlying(TopicIndex::GOAL_PUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAREADER_ID},
+        .topic_rw = Topic_rw::DataWriter,
+        .topic_name = "rt/ap/goal_lla",
+        .type_name = "geographic_msgs::msg::dds_::GeoPointStamped_",
+        .qos = {
+            .durability = UXR_DURABILITY_TRANSIENT_LOCAL,
+            .reliability = UXR_RELIABILITY_RELIABLE,
+            .history = UXR_HISTORY_KEEP_LAST,
+            .depth = 1,
+        },
+    },
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
     {
         .topic_id = to_underlying(TopicIndex::CLOCK_PUB),

libraries/AP_DDS/AP_DDS_config.h

@@ -106,6 +106,10 @@
 #define AP_DDS_DELAY_CLOCK_TOPIC_MS 10
 #endif
 
+#ifndef AP_DDS_GOAL_PUB_ENABLED
+#define AP_DDS_GOAL_PUB_ENABLED 1
+#endif
+
 #ifndef AP_DDS_STATUS_PUB_ENABLED
 #define AP_DDS_STATUS_PUB_ENABLED 1
 #endif