Feat/triangulation_weight_id

bearing_localization/config/aruco.yaml

@@ -1,7 +1,7 @@
 window_size: 15
 max_measurement_age_sec: 2.0
 min_measurements: 4
-min_baseline_m: 0.1
-min_ray_angle_deg: 1.0
+min_baseline_m: 0.3
+min_ray_angle_deg: 3.0
 outlier_residual_threshold_m: 0.1
 max_outlier_iterations: 2

bearing_localization/config/bearing_localization_config.yaml

@@ -3,8 +3,6 @@
     target_frame: orca/odom # TF frame to transform measurements into
     publish_debug_markers: true # Publish Foxglove markers for debugging
     landmark_type: 3 # Landmark type ID sent in output message
-    landmark_subtype: 2 # Landmark subtype ID sent in output message
     topics:
-      bearing_measurement: bearing_measurement # Single bearing input
-      bearing_array: /aruco_detector/marker_directions # Array bearing input
+      bearing_measurements: bearing_measurements # BearingMeasurementArray input
       bearing_localization_markers: bearing_localization/markers # Debug markers

bearing_localization/config/pinger.yaml

@@ -0,0 +1,7 @@
+window_size: 15
+max_measurement_age_sec: 30.0
+min_measurements: 5
+min_baseline_m: 1.0
+min_ray_angle_deg: 5.0
+outlier_residual_threshold_m: 3.0
+max_outlier_iterations: 3

bearing_localization/include/bearing_localization/lib/bearing_localization_config.hpp

