feat: add landmarkConfirmed node to the bt_treenodes

mission/vortex_bt_nodes/include/vortex_bt_nodes/map/landmark_confirmed.hpp

@@ -0,0 +1,32 @@
+#ifndef VORTEX_BT_NODES__MAP__LANDMARK_CONFIRMED_HPP_
+#define VORTEX_BT_NODES__MAP__LANDMARK_CONFIRMED_HPP_
+
+#include <behaviortree_cpp/condition_node.h>
+#include <memory>
+#include <string>
+
+#include "vortex_bt_nodes/map/landmark_map.hpp"
+
+namespace vortex_bt_nodes::map {
+
+/**
+ * @brief SUCCESS if the landmark has been obmax_sigma_xyserved and its horizontal std
+ * is below max_sigma_xy.
+ */
+class LandmarkConfirmed : public BT::ConditionNode {
+   public:
+    LandmarkConfirmed(const std::string& name,
+                      const BT::NodeConfig& config,
+                      std::shared_ptr<const LandmarkMap> map);
+
+    static BT::PortsList providedPorts();
+
+   private:
+    BT::NodeStatus tick() override;
+
+    std::shared_ptr<const LandmarkMap> map_;
+};
+
+}  // namespace vortex_bt_nodes::map
+
+#endif  // VORTEX_BT_NODES__MAP__LANDMARK_CONFIRMED_HPP_

mission/vortex_bt_nodes/src/map/landmark_confirmed.cpp

@@ -0,0 +1,36 @@
+#include "vortex_bt_nodes/map/landmark_confirmed.hpp"
+
+#include <spdlog/spdlog.h>
+
+namespace vortex_bt_nodes::map {
+
+LandmarkConfirmed::LandmarkConfirmed(const std::string& name,
+                                     const BT::NodeConfig& config,
+                                     std::shared_ptr<const LandmarkMap> map)
+    : BT::ConditionNode(name, config), map_(std::move(map)) {}
+
+BT::PortsList LandmarkConfirmed::providedPorts() {
+    BT::PortsList ports = LandmarkMap::ports();
+    ports.insert(BT::InputPort<double>(
+        "max_sigma_xy", "Largest horizontal std that counts as confirmed [m]"));
+    return ports;
+}
+
+BT::NodeStatus LandmarkConfirmed::tick() {
+    const auto max_sigma_xy = getInput<double>("max_sigma_xy");
+    if (!max_sigma_xy || *max_sigma_xy <= 0.0) {
+        SPDLOG_ERROR("LandmarkConfirmed: max_sigma_xy missing or not positive");
+        return BT::NodeStatus::FAILURE;
+    }
+
+    const auto landmark = map_->resolve(*this);
+    if (!landmark) {
+        return BT::NodeStatus::FAILURE;
+    }
+
+    return LandmarkMap::confirmed(*landmark, *max_sigma_xy)
+               ? BT::NodeStatus::SUCCESS
+               : BT::NodeStatus::FAILURE;
+}
+
+}  // namespace vortex_bt_nodes::map

mission/vortex_bt_nodes/src/map/register.cpp

@@ -2,16 +2,17 @@
 
 #include <memory>
 
+#include "vortex_bt_nodes/map/landmark_confirmed.hpp"
 #include "vortex_bt_nodes/map/landmark_map.hpp"
 
 namespace vortex_bt_nodes::map {
 
-void register_nodes(BT::BehaviorTreeFactory& /*factory*/,
+void register_nodes(BT::BehaviorTreeFactory& factory,
                     const rclcpp::Node::SharedPtr& node) {
     // One map for all map nodes. Pass it to the nodes that need it.
-    [[maybe_unused]] const std::shared_ptr<const LandmarkMap> map =
+    const std::shared_ptr<const LandmarkMap> map =
         std::make_shared<LandmarkMap>(node);
-    // factory.registerNodeType<MyNode>("MyNode", node, map);
+    factory.registerNodeType<LandmarkConfirmed>("LandmarkConfirmed", map);
 }
 
 }  // namespace vortex_bt_nodes::map