simulation
読み取り中…
検索中…
一致する文字列を見つけられません
get_home_pose.cpp
[詳解]
1// Copyright 2026 MIRS
2// SPDX-License-Identifier: MIT
3
4#include <chrono>
5#include <cmath>
6#include <memory>
7#include <string>
8
9#include "behaviortree_cpp/action_node.h"
10#include "behaviortree_cpp/bt_factory.h"
11#include "geometry_msgs/msg/pose_stamped.hpp"
12#include "rclcpp/rclcpp.hpp"
13
14namespace simulation
15{
16
17// Read the recorder's latched pose without blocking the navigator's executor.
18class GetHomePose : public BT::StatefulActionNode
19{
20public:
21 GetHomePose(const std::string & name, const BT::NodeConfiguration & config)
22 : BT::StatefulActionNode(name, config),
23 node_(config.blackboard->get<rclcpp::Node::SharedPtr>("node"))
24 {
25 callback_group_ = node_->create_callback_group(
26 rclcpp::CallbackGroupType::MutuallyExclusive, false);
27 executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
28 }
29
30 static BT::PortsList providedPorts()
31 {
32 return {
33 BT::InputPort<std::string>("topic", "/simulation/home_pose", "Recorded home topic"),
34 BT::InputPort<std::string>("global_frame", "map", "Required home frame"),
35 BT::InputPort<double>("timeout", 10.0, "Maximum wait in wall seconds"),
36 BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Recorded home pose")};
37 }
38
39 BT::NodeStatus onStart() override
40 {
41 std::string topic;
42 if (!getInput("topic", topic) || topic.empty() ||
43 !getInput("global_frame", frame_) || frame_.empty() ||
44 !getInput("timeout", timeout_) || !std::isfinite(timeout_) || timeout_ <= 0.0)
45 {
46 RCLCPP_ERROR(node_->get_logger(), "GetHomePose: invalid topic, frame or timeout");
47 return BT::NodeStatus::FAILURE;
48 }
49 home_.reset();
50 rclcpp::SubscriptionOptions options;
51 options.callback_group = callback_group_;
52 subscription_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
53 topic, rclcpp::QoS(1).reliable().transient_local(),
54 [this](geometry_msgs::msg::PoseStamped::ConstSharedPtr message) {
55 if (valid(*message)) {
56 home_ = std::make_shared<geometry_msgs::msg::PoseStamped>(*message);
57 }
58 }, options);
59 started_ = std::chrono::steady_clock::now();
60 return onRunning();
61 }
62
63 BT::NodeStatus onRunning() override
64 {
65 executor_.spin_some();
66 if (home_) {
67 auto pose = *home_;
68 // The stored pose is a fixed map location, not a historical TF query.
69 pose.header.stamp = node_->now();
70 setOutput("pose", pose);
71 subscription_.reset();
72 return BT::NodeStatus::SUCCESS;
73 }
74 if (std::chrono::duration<double>(std::chrono::steady_clock::now() - started_).count() >=
75 timeout_)
76 {
77 RCLCPP_ERROR(node_->get_logger(), "GetHomePose: timed out waiting for a valid home pose");
78 subscription_.reset();
79 return BT::NodeStatus::FAILURE;
80 }
81 return BT::NodeStatus::RUNNING;
82 }
83
84 void onHalted() override
85 {
86 subscription_.reset();
87 home_.reset();
88 }
89
90private:
91 bool valid(const geometry_msgs::msg::PoseStamped & message) const
92 {
93 const auto & p = message.pose.position;
94 const auto & q = message.pose.orientation;
95 const double norm = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
96 return message.header.frame_id == frame_ &&
97 std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z) &&
98 std::isfinite(q.x) && std::isfinite(q.y) &&
99 std::isfinite(q.z) && std::isfinite(q.w) && std::abs(norm - 1.0) < 1e-3;
100 }
101
102 rclcpp::Node::SharedPtr node_;
103 rclcpp::CallbackGroup::SharedPtr callback_group_;
104 rclcpp::executors::SingleThreadedExecutor executor_;
105 rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_;
106 geometry_msgs::msg::PoseStamped::SharedPtr home_;
107 std::string frame_;
108 double timeout_{10.0};
109 std::chrono::steady_clock::time_point started_;
110};
111
112} // namespace simulation
113
115{
116 factory.registerNodeType<simulation::GetHomePose>("GetHomePose");
117}
GetHomePose(const std::string &name, const BT::NodeConfiguration &config)
BT::NodeStatus onStart() override
BT::NodeStatus onRunning() override
static BT::PortsList providedPorts()
BT_REGISTER_NODES(factory)