@@ -28,7 +28,6 @@ struct BearingLocalizationConfig {
         1.0;  // Residual above this marks a measurement as outlier (m).
     int max_outlier_iterations =
         2;  // Max rounds of iterative outlier rejection.
-
     /**
      * @brief Load configuration from a YAML file.
      * @param path Filesystem path to the YAML profile.

bearing_localization/include/bearing_localization/ros/bearing_localization_node.hpp

@@ -6,14 +6,14 @@
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
+#include <vortex_msgs/msg/bearing_measurement_array.hpp>
 #include <vortex_msgs/msg/landmark_array.hpp>
-#include <vortex_msgs/msg/vector3_array.hpp>
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 namespace bearing_localization {
 
@@ -22,12 +22,10 @@ namespace bearing_localization {
  * server.
  */
 struct NodeConfig {
-    std::string target_frame =
-        "orca/odom";  // TF frame to transform measurements into.
-    bool publish_debug_markers =
-        true;               // Whether to publish debug visualisation markers.
-    int landmark_type = 0;  // Landmark type ID in the output message.
-    int landmark_subtype = 0;  // Landmark subtype ID in the output message.
+    std::string target_frame;    // TF frame to transform measurements into.
+    bool publish_debug_markers;  // Whether to publish debug visualisation
+                                 // markers.
+    int landmark_type;           // Landmark type ID in the output message.
 };
 
 /**
@@ -50,36 +48,39 @@ class BearingLocalizationNode : public rclcpp::Node {
     /// @brief Create subscribers and publishers based on declared parameters.
     void setup_pubsub();
 
-    /// @brief Callback for single Vector3Stamped bearing messages.
+    /// @brief Callback for BearingMeasurementArray messages.
     void bearing_callback(
-        const geometry_msgs::msg::Vector3Stamped::SharedPtr msg);
+        const vortex_msgs::msg::BearingMeasurementArray::SharedPtr msg);
 
-    /// @brief Callback for batched Vector3Array bearing messages.
-    void bearing_array_callback(
-        const vortex_msgs::msg::Vector3Array::SharedPtr msg);
+    /**
+     * @brief Transform a single bearing vector into the target frame.
+     * @param weight Measurement weight for the solver.
+     * @return The transformed RayMeasurement, or std::nullopt on failure.
+     */
+    std::optional<RayMeasurement> transform_bearing(
+        const geometry_msgs::msg::Vector3& vec,
+        const std_msgs::msg::Header& header,
+        double weight = 1.0);
 
     /**
-     * @brief Transform a single bearing vector into the target frame and
-     *        add it to the localiser buffer.
-     * @return True if the measurement was accepted, false on TF failure.
+     * @brief Get or create a BearingLocalizer for the given target ID.
      */
-    bool process_bearing(const geometry_msgs::msg::Vector3& vec,
-                         const std_msgs::msg::Header& header);
+    BearingLocalizer& get_localizer(int32_t target_id);
 
     /// @brief Publish the localisation result as a LandmarkArray message.
-    void publish_result(const LocalizationResult& result);
+    void publish_result(const LocalizationResult& result,
+                        int32_t target_id = 0);
 
     BearingLocalizationConfig cfg_;  // Algorithm config (from YAML profile).
     NodeConfig node_cfg_;            // ROS config (from parameter server).
-    std::unique_ptr<BearingLocalizer> localizer_;
+    std::unordered_map<int32_t, std::unique_ptr<BearingLocalizer>>
+        target_localizers_;  // Per-target localizers.
 
     std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
     std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
-    rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr
+    rclcpp::Subscription<vortex_msgs::msg::BearingMeasurementArray>::SharedPtr
         bearing_sub_;
-    rclcpp::Subscription<vortex_msgs::msg::Vector3Array>::SharedPtr
-        bearing_array_sub_;
 
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
         markers_pub_;

bearing_localization/launch/bearing_localization.launch.py

@@ -55,7 +55,7 @@ def generate_launch_description():
     profile_arg = DeclareLaunchArgument(
         "profile",
         default_value="default",
-        description="Parameter profile to use. Options: default, aruco.",
+        description="Parameter profile to use. Options: default, aruco, pinger.",
     )
 
     return LaunchDescription(

bearing_localization/src/ros/bearing_localization_node.cpp

@@ -20,9 +20,6 @@ BearingLocalizationNode::BearingLocalizationNode(
     node_cfg_.publish_debug_markers =
         get_parameter("publish_debug_markers").as_bool();
     node_cfg_.landmark_type = get_parameter("landmark_type").as_int();
-    node_cfg_.landmark_subtype = get_parameter("landmark_subtype").as_int();
-
-    localizer_ = std::make_unique<BearingLocalizer>(cfg_);
 
     tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@@ -37,29 +34,22 @@ BearingLocalizationNode::BearingLocalizationNode(
 
 void BearingLocalizationNode::declare_parameters() {
     declare_parameter<std::string>("config_file");
-    declare_parameter<std::string>("target_frame", "orca/odom");
-    declare_parameter<bool>("publish_debug_markers", true);
-    declare_parameter<int>("landmark_type", 0);
-    declare_parameter<int>("landmark_subtype", 0);
+    declare_parameter<std::string>("target_frame");
+    declare_parameter<bool>("publish_debug_markers");
+    declare_parameter<int>("landmark_type");
 
-    declare_parameter<std::string>("topics.bearing_measurement");
-    declare_parameter<std::string>("topics.bearing_array");
+    declare_parameter<std::string>("topics.bearing_measurements");
     declare_parameter<std::string>("topics.landmarks");
     declare_parameter<std::string>("topics.bearing_localization_markers");
 }
 
 void BearingLocalizationNode::setup_pubsub() {
-    bearing_sub_ = create_subscription<geometry_msgs::msg::Vector3Stamped>(
-        get_parameter("topics.bearing_measurement").as_string(),
-        vortex::utils::qos_profiles::sensor_data_profile(1),
-        std::bind(&BearingLocalizationNode::bearing_callback, this,
-                  std::placeholders::_1));
-
-    bearing_array_sub_ = create_subscription<vortex_msgs::msg::Vector3Array>(
-        get_parameter("topics.bearing_array").as_string(),
-        vortex::utils::qos_profiles::sensor_data_profile(1),
-        std::bind(&BearingLocalizationNode::bearing_array_callback, this,
-                  std::placeholders::_1));
+    bearing_sub_ =
+        create_subscription<vortex_msgs::msg::BearingMeasurementArray>(
+            get_parameter("topics.bearing_measurements").as_string(),
+            vortex::utils::qos_profiles::sensor_data_profile(1),
+            std::bind(&BearingLocalizationNode::bearing_callback, this,
+                      std::placeholders::_1));
 
     if (node_cfg_.publish_debug_markers) {
         markers_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>(
@@ -73,62 +63,62 @@ void BearingLocalizationNode::setup_pubsub() {
 }
 
 void BearingLocalizationNode::bearing_callback(
-    const geometry_msgs::msg::Vector3Stamped::SharedPtr msg) {
-    process_bearing(msg->vector, msg->header);
-
-    auto result = localizer_->solve(now().seconds());
-    if (!result) {
-        return;
-    }
-    publish_result(*result);
-    if (node_cfg_.publish_debug_markers && markers_pub_) {
-        markers_pub_->publish(
-            build_debug_markers(*result, node_cfg_.target_frame, now()));
-    }
-}
-
-void BearingLocalizationNode::bearing_array_callback(
-    const vortex_msgs::msg::Vector3Array::SharedPtr msg) {
-    for (const auto& vec : msg->vectors) {
-        process_bearing(vec, msg->header);
+    const vortex_msgs::msg::BearingMeasurementArray::SharedPtr msg) {
+    std::unordered_map<int32_t, bool> updated_targets;
+
+    for (const auto& measurement : msg->bearings) {
+        const auto& bearing = measurement.bearing;
+        auto ray = transform_bearing(bearing.vector, bearing.header,
+                                     measurement.weight);
+        if (!ray) {
+            continue;
+        }
+
+        const int32_t target_id = measurement.target_id;
+        get_localizer(target_id).add_measurement(*ray);
+        updated_targets[target_id] = true;
     }
 
-    auto result = localizer_->solve(now().seconds());
-    if (!result) {
-        return;
-    }
-    publish_result(*result);
-    if (node_cfg_.publish_debug_markers && markers_pub_) {
-        markers_pub_->publish(
-            build_debug_markers(*result, node_cfg_.target_frame, now()));
+    for (const auto& [target_id, _] : updated_targets) {
+        auto result = get_localizer(target_id).solve(now().seconds());
+        if (!result) {
+            continue;
+        }
+        publish_result(*result, target_id);
+        if (node_cfg_.publish_debug_markers && markers_pub_) {
+            markers_pub_->publish(
+                build_debug_markers(*result, node_cfg_.target_frame, now()));
+        }
     }
 }
 
-bool BearingLocalizationNode::process_bearing(
+std::optional<RayMeasurement> BearingLocalizationNode::transform_bearing(
     const geometry_msgs::msg::Vector3& vec,
-    const std_msgs::msg::Header& header) {
+    const std_msgs::msg::Header& header,
+    double weight) {
     Eigen::Vector3d dir(vec.x, vec.y, vec.z);
     const double norm = dir.norm();
     if (!std::isfinite(norm) || norm < 1e-6) {
         spdlog::warn("Invalid direction vector (norm={:.6f}), skipping.", norm);
-        return false;
+        return std::nullopt;
     }
     dir /= norm;
 
     const std::string& source_frame = header.frame_id;
     if (source_frame.empty()) {
         spdlog::warn("Received measurement with empty frame_id, skipping.");
-        return false;
+        return std::nullopt;
     }
 
     geometry_msgs::msg::TransformStamped tf_stamped;
     try {
         tf_stamped = tf_buffer_->lookupTransform(
-            node_cfg_.target_frame, source_frame, tf2::TimePointZero);
+            node_cfg_.target_frame, source_frame, rclcpp::Time(header.stamp),
+            rclcpp::Duration::from_seconds(0.2));
     } catch (const tf2::TransformException& ex) {
         spdlog::warn("TF lookup failed ({} -> {}): {}", source_frame,
                      node_cfg_.target_frame, ex.what());
-        return false;
+        return std::nullopt;
     }
 
     const auto& rot = tf_stamped.transform.rotation;
@@ -142,19 +132,30 @@ bool BearingLocalizationNode::process_bearing(
     ray.stamp_sec = rclcpp::Time(header.stamp).seconds();
     ray.origin_world = origin;
     ray.direction_world = dir_world;
-    localizer_->add_measurement(ray);
-    return true;
+    ray.weight = weight;
+    return ray;
+}
+
+BearingLocalizer& BearingLocalizationNode::get_localizer(int32_t target_id) {
+    auto it = target_localizers_.find(target_id);
+    if (it != target_localizers_.end()) {
+        return *it->second;
+    }
+    auto [inserted, _] = target_localizers_.emplace(
+        target_id, std::make_unique<BearingLocalizer>(cfg_));
+    return *inserted->second;
 }
 
-void BearingLocalizationNode::publish_result(const LocalizationResult& result) {
+void BearingLocalizationNode::publish_result(const LocalizationResult& result,
+                                             int32_t target_id) {
     auto stamp = now();
 
     vortex_msgs::msg::Landmark landmark;
     landmark.header.stamp = stamp;
     landmark.header.frame_id = node_cfg_.target_frame;
     landmark.id = 0;
     landmark.type.value = static_cast<uint16_t>(node_cfg_.landmark_type);
-    landmark.subtype.value = static_cast<uint16_t>(node_cfg_.landmark_subtype);
+    landmark.subtype.value = static_cast<uint16_t>(target_id);
     landmark.pose.pose.position.x = result.position.x();
     landmark.pose.pose.position.y = result.position.y();
     landmark.pose.pose.position.z = result.position.z();