Merge branch 'main' into feature/eskf-landmark-valve

Author: AntTra

mission/landmark_server/config/landmark_server_config.yaml

@@ -1,8 +1,7 @@
 /**:
   ros__parameters:
-    target_frame: "odom"
+    target_frame: "orca/odom"
     timer_rate_ms: 200
-    enu_ned_rotation: true
 
     track_config:
       default:

mission/landmark_server/include/landmark_server/landmark_server_ros.hpp

@@ -170,8 +170,6 @@ class LandmarkServerNode : public rclcpp::Node {
     std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
     std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
-    bool enu_ned_rotation_{false};
-
     std::shared_ptr<LandmarkPollingGoalHandle> active_landmark_polling_goal_;
     std::shared_ptr<LandmarkConvergenceGoalHandle>
         active_landmark_convergence_goal_;

mission/landmark_server/src/landmark_server_convergence.cpp

@@ -1,6 +1,7 @@
 #include <spdlog/spdlog.h>
 #include <rclcpp_action/client.hpp>
 #include <vortex/utils/ros/ros_conversions.hpp>
+#include <vortex/utils/waypoint_utils.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
 #include <vortex_msgs/msg/waypoint_mode.hpp>
 #include "landmark_server/landmark_server_ros.hpp"
@@ -464,27 +465,13 @@ geometry_msgs::msg::Pose LandmarkServerNode::compute_target_pose(
     const vortex::filtering::Track& track,
     const geometry_msgs::msg::Pose& convergence_offset) {
     const auto landmark_pose = track.to_pose();
-
-    const Eigen::Vector3d p_landmark = landmark_pose.pos_vector();
-    const Eigen::Quaterniond q_landmark =
-        landmark_pose.ori_quaternion().normalized();
-
-    const Eigen::Vector3d p_offset(convergence_offset.position.x,
-                                   convergence_offset.position.y,
-                                   convergence_offset.position.z);
-
-    const Eigen::Quaterniond q_offset =
-        Eigen::Quaterniond(
-            convergence_offset.orientation.w, convergence_offset.orientation.x,
-            convergence_offset.orientation.y, convergence_offset.orientation.z)
-            .normalized();
-
-    const Eigen::Vector3d p_target = p_landmark + p_offset;
-
-    const Eigen::Quaterniond q_target = (q_landmark * q_offset).normalized();
-
-    return vortex::utils::ros_conversions::eigen_to_pose_msg(p_target,
-                                                             q_target);
+    const auto base = vortex::utils::types::Pose::from_eigen(
+        landmark_pose.pos_vector(), landmark_pose.ori_quaternion());
+    const auto offset =
+        vortex::utils::ros_conversions::ros_pose_to_pose(convergence_offset);
+    const auto target =
+        vortex::utils::waypoints::apply_pose_offset(base, offset);
+    return vortex::utils::ros_conversions::to_pose_msg(target);
 }
 
 }  // namespace vortex::mission

mission/landmark_server/src/landmark_server_ros.cpp

@@ -65,7 +65,6 @@ void LandmarkServerNode::create_reference_publisher() {
 void LandmarkServerNode::create_pose_subscription() {
     std::string landmark_topic =
         this->declare_parameter<std::string>("topics.landmarks");
-    enu_ned_rotation_ = this->declare_parameter<bool>("enu_ned_rotation");
     target_frame_ = this->declare_parameter<std::string>("target_frame");
     auto qos_sensor_profile =
         vortex::utils::qos_profiles::sensor_data_profile(1);
@@ -98,12 +97,6 @@ void LandmarkServerNode::create_pose_subscription() {
             }
 
             auto new_measurements = ros_msg_to_landmarks(pose_tf);
-            if (enu_ned_rotation_) {
-                std::ranges::for_each(new_measurements, [](auto& m) {
-                    m.pose.set_ori(vortex::utils::math::enu_ned_rotation(
-                        m.pose.ori_quaternion()));
-                });
-            }
             {
                 std::lock_guard<std::mutex> lock(measurements_mtx_);
                 measurements_ = std::move(new_measurements);
@@ -273,6 +266,15 @@ void LandmarkServerNode::timer_callback() {
 
     convergence_update();
 
+    if (active_landmark_polling_goal_ &&
+        active_landmark_polling_goal_->is_canceling()) {
+        auto result =
+            std::make_shared<vortex_msgs::action::LandmarkPolling_Result>();
+        active_landmark_polling_goal_->canceled(result);
+        active_landmark_polling_goal_ = nullptr;
+        return;
+    }
+
     if (active_landmark_polling_goal_ &&
         active_landmark_polling_goal_->is_active()) {
         const auto goal = active_landmark_polling_goal_->get_goal();
@@ -294,6 +296,9 @@ void LandmarkServerNode::timer_callback() {
         if (!found) {
             return;
         }
+        spdlog::info(
+            "LandmarkPolling: found landmark(s) for type={}, subtype={}", type,
+            subtype);
         vortex_msgs::msg::LandmarkArray landmarks =
             tracks_to_landmark_msgs(type, subtype);
         auto polling_result =