diff --git a/mission/vortex_bt_nodes/include/vortex_bt_nodes/map/landmark_confirmed.hpp b/mission/vortex_bt_nodes/include/vortex_bt_nodes/map/landmark_confirmed.hpp new file mode 100644 index 0000000..e4e7893 --- /dev/null +++ b/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 +#include +#include + +#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 map); + + static BT::PortsList providedPorts(); + + private: + BT::NodeStatus tick() override; + + std::shared_ptr map_; +}; + +} // namespace vortex_bt_nodes::map + +#endif // VORTEX_BT_NODES__MAP__LANDMARK_CONFIRMED_HPP_ diff --git a/mission/vortex_bt_nodes/src/map/landmark_confirmed.cpp b/mission/vortex_bt_nodes/src/map/landmark_confirmed.cpp new file mode 100644 index 0000000..c4f0ee0 --- /dev/null +++ b/mission/vortex_bt_nodes/src/map/landmark_confirmed.cpp @@ -0,0 +1,36 @@ +#include "vortex_bt_nodes/map/landmark_confirmed.hpp" + +#include + +namespace vortex_bt_nodes::map { + +LandmarkConfirmed::LandmarkConfirmed(const std::string& name, + const BT::NodeConfig& config, + std::shared_ptr map) + : BT::ConditionNode(name, config), map_(std::move(map)) {} + +BT::PortsList LandmarkConfirmed::providedPorts() { + BT::PortsList ports = LandmarkMap::ports(); + ports.insert(BT::InputPort( + "max_sigma_xy", "Largest horizontal std that counts as confirmed [m]")); + return ports; +} + +BT::NodeStatus LandmarkConfirmed::tick() { + const auto max_sigma_xy = getInput("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 diff --git a/mission/vortex_bt_nodes/src/map/register.cpp b/mission/vortex_bt_nodes/src/map/register.cpp index 14522ce..5459839 100644 --- a/mission/vortex_bt_nodes/src/map/register.cpp +++ b/mission/vortex_bt_nodes/src/map/register.cpp @@ -2,16 +2,17 @@ #include +#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 map = + const std::shared_ptr map = std::make_shared(node); - // factory.registerNodeType("MyNode", node, map); + factory.registerNodeType("LandmarkConfirmed", map); } } // namespace vortex_bt_nodes::map