Skip to content

Commit ac225e1

Browse files
ugol-1ugol-1
committed
Clean shutdown of UAS node
Prohibit copying and moving of UAS Removed startup_delay_timer
1 parent 46aeec6 commit ac225e1

File tree

3 files changed

+116
-111
lines changed

3 files changed

+116
-111
lines changed

‎mavros/include/mavros/mavros_uas.hpp

+15-4
Original file line numberDiff line numberDiff line change
@@ -249,6 +249,16 @@ class UAS : public rclcpp::Node
249249
const std::string & uas_url_ = "/uas1", uint8_t target_system_ = 1,
250250
uint8_t target_component_ = 1);
251251

252+
/**
253+
* @brief Prohibit @a UAS copying, because plugins hold raw pointers to @a UAS.
254+
*/
255+
UAS(UAS const&) = delete;
256+
257+
/**
258+
* @brief Prohibit @a UAS moving, because plugins hold raw pointers to @a UAS.
259+
*/
260+
UAS(UAS&&) = delete;
261+
252262
~UAS() = default;
253263

254264
/**
@@ -578,11 +588,7 @@ class UAS : public rclcpp::Node
578588
StrV plugin_denylist;
579589

580590
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr set_parameters_handle_ptr;
581-
rclcpp::TimerBase::SharedPtr startup_delay_timer;
582591

583-
// XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us.
584-
using thread_ptr = std::unique_ptr<std::thread, std::function<void (std::thread *)>>;
585-
thread_ptr exec_spin_thd;
586592
// rclcpp::executors::MultiThreadedExecutor exec;
587593
UASExecutor exec;
588594

@@ -614,6 +620,11 @@ class UAS : public rclcpp::Node
614620
rclcpp::Subscription<mavros_msgs::msg::Mavlink>::SharedPtr source; // FCU -> UAS
615621
rclcpp::Publisher<mavros_msgs::msg::Mavlink>::SharedPtr sink; // UAS -> FCU
616622

623+
// XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us.
624+
// The executor thread is the last thing to initialize, and it must be the first thing to destroy.
625+
using thread_ptr = std::unique_ptr<std::thread, std::function<void (std::thread *)>>;
626+
thread_ptr exec_spin_thd;
627+
617628
//! initialize connection to the Router
618629
void connect_to_router();
619630

‎mavros/src/lib/mavros_uas.cpp

+101-106
Original file line numberDiff line numberDiff line change
@@ -85,119 +85,114 @@ UAS::UAS(
8585
this->declare_parameter("map_frame_id", map_frame_id);
8686

8787
// NOTE: we can add_plugin() in constructor because it does not need shared_from_this()
88-
startup_delay_timer = this->create_wall_timer(
89-
10ms, [this]() {
90-
startup_delay_timer->cancel();
91-
92-
std::string fcu_protocol;
93-
int tgt_system, tgt_component;
94-
this->get_parameter("uas_url", uas_url);
95-
this->get_parameter("fcu_protocol", fcu_protocol);
96-
this->get_parameter("system_id", source_system);
97-
this->get_parameter("component_id", source_component);
98-
this->get_parameter("target_system_id", tgt_system);
99-
this->get_parameter("target_component_id", tgt_component);
100-
this->get_parameter("plugin_allowlist", plugin_allowlist);
101-
this->get_parameter("plugin_denylist", plugin_denylist);
102-
this->get_parameter("base_link_frame_id", base_link_frame_id);
103-
this->get_parameter("odom_frame_id", odom_frame_id);
104-
this->get_parameter("map_frame_id", map_frame_id);
105-
106-
exec_spin_thd = thread_ptr(
107-
new std::thread(
108-
[this]() {
109-
utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component);
110-
auto lg = this->get_logger();
111-
112-
RCLCPP_INFO(
113-
lg, "UAS Executor started, threads: %zu",
114-
this->exec.get_number_of_threads());
115-
this->exec.spin();
116-
RCLCPP_WARN(lg, "UAS Executor terminated");
117-
}),
118-
[this](std::thread * t) {
119-
this->exec.cancel();
120-
t->join();
121-
delete t;
122-
});
123-
124-
// setup diag
125-
diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str()));
126-
diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run);
127-
128-
// setup uas link
129-
if (fcu_protocol == "v1.0") {
130-
set_protocol_version(mavconn::Protocol::V10);
131-
} else if (fcu_protocol == "v2.0") {
132-
set_protocol_version(mavconn::Protocol::V20);
133-
} else {
134-
RCLCPP_WARN(
135-
get_logger(),
136-
"Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.",
137-
fcu_protocol.c_str());
138-
set_protocol_version(mavconn::Protocol::V20);
139-
}
88+
std::string fcu_protocol;
89+
int tgt_system, tgt_component;
90+
this->get_parameter("uas_url", uas_url);
91+
this->get_parameter("fcu_protocol", fcu_protocol);
92+
this->get_parameter("system_id", source_system);
93+
this->get_parameter("component_id", source_component);
94+
this->get_parameter("target_system_id", tgt_system);
95+
this->get_parameter("target_component_id", tgt_component);
96+
this->get_parameter("plugin_allowlist", plugin_allowlist);
97+
this->get_parameter("plugin_denylist", plugin_denylist);
98+
this->get_parameter("base_link_frame_id", base_link_frame_id);
99+
this->get_parameter("odom_frame_id", odom_frame_id);
100+
this->get_parameter("map_frame_id", map_frame_id);
101+
102+
// setup diag
103+
diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str()));
104+
diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run);
105+
106+
// setup uas link
107+
if (fcu_protocol == "v1.0") {
108+
set_protocol_version(mavconn::Protocol::V10);
109+
} else if (fcu_protocol == "v2.0") {
110+
set_protocol_version(mavconn::Protocol::V20);
111+
} else {
112+
RCLCPP_WARN(
113+
get_logger(),
114+
"Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.",
115+
fcu_protocol.c_str());
116+
set_protocol_version(mavconn::Protocol::V20);
117+
}
140118

141-
// setup source and target
142-
set_tgt(tgt_system, tgt_component);
119+
// setup source and target
120+
set_tgt(tgt_system, tgt_component);
143121

144-
add_connection_change_handler(
145-
std::bind(
146-
&UAS::log_connect_change, this,
147-
std::placeholders::_1));
122+
add_connection_change_handler(
123+
std::bind(
124+
&UAS::log_connect_change, this,
125+
std::placeholders::_1));
148126

149-
// prepare plugin lists
150-
// issue #257 2: assume that all plugins blacklisted
151-
if (plugin_denylist.empty() && !plugin_allowlist.empty()) {
152-
plugin_denylist.emplace_back("*");
153-
}
127+
// prepare plugin lists
128+
// issue #257 2: assume that all plugins blacklisted
129+
if (plugin_denylist.empty() && !plugin_allowlist.empty()) {
130+
plugin_denylist.emplace_back("*");
131+
}
154132

155-
for (auto & name : plugin_factory_loader.getDeclaredClasses()) {
156-
add_plugin(name);
157-
}
133+
for (auto & name : plugin_factory_loader.getDeclaredClasses()) {
134+
add_plugin(name);
135+
}
158136

159-
connect_to_router();
160-
161-
// Publish helper TFs used for frame transformation in the odometry plugin
162-
{
163-
std::string base_link_frd = base_link_frame_id + "_frd";
164-
std::string odom_ned = odom_frame_id + "_ned";
165-
std::string map_ned = map_frame_id + "_ned";
166-
std::vector<geometry_msgs::msg::TransformStamped> transform_vector;
167-
add_static_transform(
168-
map_frame_id, map_ned, Eigen::Affine3d(
169-
ftf::quaternion_from_rpy(
170-
M_PI, 0,
171-
M_PI_2)),
172-
transform_vector);
173-
add_static_transform(
174-
odom_frame_id, odom_ned, Eigen::Affine3d(
175-
ftf::quaternion_from_rpy(
176-
M_PI, 0,
177-
M_PI_2)),
178-
transform_vector);
179-
add_static_transform(
180-
base_link_frame_id, base_link_frd,
181-
Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector);
182-
183-
tf2_static_broadcaster.sendTransform(transform_vector);
184-
}
137+
connect_to_router();
138+
139+
// Publish helper TFs used for frame transformation in the odometry plugin
140+
{
141+
std::string base_link_frd = base_link_frame_id + "_frd";
142+
std::string odom_ned = odom_frame_id + "_ned";
143+
std::string map_ned = map_frame_id + "_ned";
144+
std::vector<geometry_msgs::msg::TransformStamped> transform_vector;
145+
add_static_transform(
146+
map_frame_id, map_ned, Eigen::Affine3d(
147+
ftf::quaternion_from_rpy(
148+
M_PI, 0,
149+
M_PI_2)),
150+
transform_vector);
151+
add_static_transform(
152+
odom_frame_id, odom_ned, Eigen::Affine3d(
153+
ftf::quaternion_from_rpy(
154+
M_PI, 0,
155+
M_PI_2)),
156+
transform_vector);
157+
add_static_transform(
158+
base_link_frame_id, base_link_frd,
159+
Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector);
160+
161+
tf2_static_broadcaster.sendTransform(transform_vector);
162+
}
185163

186-
std::stringstream ss;
187-
for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) {
188-
ss << " " << s;
189-
}
164+
std::stringstream ss;
165+
for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) {
166+
ss << " " << s;
167+
}
190168

191-
RCLCPP_INFO(
192-
get_logger(), "Built-in SIMD instructions: %s",
193-
Eigen::SimdInstructionSetsInUse());
194-
RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version);
195-
RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str());
196-
RCLCPP_INFO(
197-
get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u",
198-
uas_url.c_str(),
199-
source_system, source_component,
200-
target_system, target_component);
169+
RCLCPP_INFO(
170+
get_logger(), "Built-in SIMD instructions: %s",
171+
Eigen::SimdInstructionSetsInUse());
172+
RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version);
173+
RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str());
174+
RCLCPP_INFO(
175+
get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u",
176+
uas_url.c_str(),
177+
source_system, source_component,
178+
target_system, target_component);
179+
180+
exec_spin_thd = thread_ptr(
181+
new std::thread(
182+
[this]() {
183+
utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component);
184+
auto lg = this->get_logger();
185+
186+
RCLCPP_INFO(
187+
lg, "UAS Executor started, threads: %zu",
188+
this->exec.get_number_of_threads());
189+
this->exec.spin();
190+
RCLCPP_WARN(lg, "UAS Executor terminated");
191+
}),
192+
[this](std::thread * t) {
193+
this->exec.cancel();
194+
t->join();
195+
delete t;
201196
});
202197
}
203198

‎mavros/test/test_uas.cpp

-1
Original file line numberDiff line numberDiff line change
@@ -127,7 +127,6 @@ class TestUAS : public ::testing::Test
127127
MockUAS::SharedPtr create_node()
128128
{
129129
auto uas = std::make_shared<MockUAS>("test_mavros_uas");
130-
uas->startup_delay_timer->cancel();
131130
return uas;
132131
}
133132

0 commit comments

Comments
 (0)