simulation
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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
14
namespace
simulation
15
{
16
17
// Read the recorder's latched pose without blocking the navigator's executor.
18
class
GetHomePose
:
public
BT::StatefulActionNode
19
{
20
public
:
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
90
private
:
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
114
BT_REGISTER_NODES
(factory)
115
{
116
factory.registerNodeType<
simulation::GetHomePose
>(
"GetHomePose"
);
117
}
simulation::GetHomePose
定義
get_home_pose.cpp:19
simulation::GetHomePose::GetHomePose
GetHomePose(const std::string &name, const BT::NodeConfiguration &config)
定義
get_home_pose.cpp:21
simulation::GetHomePose::onHalted
void onHalted() override
定義
get_home_pose.cpp:84
simulation::GetHomePose::onStart
BT::NodeStatus onStart() override
定義
get_home_pose.cpp:39
simulation::GetHomePose::onRunning
BT::NodeStatus onRunning() override
定義
get_home_pose.cpp:63
simulation::GetHomePose::providedPorts
static BT::PortsList providedPorts()
定義
get_home_pose.cpp:30
BT_REGISTER_NODES
BT_REGISTER_NODES(factory)
定義
get_home_pose.cpp:114
simulation
定義
get_home_pose.cpp:15
src
get_home_pose.cpp
構築:
1.18.0