From be607f26a377e548c413539c51a95540cbe418da Mon Sep 17 00:00:00 2001 From: Milosz Lagan Date: Thu, 7 Nov 2024 21:58:48 +0000 Subject: [PATCH 1/4] Generate node parameters from yaml files --- husarion_ugv_battery/CMakeLists.txt | 25 ++++- husarion_ugv_battery/README.md | 4 + .../config/battery_parameters.yaml | 52 +++++++++++ .../battery_driver_node.hpp | 5 + .../battery_publisher/battery_publisher.hpp | 3 +- .../dual_battery_publisher.hpp | 3 +- .../single_battery_publisher.hpp | 2 +- husarion_ugv_battery/package.xml | 1 + .../src/battery_driver_node.cpp | 37 ++++---- .../battery_publisher/battery_publisher.cpp | 10 +- .../dual_battery_publisher.cpp | 5 +- .../single_battery_publisher.cpp | 5 +- .../test_battery_publisher.cpp | 5 +- .../test_dual_battery_publisher.cpp | 5 +- .../test_single_battery_publisher.cpp | 4 +- husarion_ugv_lights/CMakeLists.txt | 15 ++- husarion_ugv_lights/README.md | 5 +- .../config/lights_controller_parameters.yaml | 17 ++++ .../config/lights_driver_parameters.yaml | 24 +++++ .../lights_controller_node.hpp | 5 + .../lights_driver_node.hpp | 5 + husarion_ugv_lights/package.xml | 1 + .../src/lights_controller_node.cpp | 14 +-- .../src/lights_driver_node.cpp | 15 ++- husarion_ugv_manager/CMakeLists.txt | 21 ++++- husarion_ugv_manager/README.md | 2 + .../config/lights_manager_parameters.yaml | 75 +++++++++++++++ .../config/safety_manager_parameters.yaml | 93 +++++++++++++++++++ .../lights_manager_node.hpp | 5 + .../safety_manager_node.hpp | 5 + husarion_ugv_manager/package.xml | 1 + .../src/lights_manager_node.cpp | 58 +++--------- .../src/safety_manager_node.cpp | 62 ++++--------- 33 files changed, 444 insertions(+), 145 deletions(-) create mode 100644 husarion_ugv_battery/config/battery_parameters.yaml create mode 100644 husarion_ugv_lights/config/lights_controller_parameters.yaml create mode 100644 husarion_ugv_lights/config/lights_driver_parameters.yaml create mode 100644 husarion_ugv_manager/config/lights_manager_parameters.yaml create mode 100644 husarion_ugv_manager/config/safety_manager_parameters.yaml diff --git a/husarion_ugv_battery/CMakeLists.txt b/husarion_ugv_battery/CMakeLists.txt index d928de57..a3dc0c8d 100644 --- a/husarion_ugv_battery/CMakeLists.txt +++ b/husarion_ugv_battery/CMakeLists.txt @@ -5,8 +5,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater panther_msgs - husarion_ugv_utils rclcpp sensor_msgs) +set(PACKAGE_DEPENDENCIES + ament_cmake + diagnostic_updater + generate_parameter_library + panther_msgs + husarion_ugv_utils + rclcpp + sensor_msgs) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) @@ -25,6 +31,9 @@ add_executable( src/battery_publisher/single_battery_publisher.cpp) ament_target_dependencies(battery_driver_node ${PACKAGE_DEPENDENCIES}) +generate_parameter_library(battery_parameters config/battery_parameters.yaml) +target_link_libraries(battery_driver_node battery_parameters) + install(TARGETS battery_driver_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) @@ -76,6 +85,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_publisher ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_publisher + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher @@ -89,6 +100,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_single_battery_publisher ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_single_battery_publisher + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher @@ -102,6 +115,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_dual_battery_publisher ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_dual_battery_publisher + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_battery_driver_node_adc_dual @@ -118,6 +133,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_dual ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_dual + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_battery_driver_node_adc_single @@ -134,6 +151,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_single ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_adc_single + battery_parameters) ament_add_gtest( ${PROJECT_NAME}_test_battery_driver_node_roboteq @@ -150,6 +169,8 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_roboteq ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_battery_driver_node_roboteq + battery_parameters) endif() diff --git a/husarion_ugv_battery/README.md b/husarion_ugv_battery/README.md index a6fcc35f..e952f05d 100644 --- a/husarion_ugv_battery/README.md +++ b/husarion_ugv_battery/README.md @@ -8,6 +8,10 @@ This package contains: - `battery.launch.py`: Responsible for activating battery node, which dealing with reading and publishing battery data. +## Configuration Files + +- [`battery_parameters.yaml`](./config/battery_parameters.yaml): Defines parameters for `battery_driver_node`. + ## ROS Nodes ### battery_driver_node diff --git a/husarion_ugv_battery/config/battery_parameters.yaml b/husarion_ugv_battery/config/battery_parameters.yaml new file mode 100644 index 00000000..ab09541c --- /dev/null +++ b/husarion_ugv_battery/config/battery_parameters.yaml @@ -0,0 +1,52 @@ +battery: + adc: + device0: + type: string + default_value: "/sys/bus/iio/devices/iio:device0" + description: "Internal ADC0 IIO device." + validation: { not_empty<> } + + device1: + type: string + default_value: "/sys/bus/iio/devices/iio:device1" + description: "Internal ADC1 IIO device." + validation: { not_empty<> } + + ma_window_len: + charge: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery charge readings." + validation: { gt<>: 0 } + + temp: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery temperature readings." + validation: { gt<>: 0 } + + ma_window_len: + voltage: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery voltage readings." + validation: { gt<>: 0 } + + current: + type: int + default_value: 10 + description: "Window length of a moving average, used to smooth out battery current readings." + validation: { gt<>: 0 } + + roboteq: + driver_state_timeout: + type: double + default_value: 0.2 + description: "Timeout in seconds after which driver state messages will be considered old. Used as a fallback when ADC data is not available." + validation: { gt<>: 0.0 } + + battery_timeout: + type: double + default_value: 1.0 + description: "Timeout in seconds. If the node fails to read battery data exceeding this duration, the node will publish an unknown battery state." + validation: { gt<>: 0.0 } diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp index 1bba8b99..ca113540 100644 --- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_driver_node.hpp @@ -23,6 +23,8 @@ #include "panther_msgs/msg/robot_driver_state.hpp" +#include "battery_parameters.hpp" + #include "husarion_ugv_battery/adc_data_reader.hpp" #include "husarion_ugv_battery/battery/battery.hpp" #include "husarion_ugv_battery/battery_publisher/battery_publisher.hpp" @@ -54,6 +56,9 @@ class BatteryDriverNode : public rclcpp::Node std::shared_ptr battery_2_; std::shared_ptr battery_publisher_; + std::shared_ptr param_listener_; + battery::Params params_; + rclcpp::Subscription::SharedPtr driver_state_sub_; rclcpp::TimerBase::SharedPtr battery_pub_timer_; diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp index 9258b9a5..29e52609 100644 --- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/battery_publisher.hpp @@ -37,7 +37,8 @@ class BatteryPublisher public: BatteryPublisher( const rclcpp::Node::SharedPtr & node, - const std::shared_ptr & diagnostic_updater); + const std::shared_ptr & diagnostic_updater, + const double battery_timeout); ~BatteryPublisher() {} diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp index 738d7766..4200c621 100644 --- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/dual_battery_publisher.hpp @@ -32,7 +32,8 @@ class DualBatteryPublisher : public BatteryPublisher DualBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery_1, const std::shared_ptr & battery_2); + const double battery_timeout, const std::shared_ptr & battery_1, + const std::shared_ptr & battery_2); ~DualBatteryPublisher() {} diff --git a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp index f1e65ecf..1b68e2e9 100644 --- a/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp +++ b/husarion_ugv_battery/include/husarion_ugv_battery/battery_publisher/single_battery_publisher.hpp @@ -31,7 +31,7 @@ class SingleBatteryPublisher : public BatteryPublisher SingleBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery); + const double battery_timeout, const std::shared_ptr & battery); ~SingleBatteryPublisher() {} diff --git a/husarion_ugv_battery/package.xml b/husarion_ugv_battery/package.xml index 70f025d7..512b2323 100644 --- a/husarion_ugv_battery/package.xml +++ b/husarion_ugv_battery/package.xml @@ -17,6 +17,7 @@ ament_cmake diagnostic_updater + generate_parameter_library husarion_ugv_utils panther_msgs rclcpp diff --git a/husarion_ugv_battery/src/battery_driver_node.cpp b/husarion_ugv_battery/src/battery_driver_node.cpp index 9ec16665..db6a063b 100644 --- a/husarion_ugv_battery/src/battery_driver_node.cpp +++ b/husarion_ugv_battery/src/battery_driver_node.cpp @@ -41,8 +41,9 @@ BatteryDriverNode::BatteryDriverNode( { RCLCPP_INFO(this->get_logger(), "Constructing node."); - this->declare_parameter("ma_window_len/voltage", 10); - this->declare_parameter("ma_window_len/current", 10); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); // Running at 10 Hz battery_pub_timer_ = this->create_wall_timer( @@ -75,22 +76,17 @@ void BatteryDriverNode::InitializeWithADCBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with ADC data."); - this->declare_parameter("adc/device0", "/sys/bus/iio/devices/iio:device0"); - this->declare_parameter("adc/device1", "/sys/bus/iio/devices/iio:device1"); - this->declare_parameter("adc/ma_window_len/temp", 10); - this->declare_parameter("adc/ma_window_len/charge", 10); - - const std::string adc0_device_path = this->get_parameter("adc/device0").as_string(); - const std::string adc1_device_path = this->get_parameter("adc/device1").as_string(); + const std::string adc0_device_path = this->params_.adc.device0; + const std::string adc1_device_path = this->params_.adc.device1; adc0_reader_ = std::make_shared(adc0_device_path); adc1_reader_ = std::make_shared(adc1_device_path); const ADCBatteryParams battery_params = { - static_cast(this->get_parameter("ma_window_len/voltage").as_int()), - static_cast(this->get_parameter("ma_window_len/current").as_int()), - static_cast(this->get_parameter("adc/ma_window_len/temp").as_int()), - static_cast(this->get_parameter("adc/ma_window_len/charge").as_int()), + static_cast(this->params_.ma_window_len.voltage), + static_cast(this->params_.ma_window_len.current), + static_cast(this->params_.adc.ma_window_len.temp), + static_cast(this->params_.adc.ma_window_len.charge), }; battery_2_ = std::make_shared( @@ -106,7 +102,8 @@ void BatteryDriverNode::InitializeWithADCBattery() std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0), std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params); battery_publisher_ = std::make_shared( - this->shared_from_this(), diagnostic_updater_, battery_1_, battery_2_); + this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_, + battery_2_); } else { battery_2_.reset(); battery_1_ = std::make_shared( @@ -118,7 +115,7 @@ void BatteryDriverNode::InitializeWithADCBattery() std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 1, 0), std::bind(&ADCDataReader::GetADCMeasurement, *adc0_reader_, 3, 0), battery_params); battery_publisher_ = std::make_shared( - this->shared_from_this(), diagnostic_updater_, battery_1_); + this->shared_from_this(), diagnostic_updater_, params_.battery_timeout, battery_1_); } RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data."); @@ -128,12 +125,10 @@ void BatteryDriverNode::InitializeWithRoboteqBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data."); - this->declare_parameter("roboteq/driver_state_timeout", 0.2); - const RoboteqBatteryParams battery_params = { - static_cast(this->get_parameter("roboteq/driver_state_timeout").as_double()), - static_cast(this->get_parameter("ma_window_len/voltage").as_int()), - static_cast(this->get_parameter("ma_window_len/current").as_int()), + static_cast(this->params_.roboteq.driver_state_timeout), + static_cast(this->params_.ma_window_len.voltage), + static_cast(this->params_.ma_window_len.current), }; driver_state_sub_ = this->create_subscription( @@ -143,7 +138,7 @@ void BatteryDriverNode::InitializeWithRoboteqBattery() battery_1_ = std::make_shared([&]() { return driver_state_; }, battery_params); battery_publisher_ = std::make_shared( - this->shared_from_this(), diagnostic_updater_, battery_1_); + this->shared_from_this(), diagnostic_updater_, this->params_.battery_timeout, battery_1_); RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data."); } diff --git a/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp index e37fcbfe..6f2ef870 100644 --- a/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp +++ b/husarion_ugv_battery/src/battery_publisher/battery_publisher.cpp @@ -26,12 +26,12 @@ namespace husarion_ugv_battery BatteryPublisher::BatteryPublisher( const rclcpp::Node::SharedPtr & node, - const std::shared_ptr & diagnostic_updater) -: node_(std::move(node)), diagnostic_updater_(std::move(diagnostic_updater)) + const std::shared_ptr & diagnostic_updater, + const double battery_timeout) +: node_(std::move(node)), + diagnostic_updater_(std::move(diagnostic_updater)), + battery_timeout_(battery_timeout) { - node->declare_parameter("battery_timeout", 1.0); - battery_timeout_ = node->get_parameter("battery_timeout").as_double(); - charger_connected_ = false; last_battery_info_time_ = rclcpp::Time(std::int64_t(0), RCL_ROS_TIME); diff --git a/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp index f474b1fe..1e345280 100644 --- a/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp +++ b/husarion_ugv_battery/src/battery_publisher/dual_battery_publisher.cpp @@ -34,8 +34,9 @@ namespace husarion_ugv_battery DualBatteryPublisher::DualBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery_1, const std::shared_ptr & battery_2) -: BatteryPublisher(std::move(node), std::move(diagnostic_updater)), + const double battery_timeout, const std::shared_ptr & battery_1, + const std::shared_ptr & battery_2) +: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout), battery_1_(std::move(battery_1)), battery_2_(std::move(battery_2)) { diff --git a/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp index 92e4d003..4fff8f68 100644 --- a/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp +++ b/husarion_ugv_battery/src/battery_publisher/single_battery_publisher.cpp @@ -32,8 +32,9 @@ namespace husarion_ugv_battery SingleBatteryPublisher::SingleBatteryPublisher( const rclcpp::Node::SharedPtr & node, const std::shared_ptr & diagnostic_updater, - const std::shared_ptr & battery) -: BatteryPublisher(std::move(node), std::move(diagnostic_updater)), battery_(std::move(battery)) + const double battery_timeout, const std::shared_ptr & battery) +: BatteryPublisher(std::move(node), std::move(diagnostic_updater), battery_timeout), + battery_(std::move(battery)) { battery_pub_ = node->create_publisher("battery/battery_status", 5); battery_1_pub_ = node->create_publisher("_battery/battery_1_status_raw", 5); diff --git a/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp index f5924c73..ff2fe64a 100644 --- a/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp +++ b/husarion_ugv_battery/test/battery_publisher/test_battery_publisher.cpp @@ -35,7 +35,7 @@ class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher BatteryPublisherWrapper( const rclcpp::Node::SharedPtr & node, std::shared_ptr diagnostic_updater) - : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater) + : husarion_ugv_battery::BatteryPublisher(node, diagnostic_updater, kBatteryTimeout) { } @@ -62,6 +62,9 @@ class BatteryPublisherWrapper : public husarion_ugv_battery::BatteryPublisher { status.summary(0, ""); // Avoid unused parameter compiler warning }; + +protected: + static constexpr double kBatteryTimeout = 0.2; }; class TestBatteryPublisher : public testing::Test diff --git a/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp index 74bcb358..b14ecf63 100644 --- a/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp +++ b/husarion_ugv_battery/test/battery_publisher/test_dual_battery_publisher.cpp @@ -40,7 +40,7 @@ class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPubl std::shared_ptr diagnostic_updater, std::shared_ptr & battery_1, std::shared_ptr & battery_2) - : DualBatteryPublisher(node, diagnostic_updater, battery_1, battery_2) + : DualBatteryPublisher(node, diagnostic_updater, kBatteryTimeout, battery_1, battery_2) { } @@ -71,6 +71,9 @@ class DualBatteryPublisherWrapper : public husarion_ugv_battery::DualBatteryPubl return DualBatteryPublisher::MergeChargingStatusMsgs( charging_status_msg_1, charging_status_msg_2); } + +private: + static constexpr double kBatteryTimeout = 0.2; }; class TestDualBatteryPublisher : public testing::Test diff --git a/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp index 3d98a9a7..2afd2a37 100644 --- a/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp +++ b/husarion_ugv_battery/test/battery_publisher/test_single_battery_publisher.cpp @@ -44,6 +44,8 @@ class TestSingleBatteryPublisher : public testing::Test std::shared_ptr battery_publisher_; BatteryStateMsg::SharedPtr battery_state_; BatteryStateMsg::SharedPtr battery_1_state_; + + static constexpr double kBatteryTimeout = 0.2f; }; TestSingleBatteryPublisher::TestSingleBatteryPublisher() @@ -62,7 +64,7 @@ TestSingleBatteryPublisher::TestSingleBatteryPublisher() "/_battery/battery_1_status_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; }); battery_publisher_ = std::make_shared( - node_, diagnostic_updater_, battery_); + node_, diagnostic_updater_, kBatteryTimeout, battery_); } TEST_F(TestSingleBatteryPublisher, CorrectTopicPublished) diff --git a/husarion_ugv_lights/CMakeLists.txt b/husarion_ugv_lights/CMakeLists.txt index 3fc9de63..2631fc5e 100644 --- a/husarion_ugv_lights/CMakeLists.txt +++ b/husarion_ugv_lights/CMakeLists.txt @@ -8,6 +8,7 @@ endif() set(PACKAGE_DEPENDENCIES ament_cmake diagnostic_updater + generate_parameter_library image_transport panther_msgs husarion_ugv_utils @@ -43,6 +44,10 @@ ament_target_dependencies( sensor_msgs std_srvs) +generate_parameter_library(lights_driver_parameters + config/lights_driver_parameters.yaml) +target_link_libraries(lights_driver_node_component lights_driver_parameters) + add_library( lights_controller_node_component SHARED src/lights_controller_node.cpp src/led_components/led_segment.cpp @@ -56,7 +61,11 @@ ament_target_dependencies( rclcpp rclcpp_components sensor_msgs) -target_link_libraries(lights_controller_node_component yaml-cpp) + +generate_parameter_library(lights_controller_parameters + config/lights_controller_parameters.yaml) +target_link_libraries(lights_controller_node_component yaml-cpp + lights_controller_parameters) rclcpp_components_register_node( lights_driver_node_component PLUGIN "husarion_ugv_lights::LightsDriverNode" @@ -68,7 +77,7 @@ rclcpp_components_register_node( ament_export_targets(export_lights_driver_node_component) install( - TARGETS lights_driver_node_component + TARGETS lights_driver_node_component lights_driver_parameters EXPORT export_lights_driver_node_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -76,7 +85,7 @@ install( ament_export_targets(export_lights_controller_node_component) install( - TARGETS lights_controller_node_component + TARGETS lights_controller_node_component lights_controller_parameters EXPORT export_lights_controller_node_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/husarion_ugv_lights/README.md b/husarion_ugv_lights/README.md index 50a8c4e6..f53bf287 100644 --- a/husarion_ugv_lights/README.md +++ b/husarion_ugv_lights/README.md @@ -12,6 +12,8 @@ This package contains: - [`{robot_model}_animations.yaml`](./config): Defines and describes the appearance and parameters of the animations for specific robot. - [`{robot_model}_driver.yaml`](./config): Defines and describes specific hardware configuration for specific robot. +- [`lights_controller_parameters.yaml`](./config/lights_controller_parameters.yaml): Defines parameters for `lights_controller_node`. +- [`lights_driver_parameters.yaml`](./config/lights_driver_parameters.yaml): Defines parameters for `lights_driver_node`. ## ROS Nodes @@ -59,4 +61,5 @@ This node is of type rclcpp_components is responsible for displaying frames on t - `~frame_timeout` [*float*, default: **0.1**]: Time in **[s]** after which an incoming frame will be considered too old. - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. The range between **[0.0, 1.0]**. -- `~num_led` [*int*, default: **46**]: Number of LEDs in a single bumper. +- `~channel_1_num_led` [*int*, default: **46**]: Number of LEDs in the first bumper. +- `~channel_2_num_led` [*int*, default: **46**]: Number of LEDs in the second bumper. diff --git a/husarion_ugv_lights/config/lights_controller_parameters.yaml b/husarion_ugv_lights/config/lights_controller_parameters.yaml new file mode 100644 index 00000000..c4b90ab0 --- /dev/null +++ b/husarion_ugv_lights/config/lights_controller_parameters.yaml @@ -0,0 +1,17 @@ +lights_controller: + animations_config_path: + type: string + default_value: "" + description: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. + validation: { not_empty<> } + + controller_frequency: + type: double + default_value: 50.0 + description: Frequency in Hz at which the lights controller node will process animations. + validation: { gt<>: 0.0 } + + user_led_animations_path: + type: string + default_value: "" + description: Path to a YAML file with a description of the user defined animations. diff --git a/husarion_ugv_lights/config/lights_driver_parameters.yaml b/husarion_ugv_lights/config/lights_driver_parameters.yaml new file mode 100644 index 00000000..87e89030 --- /dev/null +++ b/husarion_ugv_lights/config/lights_driver_parameters.yaml @@ -0,0 +1,24 @@ +lights_driver: + frame_timeout: + type: double + default_value: 0.1 + description: Time in seconds after which an incoming frame will be considered too old. + validation: { gt<>: 0.0 } + + global_brightness: + type: double + default_value: 1.0 + description: LED global brightness. The range should be between [0.0, 1.0]. + validation: { bounds<>: [0.0, 1.0] } + + channel_1_num_led: + type: int + default_value: 46 + description: Number of LEDs in the first bumper. + validation: { gt<>: 0 } + + channel_2_num_led: + type: int + default_value: 46 + description: Number of LEDs in the second bumper. + validation: { gt<>: 0 } diff --git a/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp index fc7b61cc..fd01c3ab 100644 --- a/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_controller_node.hpp @@ -27,6 +27,8 @@ #include "panther_msgs/srv/set_led_animation.hpp" +#include "lights_controller_parameters.hpp" + #include "husarion_ugv_lights/animation/animation.hpp" #include "husarion_ugv_lights/led_components/led_animations_queue.hpp" #include "husarion_ugv_lights/led_components/segment_converter.hpp" @@ -148,6 +150,9 @@ class LightsControllerNode : public rclcpp::Node std::unordered_map animations_descriptions_; std::shared_ptr segment_converter_; + std::shared_ptr param_listener_; + lights_controller::Params params_; + rclcpp::Service::SharedPtr set_led_animation_server_; rclcpp::TimerBase::SharedPtr controller_timer_; diff --git a/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp index 0a5de4ae..0f015c6c 100644 --- a/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp +++ b/husarion_ugv_lights/include/husarion_ugv_lights/lights_driver_node.hpp @@ -26,6 +26,8 @@ #include "panther_msgs/srv/set_led_brightness.hpp" +#include "lights_driver_parameters.hpp" + #include "husarion_ugv_lights/apa102.hpp" namespace husarion_ugv_lights @@ -120,6 +122,9 @@ class LightsDriverNode : public rclcpp::Node APA102Interface::SharedPtr channel_1_; APA102Interface::SharedPtr channel_2_; + std::shared_ptr param_listener_; + lights_driver::Params params_; + rclcpp::TimerBase::SharedPtr initialization_timer_; rclcpp::Client::SharedPtr enable_led_control_client_; diff --git a/husarion_ugv_lights/package.xml b/husarion_ugv_lights/package.xml index 8e68c10c..116e40c4 100644 --- a/husarion_ugv_lights/package.xml +++ b/husarion_ugv_lights/package.xml @@ -17,6 +17,7 @@ ament_cmake diagnostic_updater + generate_parameter_library husarion_ugv_utils image_transport libpng-dev diff --git a/husarion_ugv_lights/src/lights_controller_node.cpp b/husarion_ugv_lights/src/lights_controller_node.cpp index b54cf25d..e1d51224 100644 --- a/husarion_ugv_lights/src/lights_controller_node.cpp +++ b/husarion_ugv_lights/src/lights_controller_node.cpp @@ -31,6 +31,8 @@ #include "panther_msgs/srv/set_led_animation.hpp" +#include "lights_controller_parameters.hpp" + #include "husarion_ugv_lights/led_components/led_animations_queue.hpp" #include "husarion_ugv_lights/led_components/led_panel.hpp" #include "husarion_ugv_lights/led_components/led_segment.hpp" @@ -48,13 +50,13 @@ LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options) using namespace std::placeholders; - this->declare_parameter("animations_config_path"); - this->declare_parameter("user_led_animations_path", ""); - this->declare_parameter("controller_freq", 50.0); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - const auto animations_config_path = this->get_parameter("animations_config_path").as_string(); - const auto user_led_animations_path = this->get_parameter("user_led_animations_path").as_string(); - const float controller_freq = this->get_parameter("controller_freq").as_double(); + const auto animations_config_path = this->params_.animations_config_path; + const auto user_led_animations_path = this->params_.user_led_animations_path; + const float controller_freq = this->params_.controller_frequency; YAML::Node led_config_desc = YAML::LoadFile(animations_config_path); diff --git a/husarion_ugv_lights/src/lights_driver_node.cpp b/husarion_ugv_lights/src/lights_driver_node.cpp index 62e89236..ad502100 100644 --- a/husarion_ugv_lights/src/lights_driver_node.cpp +++ b/husarion_ugv_lights/src/lights_driver_node.cpp @@ -52,16 +52,15 @@ LightsDriverNode::LightsDriverNode(const rclcpp::NodeOptions & options) rclcpp::on_shutdown(std::bind(&LightsDriverNode::OnShutdown, this)); - this->declare_parameter("global_brightness", 1.0); - this->declare_parameter("frame_timeout", 0.1); - this->declare_parameter("channel_1_num_led", 46); - this->declare_parameter("channel_2_num_led", 46); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - frame_timeout_ = this->get_parameter("frame_timeout").as_double(); - channel_1_num_led_ = this->get_parameter("channel_1_num_led").as_int(); - channel_2_num_led_ = this->get_parameter("channel_2_num_led").as_int(); + frame_timeout_ = this->params_.frame_timeout; + channel_1_num_led_ = this->params_.channel_1_num_led; + channel_2_num_led_ = this->params_.channel_2_num_led; - const float global_brightness = this->get_parameter("global_brightness").as_double(); + const float global_brightness = this->params_.global_brightness; channel_1_->SetGlobalBrightness(global_brightness); channel_2_->SetGlobalBrightness(global_brightness); diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt index 29286611..82f9b528 100644 --- a/husarion_ugv_manager/CMakeLists.txt +++ b/husarion_ugv_manager/CMakeLists.txt @@ -10,6 +10,7 @@ set(PACKAGE_DEPENDENCIES ament_index_cpp behaviortree_cpp behaviortree_ros2 + generate_parameter_library libssh panther_msgs husarion_ugv_utils @@ -71,7 +72,11 @@ ament_target_dependencies( rclcpp sensor_msgs std_msgs) -target_link_libraries(safety_manager_node ${plugin_libs}) + +generate_parameter_library(safety_manager_parameters + config/safety_manager_parameters.yaml) +target_link_libraries(safety_manager_node ${plugin_libs} + safety_manager_parameters) add_executable( lights_manager_node src/lights_manager_node_main.cpp @@ -84,7 +89,11 @@ ament_target_dependencies( rclcpp sensor_msgs std_msgs) -target_link_libraries(lights_manager_node ${plugin_libs}) + +generate_parameter_library(lights_manager_parameters + config/lights_manager_parameters.yaml) +target_link_libraries(lights_manager_node ${plugin_libs} + lights_manager_parameters) install(TARGETS ${plugin_libs} DESTINATION lib) @@ -197,6 +206,8 @@ if(BUILD_TESTING) rclcpp sensor_msgs std_msgs) + target_link_libraries(${PROJECT_NAME}_test_lights_manager_node + lights_manager_parameters) ament_add_gtest( ${PROJECT_NAME}_test_lights_behavior_tree @@ -215,6 +226,8 @@ if(BUILD_TESTING) rclcpp sensor_msgs std_msgs) + target_link_libraries(${PROJECT_NAME}_test_lights_behavior_tree + lights_manager_parameters) ament_add_gtest( ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp @@ -232,6 +245,8 @@ if(BUILD_TESTING) rclcpp sensor_msgs std_msgs) + target_link_libraries(${PROJECT_NAME}_test_safety_manager_node + safety_manager_parameters) ament_add_gtest( ${PROJECT_NAME}_test_safety_behavior_tree @@ -251,6 +266,8 @@ if(BUILD_TESTING) sensor_msgs std_msgs std_srvs) + target_link_libraries(${PROJECT_NAME}_test_safety_behavior_tree + safety_manager_parameters) endif() ament_package() diff --git a/husarion_ugv_manager/README.md b/husarion_ugv_manager/README.md index b8a9795f..f43f5657 100644 --- a/husarion_ugv_manager/README.md +++ b/husarion_ugv_manager/README.md @@ -15,7 +15,9 @@ This package contains: - [`SafetyBT.btproj`](./behavior_trees/SafetyBT.btproj): BehaviorTree project for managing Panther safety protocols. - [`safety.xml`](./behavior_trees/safety.xml): BehaviorTree for monitoring and managing dangerous situations. - [`shutdown.xml`](./behavior_trees/shutdown.xml): BehaviorTree for initiating shutdown procedures. +- [`lights_manager_parameters.yaml`](./config/lights_manager_parameters.yaml): Defines parameters for the `lights_manager` node. - [`lights_manager.yaml`](./config/lights_manager.yaml): Contains parameters for the `lights_manager` node. +- [`safety_manager_parameters.yaml`](./config/safety_manager_parameters.yaml): Defines parameters for the `safety_manager` node. - [`safety_manager.yaml`](./config/safety_manager.yaml): Contains parameters for the `safety_manager` node. - [`shutdown_hosts.yaml`](./config/shutdown_hosts.yaml): List with all hosts to request shutdown. diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml new file mode 100644 index 00000000..e60a404c --- /dev/null +++ b/husarion_ugv_manager/config/lights_manager_parameters.yaml @@ -0,0 +1,75 @@ +lights_manager: + battery: + charging_anim_step: + type: double + default_value: 0.1 + description: This parameter defines the minimum change in battery percentage required to trigger an update in the battery charging animation. + validation: { gt<>: 0.0 } + + anim_period: + critical: + type: double + default_value: 15.0 + description: Time in seconds to wait before repeating animation, indicating a critical Battery state. + validation: { gt<>: 0.0 } + + low: + type: double + default_value: 30.0 + description: Time in seconds to wait before repeating the animation, indicating a low Battery state. + validation: { gt<>: 0.0 } + + percent: + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out Battery percentage readings. + validation: { gt<>: 0 } + + threshold: + critical: + type: double + default_value: 0.1 + description: If the Battery percentage drops below this value, an animation indicating a critical Battery state will start being displayed. + validation: { bounds<>: [0.0, 1.0] } + + low: + type: double + default_value: 0.4 + description: If the Battery percentage drops below this value, the animation indicating a low Battery state will start being displayed. + validation: { bounds<>: [0.0, 1.0] } + + bt_project_path: + type: string + default_value: "" + description: Path to a BehaviorTree project. + validation: { not_empty<> } + + plugin_libs: + type: string_array + default_value: [] + description: List with names of plugins that are used in the BT project. + + ros_communication_timeout: + availability: + type: double + default_value: 1.0 + description: Timeout in seconds to wait for a service/action while initializing BT node. + validation: { gt<>: 0.0 } + + response: + type: double + default_value: 1.0 + description: Timeout in seconds to receive a service/action response after call. + validation: { gt<>: 0.0 } + + ros_plugin_libs: + type: string_array + default_value: [] + description: List with names of ROS plugins that are used in a BT project. + + timer_frequency: + type: double + default_value: 10.0 + description: Frequency in Hz at which lights tree will be ticked. + validation: { gt<>: 0.0 } diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml new file mode 100644 index 00000000..4010ee12 --- /dev/null +++ b/husarion_ugv_manager/config/safety_manager_parameters.yaml @@ -0,0 +1,93 @@ +safety_manager: + battery: + temp: + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out temperature readings of the battery. + validation: { gt<>: 0 } + + bt_project_path: + type: string + default_value: "" + description: Path to a BehaviorTree project. + validation: { not_empty<> } + + cpu: + temp: + fan_off: + type: double + default_value: 60.0 + description: Temperature in degrees Celsius of the Built-in Computer's CPU, below which the fan is turned off. + validation: { bounds<>: [-20.0, 100.0] } + + fan_on: + type: double + default_value: 70.0 + description: Temperature in degrees Celsius of the Built-in Computer's CPU, above which the fan is turned on. + validation: { bounds<>: [-20.0, 100.0] } + + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out temperature readings of the Built-in Computer's CPU. + validation: { gt<>: 0 } + + driver: + temp: + fan_off: + type: double + default_value: 35.0 + description: Temperature in degrees Celsius of any drivers below which the fan is turned off. + validation: { bounds<>: [-20.0, 100.0] } + + fan_on: + type: double + default_value: 45.0 + description: Temperature in degrees Celsius* of any drivers above which the fan is turned on. + validation: { bounds<>: [-20.0, 100.0] } + + window_len: + type: int + default_value: 6 + description: Moving average window length used to smooth out the temperature readings of each driver. + validation: { gt<>: 0 } + + fan_turn_off_timeout: + type: double + default_value: 60.0 + description: Minimal time in seconds, after which the fan may be turned off. + validation: { gt<>: 0.0 } + + plugin_libs: + type: string_array + default_value: [] + description: List with names of plugins that are used in the BT project. + + ros_communication_timeout: + availability: + type: double + default_value: 1.0 + description: Timeout seconds to wait for a service/action while initializing BT node. + validation: { gt<>: 0.0 } + + response: + type: double + default_value: 1.0 + description: Timeout seconds to receive a service/action response after call. + validation: { gt<>: 0.0 } + + ros_plugin_libs: + type: string_array + default_value: [] + description: List with names of ROS plugins that are used in a BT project. + + shutdown_hosts_path: + type: string + default_value: "" + description: Path to a YAML file containing a list of hosts to request shutdown. To correctly format the YAML file, consult the README. + + timer_frequency: + type: double + default_value: 10.0 + description: Frequency in Hz at which safety tree will be ticked. diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp index 6fd54433..655e19e7 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp @@ -26,6 +26,8 @@ #include "panther_msgs/msg/led_animation.hpp" +#include "lights_manager_parameters.hpp" + #include "husarion_ugv_utils/moving_average.hpp" #include @@ -73,6 +75,9 @@ class LightsManagerNode : public rclcpp::Node float update_charging_anim_step_; + std::shared_ptr param_listener_; + lights_manager::Params params_; + rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; rclcpp::TimerBase::SharedPtr lights_tree_timer_; diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp index 51dcb3e2..3d2b247b 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/safety_manager_node.hpp @@ -29,6 +29,8 @@ #include "panther_msgs/msg/robot_driver_state.hpp" #include "panther_msgs/msg/system_status.hpp" +#include "safety_manager_parameters.hpp" + #include "husarion_ugv_utils/moving_average.hpp" #include @@ -89,6 +91,9 @@ class SafetyManagerNode : public rclcpp::Node int driver_temp_window_len_; float update_charging_anim_step_; + std::shared_ptr param_listener_; + safety_manager::Params params_; + rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr driver_state_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; diff --git a/husarion_ugv_manager/package.xml b/husarion_ugv_manager/package.xml index 3b5688d0..f6b39f90 100644 --- a/husarion_ugv_manager/package.xml +++ b/husarion_ugv_manager/package.xml @@ -19,6 +19,7 @@ behaviortree_cpp behaviortree_ros2 + generate_parameter_library husarion_ugv_utils iputils-ping libboost-dev diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp index e4dbe992..730cf9d8 100644 --- a/husarion_ugv_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -40,10 +40,11 @@ LightsManagerNode::LightsManagerNode( { RCLCPP_INFO(this->get_logger(), "Constructing node."); - DeclareParameters(); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - const auto battery_percent_window_len = - this->get_parameter("battery.percent.window_len").as_int(); + const auto battery_percent_window_len = this->params_.battery.percent.window_len; battery_percent_ma_ = std::make_unique>( battery_percent_window_len, 1.0); @@ -69,7 +70,7 @@ void LightsManagerNode::Initialize() "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::EStopCB, this, _1)); - const float timer_freq = this->get_parameter("timer_frequency").as_double(); + const float timer_freq = this->params_.timer_frequency; const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); @@ -79,40 +80,15 @@ void LightsManagerNode::Initialize() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void LightsManagerNode::DeclareParameters() -{ - const auto husarion_ugv_manager_pkg_path = - ament_index_cpp::get_package_share_directory("husarion_ugv_manager"); - const std::string default_bt_project_path = husarion_ugv_manager_pkg_path + - "/behavior_trees/LightsBT.btproj"; - const std::vector default_plugin_libs = {}; - - this->declare_parameter("bt_project_path", default_bt_project_path); - this->declare_parameter>("plugin_libs", default_plugin_libs); - this->declare_parameter>("ros_plugin_libs", default_plugin_libs); - this->declare_parameter("ros_communication_timeout.availability", 1.0); - this->declare_parameter("ros_communication_timeout.response", 1.0); - - this->declare_parameter("battery.percent.window_len", 6); - this->declare_parameter("battery.percent.threshold.low", 0.4); - this->declare_parameter("battery.percent.threshold.critical", 0.1); - this->declare_parameter("battery.animation_period.low", 30.0); - this->declare_parameter("battery.animation_period.critical", 15.0); - this->declare_parameter("battery.charging_anim_step", 0.1); - this->declare_parameter("timer_frequency", 10.0); -} - void LightsManagerNode::RegisterBehaviorTree() { - const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + const auto bt_project_path = this->params_.bt_project_path; - const auto plugin_libs = this->get_parameter("plugin_libs").as_string_array(); - const auto ros_plugin_libs = this->get_parameter("ros_plugin_libs").as_string_array(); + const auto plugin_libs = this->params_.plugin_libs; + const auto ros_plugin_libs = this->params_.ros_plugin_libs; - const auto service_availability_timeout = - this->get_parameter("ros_communication_timeout.availability").as_double(); - const auto service_response_timeout = - this->get_parameter("ros_communication_timeout.response").as_double(); + const auto service_availability_timeout = this->params_.ros_communication_timeout.availability; + const auto service_response_timeout = this->params_.ros_communication_timeout.response; BT::RosNodeParams params; params.nh = this->shared_from_this(); @@ -130,15 +106,11 @@ void LightsManagerNode::RegisterBehaviorTree() std::map LightsManagerNode::CreateLightsInitialBlackboard() { - update_charging_anim_step_ = this->get_parameter("battery.charging_anim_step").as_double(); - const float critical_battery_anim_period = - this->get_parameter("battery.animation_period.critical").as_double(); - const float critical_battery_threshold_percent = - this->get_parameter("battery.percent.threshold.critical").as_double(); - const float low_battery_anim_period = - this->get_parameter("battery.animation_period.low").as_double(); - const float low_battery_threshold_percent = - this->get_parameter("battery.percent.threshold.low").as_double(); + update_charging_anim_step_ = this->params_.battery.charging_anim_step; + const float critical_battery_anim_period = this->params_.battery.anim_period.critical; + const float critical_battery_threshold_percent = this->params_.battery.percent.threshold.critical; + const float low_battery_anim_period = this->params_.battery.anim_period.low; + const float low_battery_threshold_percent = this->params_.battery.percent.threshold.low; const std::string undefined_charging_anim_percent = ""; const int undefined_anim_id = -1; diff --git a/husarion_ugv_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp index 0eaf05b5..56c69bfe 100644 --- a/husarion_ugv_manager/src/safety_manager_node.cpp +++ b/husarion_ugv_manager/src/safety_manager_node.cpp @@ -42,10 +42,12 @@ SafetyManagerNode::SafetyManagerNode( { RCLCPP_INFO(this->get_logger(), "Constructing node."); - DeclareParameters(); + this->param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + this->params_ = this->param_listener_->get_params(); - const auto battery_temp_window_len = this->get_parameter("battery.temp.window_len").as_int(); - const auto cpu_temp_window_len = this->get_parameter("cpu.temp.window_len").as_int(); + const auto battery_temp_window_len = this->params_.battery.temp.window_len; + const auto cpu_temp_window_len = this->params_.cpu.temp.window_len; battery_temp_ma_ = std::make_unique>(battery_temp_window_len); @@ -55,7 +57,7 @@ SafetyManagerNode::SafetyManagerNode( safety_tree_manager_ = std::make_unique( "Safety", safety_initial_blackboard, 6666); - const auto shutdown_hosts_path = this->get_parameter("shutdown_hosts_path").as_string(); + const auto shutdown_hosts_path = this->params_.shutdown_hosts_path; const std::map shutdown_initial_blackboard = { {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path}, }; @@ -96,7 +98,7 @@ void SafetyManagerNode::Initialize() // Timers // ------------------------------- - const float timer_freq = this->get_parameter("timer_frequency").as_double(); + const float timer_freq = this->params_.timer_frequency; const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); @@ -106,43 +108,15 @@ void SafetyManagerNode::Initialize() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void SafetyManagerNode::DeclareParameters() -{ - const auto husarion_ugv_manager_pkg_path = - ament_index_cpp::get_package_share_directory("husarion_ugv_manager"); - const std::string default_bt_project_path = husarion_ugv_manager_pkg_path + - "/behavior_trees/SafetyBT.btproj"; - const std::vector default_plugin_libs = {}; - - this->declare_parameter("bt_project_path", default_bt_project_path); - this->declare_parameter("shutdown_hosts_path", ""); - this->declare_parameter>("plugin_libs", default_plugin_libs); - this->declare_parameter>("ros_plugin_libs", default_plugin_libs); - this->declare_parameter("ros_communication_timeout.availability", 1.0); - this->declare_parameter("ros_communication_timeout.response", 1.0); - - this->declare_parameter("battery.temp.window_len", 6); - this->declare_parameter("cpu.temp.window_len", 6); - this->declare_parameter("cpu.temp.fan_on", 70.0); - this->declare_parameter("cpu.temp.fan_off", 60.0); - this->declare_parameter("driver.temp.window_len", 6); - this->declare_parameter("driver.temp.fan_on", 45.0); - this->declare_parameter("driver.temp.fan_off", 35.0); - this->declare_parameter("timer_frequency", 10.0); - this->declare_parameter("fan_turn_off_timeout", 60.0); -} - void SafetyManagerNode::RegisterBehaviorTree() { - const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + const auto bt_project_path = this->params_.bt_project_path; - const auto plugin_libs = this->get_parameter("plugin_libs").as_string_array(); - const auto ros_plugin_libs = this->get_parameter("ros_plugin_libs").as_string_array(); + const auto plugin_libs = this->params_.plugin_libs; + const auto ros_plugin_libs = this->params_.ros_plugin_libs; - const auto service_availability_timeout = - this->get_parameter("ros_communication_timeout.availability").as_double(); - const auto service_response_timeout = - this->get_parameter("ros_communication_timeout.response").as_double(); + const auto service_availability_timeout = this->params_.ros_communication_timeout.availability; + const auto service_response_timeout = this->params_.ros_communication_timeout.response; BT::RosNodeParams params; params.nh = this->shared_from_this(); @@ -160,11 +134,11 @@ void SafetyManagerNode::RegisterBehaviorTree() std::map SafetyManagerNode::CreateSafetyInitialBlackboard() { - const float cpu_fan_on_temp = this->get_parameter("cpu.temp.fan_on").as_double(); - const float cpu_fan_off_temp = this->get_parameter("cpu.temp.fan_off").as_double(); - const float driver_fan_on_temp = this->get_parameter("driver.temp.fan_on").as_double(); - const float driver_fan_off_temp = this->get_parameter("driver.temp.fan_off").as_double(); - const float fan_turn_off_timeout = this->get_parameter("fan_turn_off_timeout").as_double(); + const float cpu_fan_on_temp = this->params_.cpu.temp.fan_on; + const float cpu_fan_off_temp = this->params_.cpu.temp.fan_off; + const float driver_fan_on_temp = this->params_.driver.temp.fan_on; + const float driver_fan_off_temp = this->params_.driver.temp.fan_off; + const float fan_turn_off_timeout = this->params_.fan_turn_off_timeout; const std::map safety_initial_bb = { {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp}, @@ -221,7 +195,7 @@ void SafetyManagerNode::RobotDriverStateCB(const RobotDriverStateMsg::SharedPtr if (driver_temp_ma_.find(driver.name) == driver_temp_ma_.end()) { RCLCPP_DEBUG( this->get_logger(), "Creating moving average for driver '%s'", driver.name.c_str()); - const auto driver_temp_window_len = this->get_parameter("driver.temp.window_len").as_int(); + const auto driver_temp_window_len = this->params_.driver.temp.window_len; driver_temp_ma_[driver.name] = std::make_unique>(driver_temp_window_len); } From b0600b35af76df823c896661ea15142fcacc6d6e Mon Sep 17 00:00:00 2001 From: Milosz Lagan Date: Tue, 19 Nov 2024 12:08:15 +0000 Subject: [PATCH 2/4] Parameter type casts and typo fixes --- .../config/lights_controller_parameters.yaml | 2 +- .../src/lights_controller_node.cpp | 2 +- .../config/lights_manager_parameters.yaml | 17 ++++++++--------- .../config/safety_manager_parameters.yaml | 9 ++++----- .../src/lights_manager_node.cpp | 9 +++++---- .../src/safety_manager_node.cpp | 12 ++++++------ 6 files changed, 25 insertions(+), 26 deletions(-) diff --git a/husarion_ugv_lights/config/lights_controller_parameters.yaml b/husarion_ugv_lights/config/lights_controller_parameters.yaml index c4b90ab0..0994c7b4 100644 --- a/husarion_ugv_lights/config/lights_controller_parameters.yaml +++ b/husarion_ugv_lights/config/lights_controller_parameters.yaml @@ -2,7 +2,7 @@ lights_controller: animations_config_path: type: string default_value: "" - description: Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. + description: Path to a YAML file with a description of LED configuration. This file includes definition of robot panels, virtual segments and default animations. validation: { not_empty<> } controller_frequency: diff --git a/husarion_ugv_lights/src/lights_controller_node.cpp b/husarion_ugv_lights/src/lights_controller_node.cpp index e1d51224..9306ff0c 100644 --- a/husarion_ugv_lights/src/lights_controller_node.cpp +++ b/husarion_ugv_lights/src/lights_controller_node.cpp @@ -56,7 +56,7 @@ LightsControllerNode::LightsControllerNode(const rclcpp::NodeOptions & options) const auto animations_config_path = this->params_.animations_config_path; const auto user_led_animations_path = this->params_.user_led_animations_path; - const float controller_freq = this->params_.controller_frequency; + const float controller_freq = static_cast(this->params_.controller_frequency); YAML::Node led_config_desc = YAML::LoadFile(animations_config_path); diff --git a/husarion_ugv_manager/config/lights_manager_parameters.yaml b/husarion_ugv_manager/config/lights_manager_parameters.yaml index e60a404c..efe28cfd 100644 --- a/husarion_ugv_manager/config/lights_manager_parameters.yaml +++ b/husarion_ugv_manager/config/lights_manager_parameters.yaml @@ -10,51 +10,50 @@ lights_manager: critical: type: double default_value: 15.0 - description: Time in seconds to wait before repeating animation, indicating a critical Battery state. + description: Time in seconds to wait before repeating animation, indicating a critical battery state. validation: { gt<>: 0.0 } low: type: double default_value: 30.0 - description: Time in seconds to wait before repeating the animation, indicating a low Battery state. + description: Time in seconds to wait before repeating the animation, indicating a low battery state. validation: { gt<>: 0.0 } percent: window_len: type: int default_value: 6 - description: Moving average window length used to smooth out Battery percentage readings. + description: Moving average window length used to smooth out battery percentage readings. validation: { gt<>: 0 } threshold: critical: type: double default_value: 0.1 - description: If the Battery percentage drops below this value, an animation indicating a critical Battery state will start being displayed. + description: If the battery percentage drops below this value, an animation indicating a critical battery state will start being displayed. validation: { bounds<>: [0.0, 1.0] } low: type: double default_value: 0.4 - description: If the Battery percentage drops below this value, the animation indicating a low Battery state will start being displayed. + description: If the battery percentage drops below this value, the animation indicating a low battery state will start being displayed. validation: { bounds<>: [0.0, 1.0] } bt_project_path: type: string default_value: "" description: Path to a BehaviorTree project. - validation: { not_empty<> } plugin_libs: type: string_array default_value: [] - description: List with names of plugins that are used in the BT project. + description: A list with names of plugins that are used in the BehaviorTree project. ros_communication_timeout: availability: type: double default_value: 1.0 - description: Timeout in seconds to wait for a service/action while initializing BT node. + description: Timeout in seconds to wait for a service/action while initializing a BehaviorTree node. validation: { gt<>: 0.0 } response: @@ -66,7 +65,7 @@ lights_manager: ros_plugin_libs: type: string_array default_value: [] - description: List with names of ROS plugins that are used in a BT project. + description: A list with names of ROS plugins that are used in a BehaviorTree project. timer_frequency: type: double diff --git a/husarion_ugv_manager/config/safety_manager_parameters.yaml b/husarion_ugv_manager/config/safety_manager_parameters.yaml index 4010ee12..068a4d9d 100644 --- a/husarion_ugv_manager/config/safety_manager_parameters.yaml +++ b/husarion_ugv_manager/config/safety_manager_parameters.yaml @@ -11,7 +11,6 @@ safety_manager: type: string default_value: "" description: Path to a BehaviorTree project. - validation: { not_empty<> } cpu: temp: @@ -44,7 +43,7 @@ safety_manager: fan_on: type: double default_value: 45.0 - description: Temperature in degrees Celsius* of any drivers above which the fan is turned on. + description: Temperature in degrees Celsius of any drivers above which the fan is turned on. validation: { bounds<>: [-20.0, 100.0] } window_len: @@ -62,13 +61,13 @@ safety_manager: plugin_libs: type: string_array default_value: [] - description: List with names of plugins that are used in the BT project. + description: A list with names of plugins that are used in the a BehaviorTree project. ros_communication_timeout: availability: type: double default_value: 1.0 - description: Timeout seconds to wait for a service/action while initializing BT node. + description: Timeout seconds to wait for a service/action while initializing a BehaviorTree node. validation: { gt<>: 0.0 } response: @@ -80,7 +79,7 @@ safety_manager: ros_plugin_libs: type: string_array default_value: [] - description: List with names of ROS plugins that are used in a BT project. + description: A list with names of ROS plugins that are used in a BehaviorTree project. shutdown_hosts_path: type: string diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp index 730cf9d8..4cdd5484 100644 --- a/husarion_ugv_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -107,10 +107,11 @@ void LightsManagerNode::RegisterBehaviorTree() std::map LightsManagerNode::CreateLightsInitialBlackboard() { update_charging_anim_step_ = this->params_.battery.charging_anim_step; - const float critical_battery_anim_period = this->params_.battery.anim_period.critical; - const float critical_battery_threshold_percent = this->params_.battery.percent.threshold.critical; - const float low_battery_anim_period = this->params_.battery.anim_period.low; - const float low_battery_threshold_percent = this->params_.battery.percent.threshold.low; + const double critical_battery_anim_period = this->params_.battery.anim_period.critical; + const double critical_battery_threshold_percent = + this->params_.battery.percent.threshold.critical; + const double low_battery_anim_period = this->params_.battery.anim_period.low; + const double low_battery_threshold_percent = this->params_.battery.percent.threshold.low; const std::string undefined_charging_anim_percent = ""; const int undefined_anim_id = -1; diff --git a/husarion_ugv_manager/src/safety_manager_node.cpp b/husarion_ugv_manager/src/safety_manager_node.cpp index 56c69bfe..63965f4f 100644 --- a/husarion_ugv_manager/src/safety_manager_node.cpp +++ b/husarion_ugv_manager/src/safety_manager_node.cpp @@ -98,7 +98,7 @@ void SafetyManagerNode::Initialize() // Timers // ------------------------------- - const float timer_freq = this->params_.timer_frequency; + const double timer_freq = this->params_.timer_frequency; const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); @@ -134,11 +134,11 @@ void SafetyManagerNode::RegisterBehaviorTree() std::map SafetyManagerNode::CreateSafetyInitialBlackboard() { - const float cpu_fan_on_temp = this->params_.cpu.temp.fan_on; - const float cpu_fan_off_temp = this->params_.cpu.temp.fan_off; - const float driver_fan_on_temp = this->params_.driver.temp.fan_on; - const float driver_fan_off_temp = this->params_.driver.temp.fan_off; - const float fan_turn_off_timeout = this->params_.fan_turn_off_timeout; + const double cpu_fan_on_temp = this->params_.cpu.temp.fan_on; + const double cpu_fan_off_temp = this->params_.cpu.temp.fan_off; + const double driver_fan_on_temp = this->params_.driver.temp.fan_on; + const double driver_fan_off_temp = this->params_.driver.temp.fan_off; + const double fan_turn_off_timeout = this->params_.fan_turn_off_timeout; const std::map safety_initial_bb = { {"CPU_FAN_OFF_TEMP", cpu_fan_off_temp}, From 993a8a3774ac43dc5232ed481eb18ce1c777f8b1 Mon Sep 17 00:00:00 2001 From: Milosz Lagan Date: Thu, 21 Nov 2024 15:18:05 +0000 Subject: [PATCH 3/4] Change lights BT param types --- husarion_ugv_manager/src/lights_manager_node.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp index 4cdd5484..8d918272 100644 --- a/husarion_ugv_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -107,11 +107,13 @@ void LightsManagerNode::RegisterBehaviorTree() std::map LightsManagerNode::CreateLightsInitialBlackboard() { update_charging_anim_step_ = this->params_.battery.charging_anim_step; - const double critical_battery_anim_period = this->params_.battery.anim_period.critical; - const double critical_battery_threshold_percent = - this->params_.battery.percent.threshold.critical; - const double low_battery_anim_period = this->params_.battery.anim_period.low; - const double low_battery_threshold_percent = this->params_.battery.percent.threshold.low; + const float critical_battery_anim_period = + static_cast(this->params_.battery.anim_period.critical); + const float critical_battery_threshold_percent = + static_cast(this->params_.battery.percent.threshold.critical); + const float low_battery_anim_period = static_cast(this->params_.battery.anim_period.low); + const float low_battery_threshold_percent = + static_cast(this->params_.battery.percent.threshold.low); const std::string undefined_charging_anim_percent = ""; const int undefined_anim_id = -1; From 93b18fa3d56fe9bd4b48ef7dd99fc99c4b393f53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mi=C5=82osz=20=C5=81agan?= Date: Fri, 22 Nov 2024 09:35:38 +0100 Subject: [PATCH 4/4] Change param type Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --- husarion_ugv_manager/src/lights_manager_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp index 8d918272..289a2f54 100644 --- a/husarion_ugv_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -70,7 +70,7 @@ void LightsManagerNode::Initialize() "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::EStopCB, this, _1)); - const float timer_freq = this->params_.timer_frequency; + const double timer_freq = this->params_.timer_frequency; const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000));