From feef01fc488029749b7c41113c40ec07668a5a85 Mon Sep 17 00:00:00 2001 From: ugol-1 Date: Tue, 25 Jun 2024 20:27:01 +0000 Subject: [PATCH 1/5] Fix some compiler warnings --- libmavconn/src/interface.cpp | 2 +- mavros/include/mavros/mavros_uas.hpp | 6 ++++-- mavros/include/mavros/px4_custom_mode.hpp | 2 +- mavros/include/mavros/setpoint_mixin.hpp | 6 +++--- mavros/src/plugins/setpoint_accel.cpp | 3 +-- mavros/src/plugins/setpoint_trajectory.cpp | 3 ++- mavros/src/plugins/sys_status.cpp | 10 ++++++---- mavros/test/test_router.cpp | 2 +- mavros_extras/src/plugins/gimbal_control.cpp | 16 ++++++++++------ .../src/plugins/onboard_computer_status.cpp | 5 +++-- mavros_extras/src/plugins/trajectory.cpp | 6 +++--- 11 files changed, 35 insertions(+), 26 deletions(-) diff --git a/libmavconn/src/interface.cpp b/libmavconn/src/interface.cpp index 38a12bb67..3ed5a17fe 100644 --- a/libmavconn/src/interface.cpp +++ b/libmavconn/src/interface.cpp @@ -99,7 +99,7 @@ void MAVConnInterface::iostat_rx_add(size_t bytes) } void MAVConnInterface::parse_buffer( - const char * pfx, uint8_t * buf, const size_t bufsize, + const char * pfx, uint8_t * buf, const size_t bufsize [[maybe_unused]], size_t bytes_received) { mavlink::mavlink_message_t message; diff --git a/mavros/include/mavros/mavros_uas.hpp b/mavros/include/mavros/mavros_uas.hpp index 2aed4b41d..fb24c25db 100644 --- a/mavros/include/mavros/mavros_uas.hpp +++ b/mavros/include/mavros/mavros_uas.hpp @@ -161,7 +161,8 @@ class Data inline double geoid_to_ellipsoid_height(const T lla) { if (egm96_5) { - return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude); + return static_cast(GeographicLib::Geoid::GEOIDTOELLIPSOID) * + (*egm96_5)(lla->latitude, lla->longitude); } else { return 0.0; } @@ -181,7 +182,8 @@ class Data inline double ellipsoid_to_geoid_height(const T lla) { if (egm96_5) { - return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude); + return static_cast(GeographicLib::Geoid::ELLIPSOIDTOGEOID) * + (*egm96_5)(lla->latitude, lla->longitude); } else { return 0.0; } diff --git a/mavros/include/mavros/px4_custom_mode.hpp b/mavros/include/mavros/px4_custom_mode.hpp index a8e664e30..3fb19338b 100644 --- a/mavros/include/mavros/px4_custom_mode.hpp +++ b/mavros/include/mavros/px4_custom_mode.hpp @@ -100,7 +100,7 @@ union custom_mode { } constexpr custom_mode(uint8_t mm, uint8_t sm) - : mode{0, mm, sm} + : mode{0, mm, sm} { } }; diff --git a/mavros/include/mavros/setpoint_mixin.hpp b/mavros/include/mavros/setpoint_mixin.hpp index dc9c3c6bd..e6c2fa771 100644 --- a/mavros/include/mavros/setpoint_mixin.hpp +++ b/mavros/include/mavros/setpoint_mixin.hpp @@ -45,9 +45,9 @@ class SetPositionTargetLocalNEDMixin void set_position_target_local_ned( uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, - Eigen::Vector3d p, - Eigen::Vector3d v, - Eigen::Vector3d af, + Eigen::Vector3d const & p, + Eigen::Vector3d const & v, + Eigen::Vector3d const & af, float yaw, float yaw_rate) { static_assert( diff --git a/mavros/src/plugins/setpoint_accel.cpp b/mavros/src/plugins/setpoint_accel.cpp index e11ff4742..385c2f10f 100644 --- a/mavros/src/plugins/setpoint_accel.cpp +++ b/mavros/src/plugins/setpoint_accel.cpp @@ -75,8 +75,7 @@ class SetpointAccelerationPlugin : public plugin::Plugin, { using mavlink::common::MAV_FRAME; - bool send_force; - node->get_parameter("send_force", send_force); + bool const send_force = node->get_parameter("send_force").as_bool(); /* Documentation start from bit 1 instead 0. * Ignore position and velocity vectors, yaw and yaw rate diff --git a/mavros/src/plugins/setpoint_trajectory.cpp b/mavros/src/plugins/setpoint_trajectory.cpp index 732c06167..59e3396d9 100644 --- a/mavros/src/plugins/setpoint_trajectory.cpp +++ b/mavros/src/plugins/setpoint_trajectory.cpp @@ -183,7 +183,8 @@ class SetpointTrajectoryPlugin : public plugin::Plugin, return; } - Eigen::Vector3d position, velocity, af; + Eigen::Vector3d position = Eigen::Vector3d::Zero(), velocity = Eigen::Vector3d::Zero(), + af = Eigen::Vector3d::Zero(); Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity(); float yaw_rate = 0; uint16_t type_mask = 0; diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index a8a6bea0f..45902b715 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -786,8 +786,9 @@ class SystemStatusPlugin : public plugin::Plugin case enum_value(MAV_SEVERITY::ALERT): case enum_value(MAV_SEVERITY::CRITICAL): case enum_value(MAV_SEVERITY::ERROR): - RCLCPP_ERROR_STREAM(node->get_logger(), - "FCU: EVENT " << px4_id << " with args " << arg_str); + RCLCPP_ERROR_STREAM( + node->get_logger(), + "FCU: EVENT " << px4_id << " with args " << arg_str); break; case enum_value(MAV_SEVERITY::WARNING): case enum_value(MAV_SEVERITY::NOTICE): @@ -797,8 +798,9 @@ class SystemStatusPlugin : public plugin::Plugin RCLCPP_INFO_STREAM(node->get_logger(), "FCU: EVENT " << px4_id << " with args " << arg_str); break; case enum_value(MAV_SEVERITY::DEBUG): - RCLCPP_DEBUG_STREAM(node->get_logger(), - "FCU: EVENT " << px4_id << " with args " << arg_str); + RCLCPP_DEBUG_STREAM( + node->get_logger(), + "FCU: EVENT " << px4_id << " with args " << arg_str); break; // [[[end]]] (checksum: 83f5eab6a8989f95de46d2a95387304c) default: diff --git a/mavros/test/test_router.cpp b/mavros/test/test_router.cpp index 1f68c16d1..335908e2d 100644 --- a/mavros/test/test_router.cpp +++ b/mavros/test/test_router.cpp @@ -93,7 +93,7 @@ class TestRouter : public ::testing::Test auto make_and_add_mock_endpoint = [router](id_t id, const std::string & url, LT type, - std::set remotes) { + std::set remotes) { auto ep = std::make_shared(); ep->parent = router; diff --git a/mavros_extras/src/plugins/gimbal_control.cpp b/mavros_extras/src/plugins/gimbal_control.cpp index 3f6c9ca85..a62ce1b01 100644 --- a/mavros_extras/src/plugins/gimbal_control.cpp +++ b/mavros_extras/src/plugins/gimbal_control.cpp @@ -123,14 +123,16 @@ class GimbalControlPlugin : public plugin::Plugin _1)); // --Not successfully validated-- also note that the message is the same as pitchyaw and will likely change - set_manager_manual_control_sub = node->create_subscription( + set_manager_manual_control_sub = + node->create_subscription( "~/manager/set_manual_control", 10, std::bind( &GimbalControlPlugin::manager_set_manual_control_cb, this, _1)); // Publishers - gimbal_attitude_status_pub = node->create_publisher( + gimbal_attitude_status_pub = + node->create_publisher( "~/device/attitude_status", 10); @@ -243,11 +245,13 @@ class GimbalControlPlugin : public plugin::Plugin auto services_qos = rclcpp::ServicesQoS(); #endif - cmd_cli = node->create_client("cmd/command", services_qos, - cb_group); + cmd_cli = node->create_client( + "cmd/command", services_qos, + cb_group); while (!cmd_cli->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(node->get_logger(), - "GimbalControl: mavros/cmd/command service not available after waiting"); + RCLCPP_ERROR( + node->get_logger(), + "GimbalControl: mavros/cmd/command service not available after waiting"); cmd_cli.reset(); throw std::logic_error("client not connected"); } diff --git a/mavros_extras/src/plugins/onboard_computer_status.cpp b/mavros_extras/src/plugins/onboard_computer_status.cpp index 2a0aa9e61..ceff008d1 100644 --- a/mavros_extras/src/plugins/onboard_computer_status.cpp +++ b/mavros_extras/src/plugins/onboard_computer_status.cpp @@ -99,8 +99,9 @@ class OnboardComputerStatusPlugin : public plugin::Plugin std::copy(req->cpu_combined.cbegin(), req->cpu_combined.cend(), status.cpu_combined.begin()); std::copy(req->gpu_cores.cbegin(), req->gpu_cores.cend(), status.gpu_cores.begin()); std::copy(req->gpu_combined.cbegin(), req->gpu_combined.cend(), status.gpu_combined.begin()); - std::copy(req->temperature_core.cbegin(), req->temperature_core.cend(), - status.temperature_core.begin()); + std::copy( + req->temperature_core.cbegin(), req->temperature_core.cend(), + status.temperature_core.begin()); std::copy(req->fan_speed.cbegin(), req->fan_speed.cend(), status.fan_speed.begin()); std::copy(req->storage_type.cbegin(), req->storage_type.cend(), status.storage_type.begin()); std::copy(req->storage_usage.cbegin(), req->storage_usage.cend(), status.storage_usage.begin()); diff --git a/mavros_extras/src/plugins/trajectory.cpp b/mavros_extras/src/plugins/trajectory.cpp index 209dd1582..51963c748 100644 --- a/mavros_extras/src/plugins/trajectory.cpp +++ b/mavros_extras/src/plugins/trajectory.cpp @@ -262,7 +262,7 @@ class TrajectoryPlugin : public plugin::Plugin auto fill_point_rep_waypoints = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, const RosPoints & rp, - const size_t i) { + const size_t i) { const auto valid = req->point_valid[i]; auto valid_so_far = trajectory.valid_points; @@ -299,7 +299,7 @@ class TrajectoryPlugin : public plugin::Plugin mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER trajectory {}; auto fill_point_rep_bezier = [&](mavlink::common::msg::TRAJECTORY_REPRESENTATION_BEZIER & t, const RosPoints & rp, - const size_t i) { + const size_t i) { const auto valid = req->point_valid[i]; auto valid_so_far = trajectory.valid_points; @@ -382,7 +382,7 @@ class TrajectoryPlugin : public plugin::Plugin auto fill_msg_point = [&](RosPoints & p, const mavlink::common::msg::TRAJECTORY_REPRESENTATION_WAYPOINTS & t, - const size_t i) { + const size_t i) { fill_msg_position(p.position, t, i); fill_msg_velocity(p.velocity, t, i); fill_msg_acceleration(p.acceleration_or_force, t, i); From 0ce85fe865bac8ebb8cbdba7d9fd728e232fec0e Mon Sep 17 00:00:00 2001 From: ugol-1 Date: Tue, 25 Jun 2024 20:27:18 +0000 Subject: [PATCH 2/5] - Plugin class ctors implementation moved to plugin.cpp. - Removed unnecessary dynamic_pointer_cast<>'s Removed unused Plugin ctor --- mavros/include/mavros/plugin.hpp | 13 ++----------- mavros/src/lib/plugin.cpp | 11 ++++++++++- mavros/test/test_uas.cpp | 2 +- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/mavros/include/mavros/plugin.hpp b/mavros/include/mavros/plugin.hpp index ee4c48bba..17725ae0f 100644 --- a/mavros/include/mavros/plugin.hpp +++ b/mavros/include/mavros/plugin.hpp @@ -74,18 +74,9 @@ class Plugin : public std::enable_shared_from_this //! Subscriptions vector using Subscriptions = std::vector; - explicit Plugin(UASPtr uas_) - : uas(uas_), node(std::dynamic_pointer_cast(uas_)) - {} - explicit Plugin( - UASPtr uas_, const std::string & subnode, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : uas(uas_), - // node(std::dynamic_pointer_cast(uas_)->create_sub_node(subnode)) // https://github.com/ros2/rclcpp/issues/731 - node(rclcpp::Node::make_shared(subnode, - std::dynamic_pointer_cast(uas_)->get_fully_qualified_name(), options)) - {} + UASPtr uas, const std::string & subnode_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~Plugin() = default; diff --git a/mavros/src/lib/plugin.cpp b/mavros/src/lib/plugin.cpp index bb8dd1923..2f358e6ec 100644 --- a/mavros/src/lib/plugin.cpp +++ b/mavros/src/lib/plugin.cpp @@ -16,7 +16,16 @@ #include "mavros/mavros_uas.hpp" #include "mavros/plugin.hpp" -using mavros::plugin::Plugin; +using mavros::plugin::Plugin; + +Plugin::Plugin( + UASPtr uas_, const std::string & subnode, + const rclcpp::NodeOptions & options) +: uas(uas_), + // node(std::dynamic_pointer_cast(uas_)->create_sub_node(subnode)) // https://github.com/ros2/rclcpp/issues/731 + node(rclcpp::Node::make_shared(subnode, + uas_->get_fully_qualified_name(), options)) +{} void Plugin::enable_connection_cb() { diff --git a/mavros/test/test_uas.cpp b/mavros/test/test_uas.cpp index f207f5eba..a6d1edd5a 100644 --- a/mavros/test/test_uas.cpp +++ b/mavros/test/test_uas.cpp @@ -62,7 +62,7 @@ class MockPlugin : public plugin::Plugin using SharedPtr = std::shared_ptr; explicit MockPlugin(UAS::SharedPtr uas_) - : Plugin(uas_) {} + : Plugin(uas_, "mock_plugin") {} MOCK_METHOD0(get_subscriptions, plugin::Plugin::Subscriptions(void)); From 1bfdd5a044f064a05ee3201b1bd5123da4c8f49e Mon Sep 17 00:00:00 2001 From: ugol-1 Date: Tue, 25 Jun 2024 20:27:24 +0000 Subject: [PATCH 3/5] Fixed unresolved symbol runtime error due to not linking to yaml-cpp properly --- mavros_extras/CMakeLists.txt | 11 +++++++---- mavros_extras/package.xml | 1 - 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index dc0fd0cbb..a2bd0ea3b 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -32,8 +32,7 @@ find_package(libmavconn REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -# find_package(yaml_cpp REQUIRED) -find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) ## Find GeographicLib # Append to CMAKE_MODULE_PATH since debian/ubuntu installs @@ -153,7 +152,9 @@ ament_target_dependencies(mavros_extras_plugins tf2_eigen message_filters Eigen3 - yaml_cpp_vendor +) +target_link_libraries(mavros_extras_plugins + yaml-cpp ) pluginlib_export_plugin_description_file(mavros mavros_plugins.xml) @@ -171,9 +172,11 @@ ament_target_dependencies(mavros_extras sensor_msgs mavros_msgs #console_bridge - yaml_cpp_vendor urdf ) +target_link_libraries(mavros_extras + yaml-cpp +) rclcpp_components_register_node(mavros_extras PLUGIN "mavros::extras::ServoStatePublisher" EXECUTABLE servo_state_publisher) install(TARGETS mavros_extras mavros_extras_plugins diff --git a/mavros_extras/package.xml b/mavros_extras/package.xml index eeb0d29f5..4f2770c68 100644 --- a/mavros_extras/package.xml +++ b/mavros_extras/package.xml @@ -49,7 +49,6 @@ rcpputils urdf yaml-cpp - yaml_cpp_vendor diagnostic_msgs From ff8b485cdd76fdd26e0e63ea152a4d2c84cbf9c9 Mon Sep 17 00:00:00 2001 From: ugol-1 Date: Tue, 25 Jun 2024 20:27:30 +0000 Subject: [PATCH 4/5] Break circular dependencies by using raw pointers to UAS in plugins --- mavros/include/mavros/plugin.hpp | 2 +- mavros/include/mavros/plugin_filter.hpp | 2 +- mavros/src/lib/mavros_uas.cpp | 4 ++-- mavros/src/plugins/home_position.cpp | 2 +- mavros/test/test_uas.cpp | 6 +++--- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mavros/include/mavros/plugin.hpp b/mavros/include/mavros/plugin.hpp index 17725ae0f..37fda8025 100644 --- a/mavros/include/mavros/plugin.hpp +++ b/mavros/include/mavros/plugin.hpp @@ -44,7 +44,7 @@ namespace plugin { using mavros::uas::UAS; -using UASPtr = std::shared_ptr; +using UASPtr = UAS *; using r_unique_lock = std::unique_lock; using s_unique_lock = std::unique_lock; using s_shared_lock = std::shared_lock; diff --git a/mavros/include/mavros/plugin_filter.hpp b/mavros/include/mavros/plugin_filter.hpp index 3d3b28a17..b89e0192c 100644 --- a/mavros/include/mavros/plugin_filter.hpp +++ b/mavros/include/mavros/plugin_filter.hpp @@ -32,7 +32,7 @@ namespace filter { using mavros::plugin::Filter; using mavros::uas::UAS; -using UASPtr = UAS::SharedPtr; +using UASPtr = UAS *; using mavconn::Framing; diff --git a/mavros/src/lib/mavros_uas.cpp b/mavros/src/lib/mavros_uas.cpp index e185df6a4..424d92404 100644 --- a/mavros/src/lib/mavros_uas.cpp +++ b/mavros/src/lib/mavros_uas.cpp @@ -84,7 +84,7 @@ UAS::UAS( this->declare_parameter("odom_frame_id", odom_frame_id); this->declare_parameter("map_frame_id", map_frame_id); - // NOTE(vooon): we couldn't add_plugin() in constructor because it needs shared_from_this() + // 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(); @@ -264,7 +264,7 @@ plugin::Plugin::SharedPtr UAS::create_plugin_instance(const std::string & pl_nam auto plugin_factory = plugin_factory_loader.createSharedInstance(pl_name); return - plugin_factory->create_plugin_instance(std::static_pointer_cast(shared_from_this())); + plugin_factory->create_plugin_instance(this); } void UAS::add_plugin(const std::string & pl_name) diff --git a/mavros/src/plugins/home_position.cpp b/mavros/src/plugins/home_position.cpp index 57325fe60..81ab88c61 100644 --- a/mavros/src/plugins/home_position.cpp +++ b/mavros/src/plugins/home_position.cpp @@ -130,7 +130,7 @@ class HomePositionPlugin : public plugin::Plugin hp.geo.latitude = home_position.latitude / 1E7; // deg hp.geo.longitude = home_position.longitude / 1E7; // deg hp.geo.altitude = home_position.altitude / 1E3 + - uas->data.geoid_to_ellipsoid_height(hp.geo); // in meters + uas->data.geoid_to_ellipsoid_height(hp.geo); // in meters hp.orientation = tf2::toMsg(q); hp.position = tf2::toMsg(pos); tf2::toMsg(hp_approach_enu, hp.approach); diff --git a/mavros/test/test_uas.cpp b/mavros/test/test_uas.cpp index a6d1edd5a..e23f378cc 100644 --- a/mavros/test/test_uas.cpp +++ b/mavros/test/test_uas.cpp @@ -61,7 +61,7 @@ class MockPlugin : public plugin::Plugin public: using SharedPtr = std::shared_ptr; - explicit MockPlugin(UAS::SharedPtr uas_) + explicit MockPlugin(UASPtr uas_) : Plugin(uas_, "mock_plugin") {} MOCK_METHOD0(get_subscriptions, plugin::Plugin::Subscriptions(void)); @@ -208,9 +208,9 @@ TEST_F(TestUAS, is_plugin_allowed) TEST_F(TestUAS, add_plugin__route_message__filter) { auto uas = create_node(); - auto plugin1 = std::make_shared(uas); + auto plugin1 = std::make_shared(uas.get()); auto subs1 = plugin1->allsubs(); - auto plugin2 = std::make_shared(uas); + auto plugin2 = std::make_shared(uas.get()); auto subs2 = plugin2->rawsubs(); // XXX(vooon): silence leak warnings: they work badly with shared_ptr From fe0e21d7f8ef4be027984e09988c094674eb2ab3 Mon Sep 17 00:00:00 2001 From: ugol-1 Date: Tue, 25 Jun 2024 20:27:35 +0000 Subject: [PATCH 5/5] Clean shutdown of UAS node Prohibit copying and moving of UAS Removed startup_delay_timer --- mavros/include/mavros/mavros_uas.hpp | 19 ++- mavros/src/lib/mavros_uas.cpp | 207 +++++++++++++-------------- mavros/test/test_uas.cpp | 1 - 3 files changed, 116 insertions(+), 111 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.hpp b/mavros/include/mavros/mavros_uas.hpp index fb24c25db..a44a38756 100644 --- a/mavros/include/mavros/mavros_uas.hpp +++ b/mavros/include/mavros/mavros_uas.hpp @@ -251,6 +251,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; /** @@ -580,11 +590,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>; - thread_ptr exec_spin_thd; // rclcpp::executors::MultiThreadedExecutor exec; UASExecutor exec; @@ -616,6 +622,11 @@ class UAS : public rclcpp::Node rclcpp::Subscription::SharedPtr source; // FCU -> UAS rclcpp::Publisher::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>; + thread_ptr exec_spin_thd; + //! initialize connection to the Router void connect_to_router(); diff --git a/mavros/src/lib/mavros_uas.cpp b/mavros/src/lib/mavros_uas.cpp index 424d92404..e609ff148 100644 --- a/mavros/src/lib/mavros_uas.cpp +++ b/mavros/src/lib/mavros_uas.cpp @@ -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 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 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; }); } diff --git a/mavros/test/test_uas.cpp b/mavros/test/test_uas.cpp index e23f378cc..539fffd0c 100644 --- a/mavros/test/test_uas.cpp +++ b/mavros/test/test_uas.cpp @@ -127,7 +127,6 @@ class TestUAS : public ::testing::Test MockUAS::SharedPtr create_node() { auto uas = std::make_shared("test_mavros_uas"); - uas->startup_delay_timer->cancel(); return uas; }