Skip to content

Commit

Permalink
Clean shutdown of UAS node
Browse files Browse the repository at this point in the history 
Prohibit copying and moving of UAS

Removed startup_delay_timer
  • Loading branch information
@ugol-1
ugol-1 committed Jun 16, 2024
1 parent 46aeec6 commit ac225e1
Show file tree
Hide file tree
Showing 3 changed files with 116 additions and 111 deletions.
19 changes: 15 additions & 4 deletions mavros/include/mavros/mavros_uas.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -249,6 +249,16 @@ class UAS : public rclcpp::Node
const std::string & uas_url_ = "/uas1", uint8_t target_system_ = 1,
uint8_t target_component_ = 1);

/**
* @brief Prohibit @a UAS copying, because plugins hold raw pointers to @a UAS.
*/
UAS(UAS const&) = delete;

/**
* @brief Prohibit @a UAS moving, because plugins hold raw pointers to @a UAS.
*/
UAS(UAS&&) = delete;

~UAS() = default;

/**
Expand Down Expand Up @@ -578,11 +588,7 @@ class UAS : public rclcpp::Node
StrV plugin_denylist;

rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr set_parameters_handle_ptr;
rclcpp::TimerBase::SharedPtr startup_delay_timer;

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

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

// XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us.
// The executor thread is the last thing to initialize, and it must be the first thing to destroy.
using thread_ptr = std::unique_ptr<std::thread, std::function<void (std::thread *)>>;
thread_ptr exec_spin_thd;

//! initialize connection to the Router
void connect_to_router();

Expand Down
207 changes: 101 additions & 106 deletions mavros/src/lib/mavros_uas.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,119 +85,114 @@ UAS::UAS(
this->declare_parameter("map_frame_id", map_frame_id);

// NOTE: we can add_plugin() in constructor because it does not need shared_from_this()
startup_delay_timer = this->create_wall_timer(
10ms, [this]() {
startup_delay_timer->cancel();

std::string fcu_protocol;
int tgt_system, tgt_component;
this->get_parameter("uas_url", uas_url);
this->get_parameter("fcu_protocol", fcu_protocol);
this->get_parameter("system_id", source_system);
this->get_parameter("component_id", source_component);
this->get_parameter("target_system_id", tgt_system);
this->get_parameter("target_component_id", tgt_component);
this->get_parameter("plugin_allowlist", plugin_allowlist);
this->get_parameter("plugin_denylist", plugin_denylist);
this->get_parameter("base_link_frame_id", base_link_frame_id);
this->get_parameter("odom_frame_id", odom_frame_id);
this->get_parameter("map_frame_id", map_frame_id);

exec_spin_thd = thread_ptr(
new std::thread(
[this]() {
utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component);
auto lg = this->get_logger();

RCLCPP_INFO(
lg, "UAS Executor started, threads: %zu",
this->exec.get_number_of_threads());
this->exec.spin();
RCLCPP_WARN(lg, "UAS Executor terminated");
}),
[this](std::thread * t) {
this->exec.cancel();
t->join();
delete t;
});

// setup diag
diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str()));
diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run);

// setup uas link
if (fcu_protocol == "v1.0") {
set_protocol_version(mavconn::Protocol::V10);
} else if (fcu_protocol == "v2.0") {
set_protocol_version(mavconn::Protocol::V20);
} else {
RCLCPP_WARN(
get_logger(),
"Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.",
fcu_protocol.c_str());
set_protocol_version(mavconn::Protocol::V20);
}
std::string fcu_protocol;
int tgt_system, tgt_component;
this->get_parameter("uas_url", uas_url);
this->get_parameter("fcu_protocol", fcu_protocol);
this->get_parameter("system_id", source_system);
this->get_parameter("component_id", source_component);
this->get_parameter("target_system_id", tgt_system);
this->get_parameter("target_component_id", tgt_component);
this->get_parameter("plugin_allowlist", plugin_allowlist);
this->get_parameter("plugin_denylist", plugin_denylist);
this->get_parameter("base_link_frame_id", base_link_frame_id);
this->get_parameter("odom_frame_id", odom_frame_id);
this->get_parameter("map_frame_id", map_frame_id);

// setup diag
diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str()));
diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run);

// setup uas link
if (fcu_protocol == "v1.0") {
set_protocol_version(mavconn::Protocol::V10);
} else if (fcu_protocol == "v2.0") {
set_protocol_version(mavconn::Protocol::V20);
} else {
RCLCPP_WARN(
get_logger(),
"Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.",
fcu_protocol.c_str());
set_protocol_version(mavconn::Protocol::V20);
}

// setup source and target
set_tgt(tgt_system, tgt_component);
// setup source and target
set_tgt(tgt_system, tgt_component);

add_connection_change_handler(
std::bind(
&UAS::log_connect_change, this,
std::placeholders::_1));
add_connection_change_handler(
std::bind(
&UAS::log_connect_change, this,
std::placeholders::_1));

// prepare plugin lists
// issue #257 2: assume that all plugins blacklisted
if (plugin_denylist.empty() && !plugin_allowlist.empty()) {
plugin_denylist.emplace_back("*");
}
// prepare plugin lists
// issue #257 2: assume that all plugins blacklisted
if (plugin_denylist.empty() && !plugin_allowlist.empty()) {
plugin_denylist.emplace_back("*");
}

for (auto & name : plugin_factory_loader.getDeclaredClasses()) {
add_plugin(name);
}
for (auto & name : plugin_factory_loader.getDeclaredClasses()) {
add_plugin(name);
}

connect_to_router();

// Publish helper TFs used for frame transformation in the odometry plugin
{
std::string base_link_frd = base_link_frame_id + "_frd";
std::string odom_ned = odom_frame_id + "_ned";
std::string map_ned = map_frame_id + "_ned";
std::vector<geometry_msgs::msg::TransformStamped> transform_vector;
add_static_transform(
map_frame_id, map_ned, Eigen::Affine3d(
ftf::quaternion_from_rpy(
M_PI, 0,
M_PI_2)),
transform_vector);
add_static_transform(
odom_frame_id, odom_ned, Eigen::Affine3d(
ftf::quaternion_from_rpy(
M_PI, 0,
M_PI_2)),
transform_vector);
add_static_transform(
base_link_frame_id, base_link_frd,
Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector);

tf2_static_broadcaster.sendTransform(transform_vector);
}
connect_to_router();

// Publish helper TFs used for frame transformation in the odometry plugin
{
std::string base_link_frd = base_link_frame_id + "_frd";
std::string odom_ned = odom_frame_id + "_ned";
std::string map_ned = map_frame_id + "_ned";
std::vector<geometry_msgs::msg::TransformStamped> transform_vector;
add_static_transform(
map_frame_id, map_ned, Eigen::Affine3d(
ftf::quaternion_from_rpy(
M_PI, 0,
M_PI_2)),
transform_vector);
add_static_transform(
odom_frame_id, odom_ned, Eigen::Affine3d(
ftf::quaternion_from_rpy(
M_PI, 0,
M_PI_2)),
transform_vector);
add_static_transform(
base_link_frame_id, base_link_frd,
Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector);

tf2_static_broadcaster.sendTransform(transform_vector);
}

std::stringstream ss;
for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) {
ss << " " << s;
}
std::stringstream ss;
for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) {
ss << " " << s;
}

RCLCPP_INFO(
get_logger(), "Built-in SIMD instructions: %s",
Eigen::SimdInstructionSetsInUse());
RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version);
RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str());
RCLCPP_INFO(
get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u",
uas_url.c_str(),
source_system, source_component,
target_system, target_component);
RCLCPP_INFO(
get_logger(), "Built-in SIMD instructions: %s",
Eigen::SimdInstructionSetsInUse());
RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version);
RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str());
RCLCPP_INFO(
get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u",
uas_url.c_str(),
source_system, source_component,
target_system, target_component);

exec_spin_thd = thread_ptr(
new std::thread(
[this]() {
utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component);
auto lg = this->get_logger();

RCLCPP_INFO(
lg, "UAS Executor started, threads: %zu",
this->exec.get_number_of_threads());
this->exec.spin();
RCLCPP_WARN(lg, "UAS Executor terminated");
}),
[this](std::thread * t) {
this->exec.cancel();
t->join();
delete t;
});
}

Expand Down
1 change: 0 additions & 1 deletion mavros/test/test_uas.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,6 @@ class TestUAS : public ::testing::Test
MockUAS::SharedPtr create_node()
{
auto uas = std::make_shared<MockUAS>("test_mavros_uas");
uas->startup_delay_timer->cancel();
return uas;
}

Expand Down

0 comments on commit ac225e1

Please sign in to comment.