From 97cb1e902cf72969e18be43bf6ad91bd8397449b Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 16 Oct 2024 11:46:49 +0200 Subject: [PATCH 01/13] Manage lights --- .../config/docking_components.yaml | 10 +- panther_manager/CMakeLists.txt | 16 +- .../behavior_trees/RobotStatesBT.btproj | 31 ++ panther_manager/behavior_trees/lights.xml | 72 +++-- .../behavior_trees/robot_states.xml | 64 ++++ .../config/robot_states_manager.yaml | 23 ++ .../panther_manager/lights_manager_node.hpp | 4 + .../plugins/action/dock_robot_node.hpp | 8 +- .../plugins/action/undock_robot_node.hpp | 4 +- .../robot_states_manager_node.hpp | 96 ++++++ panther_manager/launch/manager.launch.py | 23 ++ panther_manager/src/lights_manager_node.cpp | 16 +- .../src/plugins/action/dock_robot_node.cpp | 3 - .../src/plugins/action/undock_robot_node.cpp | 3 +- .../src/robot_states_manager_node.cpp | 285 ++++++++++++++++++ .../src/robot_states_manager_node_main.cpp | 38 +++ .../plugins/action/test_dock_robot_node.cpp | 29 +- .../plugins/action/test_undock_robot_node.cpp | 11 +- .../test/utils/plugin_test_utils.hpp | 5 +- 19 files changed, 670 insertions(+), 71 deletions(-) create mode 100644 panther_manager/behavior_trees/RobotStatesBT.btproj create mode 100644 panther_manager/behavior_trees/robot_states.xml create mode 100644 panther_manager/config/robot_states_manager.yaml create mode 100644 panther_manager/include/panther_manager/robot_states_manager_node.hpp create mode 100644 panther_manager/src/robot_states_manager_node.cpp create mode 100644 panther_manager/src/robot_states_manager_node_main.cpp diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index 169b3a13e..4f2b046c7 100644 --- a/panther_description/config/docking_components.yaml +++ b/panther_description/config/docking_components.yaml @@ -6,11 +6,11 @@ components: rpy: 0.0 0.0 0.0 device_namespace: wireless_charger - - type: WCH02 - parent_link: world - xyz: 3.0 -2.0 1.0 - rpy: -1.57 0.0 1.57 - device_namespace: wireless_charger + # - type: WCH02 + # parent_link: world + # xyz: 3.0 -2.0 1.0 + # rpy: -1.57 0.0 1.57 + # device_namespace: wireless_charger - type: CAM01 name: camera diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 1f7bcfb9e..0208bb2a3 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -98,10 +98,24 @@ ament_target_dependencies( std_msgs) target_link_libraries(lights_manager_node ${plugin_libs}) +add_executable( + robot_states_manager_node + src/robot_states_manager_node_main.cpp src/robot_states_manager_node.cpp + src/behavior_tree_manager.cpp) +ament_target_dependencies( + robot_states_manager_node + behaviortree_ros2 + panther_msgs + panther_utils + rclcpp + sensor_msgs + std_msgs) +target_link_libraries(robot_states_manager_node ${plugin_libs}) + install(TARGETS ${plugin_libs} DESTINATION lib) install(TARGETS safety_manager_node lights_manager_node - DESTINATION lib/${PROJECT_NAME}) + robot_states_manager_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY behavior_trees config launch DESTINATION share/${PROJECT_NAME}) diff --git a/panther_manager/behavior_trees/RobotStatesBT.btproj b/panther_manager/behavior_trees/RobotStatesBT.btproj new file mode 100644 index 000000000..f632f1431 --- /dev/null +++ b/panther_manager/behavior_trees/RobotStatesBT.btproj @@ -0,0 +1,31 @@ + + + + + + + + animation ID + optional parameter + indicates if animation should repeat + ROS service name + + + bool value indicating whether to use dock ID + dock ID + type of the dock + maximum staging time + bool value indicating whether to navigate to + staging pose + action name to call + + + time in s to wait before ticking child again + + + type of the dock + maximum time to get back to the staging pose + action name to call + + + diff --git a/panther_manager/behavior_trees/lights.xml b/panther_manager/behavior_trees/lights.xml index 5af3c4865..c632cdc75 100644 --- a/panther_manager/behavior_trees/lights.xml +++ b/panther_manager/behavior_trees/lights.xml @@ -3,15 +3,13 @@ + _skipIf="battery_status != POWER_SUPPLY_STATUS_CHARGING \ && battery_status != POWER_SUPPLY_STATUS_FULL"> @@ -20,10 +18,8 @@ param="{battery_percent_round}" repeating="true" service_name="lights/set_animation" - _skipIf="battery_percent_round == charging_anim_percent \ -&& current_anim_id == CHARGING_BATTERY_ANIM_ID" - _onSuccess="charging_anim_percent = battery_percent_round; \ -current_anim_id = CHARGING_BATTERY_ANIM_ID"/> + _skipIf="battery_percent_round == charging_anim_percent \ && current_anim_id == CHARGING_BATTERY_ANIM_ID" + _onSuccess="charging_anim_percent = battery_percent_round; \ current_anim_id = CHARGING_BATTERY_ANIM_ID"/> - - + _skipIf="battery_status != POWER_SUPPLY_STATUS_DISCHARGING \ && battery_status != POWER_SUPPLY_STATUS_NOT_CHARGING"> + + + + + + + + _skipIf="battery_percent < CRITICAL_BATTERY_THRESHOLD_PERCENT \ || battery_percent >= LOW_BATTERY_THRESHOLD_PERCENT"> param="" repeating="true" service_name="lights/set_animation" - _skipIf="battery_status != POWER_SUPPLY_STATUS_UNKNOWN \ -|| current_anim_id == ERROR_ANIM_ID" + _skipIf="battery_status != POWER_SUPPLY_STATUS_UNKNOWN \ || current_anim_id == ERROR_ANIM_ID" _onSuccess="current_anim_id = ERROR_ANIM_ID"/> diff --git a/panther_manager/behavior_trees/robot_states.xml b/panther_manager/behavior_trees/robot_states.xml new file mode 100644 index 000000000..3dbbc361e --- /dev/null +++ b/panther_manager/behavior_trees/robot_states.xml @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/panther_manager/config/robot_states_manager.yaml b/panther_manager/config/robot_states_manager.yaml new file mode 100644 index 000000000..8ac9a7a12 --- /dev/null +++ b/panther_manager/config/robot_states_manager.yaml @@ -0,0 +1,23 @@ +/**: + robot_states_manager: + ros__parameters: + timer_frequency: 50.0 + battery: + percent: + window_len: 6 + threshold: + low: 0.4 + critical: 0.1 + animation_period: + low: 30.0 + critical: 15.0 + charging_anim_step: 0.1 + ros_communication_timeout: + availability: 1.0 + response: 1.0 + plugin_libs: + - tick_after_timeout_bt_node + ros_plugin_libs: + - call_set_led_animation_service_bt_node + - dock_robot_bt_node + - undock_robot_bt_node diff --git a/panther_manager/include/panther_manager/lights_manager_node.hpp b/panther_manager/include/panther_manager/lights_manager_node.hpp index 21937cd86..330c233ed 100644 --- a/panther_manager/include/panther_manager/lights_manager_node.hpp +++ b/panther_manager/include/panther_manager/lights_manager_node.hpp @@ -23,6 +23,7 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/int8.hpp" #include "panther_msgs/msg/led_animation.hpp" @@ -35,6 +36,7 @@ namespace panther_manager using BatteryStateMsg = sensor_msgs::msg::BatteryState; using BoolMsg = std_msgs::msg::Bool; +using Int8Msg = std_msgs::msg::Int8; using LEDAnimationMsg = panther_msgs::msg::LEDAnimation; /** @@ -69,12 +71,14 @@ class LightsManagerNode : public rclcpp::Node private: void BatteryCB(const BatteryStateMsg::SharedPtr battery); void EStopCB(const BoolMsg::SharedPtr e_stop); + void RobotStateCB(const Int8Msg::SharedPtr robot_state); void LightsTreeTimerCB(); float update_charging_anim_step_; rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; + rclcpp::Subscription::SharedPtr robot_state_sub_; rclcpp::TimerBase::SharedPtr lights_tree_timer_; std::unique_ptr> battery_percent_ma_; diff --git a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp index 2e4a5c03f..f2bed74b5 100644 --- a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp @@ -57,12 +57,8 @@ class DockRobot : public BT::RosActionNode( "navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"), - BT::OutputPort( - "success", "If the action was successful"), - BT::OutputPort( - "error_code", "Contextual error code, if any"), - BT::OutputPort( - "num_retries", "Number of retries attempted"), + BT::OutputPort("error_code", "Contextual error code, if any"), + BT::OutputPort("num_retries", "Number of retries attempted"), }); } }; diff --git a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp index ade7380ff..157ca58da 100644 --- a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp @@ -55,9 +55,7 @@ class UndockRobot : public BT::RosActionNode( "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"), - BT::OutputPort( - "success", "If the action was successful"), - BT::OutputPort("error_code", "Error code"), + // BT::OutputPort("error_code", "Error code"), }); } }; diff --git a/panther_manager/include/panther_manager/robot_states_manager_node.hpp b/panther_manager/include/panther_manager/robot_states_manager_node.hpp new file mode 100644 index 000000000..5fce64ba3 --- /dev/null +++ b/panther_manager/include/panther_manager/robot_states_manager_node.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_MANAGER_ROBOT_STATES_MANAGER_NODE_HPP_ +#define PANTHER_MANAGER_ROBOT_STATES_MANAGER_NODE_HPP_ + +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "rclcpp/rclcpp.hpp" + +#include "sensor_msgs/msg/battery_state.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/int8.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include "panther_msgs/msg/led_animation.hpp" + +#include "panther_utils/moving_average.hpp" + +#include + +namespace panther_manager +{ + +using BatteryStateMsg = sensor_msgs::msg::BatteryState; +using BoolMsg = std_msgs::msg::Bool; +using Int8Msg = std_msgs::msg::Int8; +using SetBoolSrv = std_srvs::srv::SetBool; +using LEDAnimationMsg = panther_msgs::msg::LEDAnimation; + +/** + * @brief This class is responsible for creating a BehaviorTree responsible for docking management, + * spinning it, and updating blackboard entries based on subscribed topics. + */ +class DockingManagerNode : public rclcpp::Node +{ +public: + DockingManagerNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~DockingManagerNode() {} + + void Initialize(); + +protected: + void DeclareParameters(); + void RegisterBehaviorTree(); + std::map CreateBlackboard(); + + /** + * @brief Checks whether the required blackboard entries for the docking tree are present. These + * entries are usually updated when the first ROS message containing required information is + * received. + * + * @return True if all required blackboard entries are present. + */ + bool SystemReady(); + + std::unique_ptr docking_tree_manager_; + +private: + void BatteryCB(const BatteryStateMsg::SharedPtr battery); + void EStopCB(const BoolMsg::SharedPtr e_stop); + void DockingSrvCB( + const SetBoolSrv::Request::SharedPtr & request, SetBoolSrv::Response::SharedPtr response); + void DockingTreeTimerCB(); + + void PublishRobotState(); + + float update_charging_anim_step_; + + rclcpp::Subscription::SharedPtr battery_sub_; + rclcpp::Subscription::SharedPtr e_stop_sub_; + rclcpp::Publisher::SharedPtr robot_state_pub_; + rclcpp::Service::SharedPtr docking_srv_; + rclcpp::TimerBase::SharedPtr docking_tree_timer_; + + std::unique_ptr> battery_percent_ma_; + BT::BehaviorTreeFactory factory_; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_ROBOT_STATES_MANAGER_NODE_HPP_ diff --git a/panther_manager/launch/manager.launch.py b/panther_manager/launch/manager.launch.py index 14f4c8c75..8b4f55f5b 100644 --- a/panther_manager/launch/manager.launch.py +++ b/panther_manager/launch/manager.launch.py @@ -32,6 +32,15 @@ def generate_launch_description(): panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0") panther_manager_dir = FindPackageShare("panther_manager") + docking_bt_project_path = LaunchConfiguration("docking_bt_project_path") + declare_docking_bt_project_path_arg = DeclareLaunchArgument( + "docking_bt_project_path", + default_value=PathJoinSubstitution( + [panther_manager_dir, "behavior_trees", "RobotStatesBT.btproj"] + ), + description="Path to BehaviorTree project file, responsible for lights management.", + ) + lights_bt_project_path = LaunchConfiguration("lights_bt_project_path") declare_lights_bt_project_path_arg = DeclareLaunchArgument( "lights_bt_project_path", @@ -78,6 +87,18 @@ def generate_launch_description(): description="Whether simulation is used", ) + robot_states_manager_node = Node( + package="panther_manager", + executable="robot_states_manager_node", + name="robot_states_manager", + parameters=[ + PathJoinSubstitution([panther_manager_dir, "config", "robot_states_manager.yaml"]), + {"bt_project_path": docking_bt_project_path}, + ], + namespace=namespace, + emulate_tty=True, + ) + lights_manager_node = Node( package="panther_manager", executable="lights_manager_node", @@ -107,11 +128,13 @@ def generate_launch_description(): ) actions = [ + declare_docking_bt_project_path_arg, declare_lights_bt_project_path_arg, declare_safety_bt_project_path_arg, declare_namespace_arg, declare_shutdown_hosts_config_path_arg, declare_use_sim_arg, + robot_states_manager_node, lights_manager_node, safety_manager_node, ] diff --git a/panther_manager/src/lights_manager_node.cpp b/panther_manager/src/lights_manager_node.cpp index 53ff612bd..2f04bd85e 100644 --- a/panther_manager/src/lights_manager_node.cpp +++ b/panther_manager/src/lights_manager_node.cpp @@ -68,6 +68,9 @@ void LightsManagerNode::Initialize() e_stop_sub_ = this->create_subscription( "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::EStopCB, this, _1)); + robot_state_sub_ = this->create_subscription( + "robot_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&LightsManagerNode::RobotStateCB, this, _1)); const float timer_freq = this->get_parameter("timer_frequency").as_double(); const auto timer_period_ms = @@ -146,10 +149,17 @@ std::map LightsManagerNode::CreateLightsInitialBlackboard const std::map lights_initial_bb = { {"charging_anim_percent", undefined_charging_anim_percent}, {"current_anim_id", undefined_anim_id}, + {"robot_state", 0}, {"CRITICAL_BATTERY_ANIM_PERIOD", critical_battery_anim_period}, {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent}, {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period}, {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent}, + // robot states + {"ROBOT_STATE_ERROR", -1}, + {"ROBOT_STATE_ESTOP", 0}, + {"ROBOT_STATE_STANDBY", 1}, + {"ROBOT_STATE_DOCKING", 2}, + {"ROBOT_STATE_SUCCESS", 3}, // anim constants {"E_STOP_ANIM_ID", unsigned(LEDAnimationMsg::E_STOP)}, {"READY_ANIM_ID", unsigned(LEDAnimationMsg::READY)}, @@ -170,7 +180,6 @@ std::map LightsManagerNode::CreateLightsInitialBlackboard {"POWER_SUPPLY_STATUS_FULL", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_FULL)}, // battery health constants {"POWER_SUPPLY_HEALTH_OVERHEAT", unsigned(BatteryStateMsg::POWER_SUPPLY_HEALTH_OVERHEAT)}, - }; RCLCPP_INFO(this->get_logger(), "Blackboard created."); @@ -218,6 +227,11 @@ void LightsManagerNode::LightsTreeTimerCB() } } +void LightsManagerNode::RobotStateCB(const Int8Msg::SharedPtr robot_state) +{ + lights_tree_manager_->GetBlackboard()->set("robot_state", robot_state->data); +} + bool LightsManagerNode::SystemReady() { if ( diff --git a/panther_manager/src/plugins/action/dock_robot_node.cpp b/panther_manager/src/plugins/action/dock_robot_node.cpp index 065476be6..8f5a60cf0 100644 --- a/panther_manager/src/plugins/action/dock_robot_node.cpp +++ b/panther_manager/src/plugins/action/dock_robot_node.cpp @@ -27,7 +27,6 @@ bool DockRobot::setGoal(Goal & goal) } if (!this->getInput("use_dock_id", goal.use_dock_id)) { - goal.use_dock_id = false; RCLCPP_WARN_STREAM( this->logger(), GetLoggerPrefix(name()) << "use_dock_id not set, using default value: " << goal.use_dock_id); @@ -41,7 +40,6 @@ bool DockRobot::setGoal(Goal & goal) } if (!this->getInput("navigate_to_staging_pose", goal.navigate_to_staging_pose)) { - goal.navigate_to_staging_pose = false; RCLCPP_WARN_STREAM( this->logger(), GetLoggerPrefix(name()) << "navigate_to_staging_pose not set, using default " "value: " @@ -63,7 +61,6 @@ BT::NodeStatus DockRobot::onResultReceived(const WrappedResult & wr) { const auto & result = wr.result; - this->setOutput("success", result->success); this->setOutput("error_code", result->error_code); this->setOutput("num_retries", result->num_retries); diff --git a/panther_manager/src/plugins/action/undock_robot_node.cpp b/panther_manager/src/plugins/action/undock_robot_node.cpp index 89c8cc101..89b7ac10c 100644 --- a/panther_manager/src/plugins/action/undock_robot_node.cpp +++ b/panther_manager/src/plugins/action/undock_robot_node.cpp @@ -39,8 +39,7 @@ BT::NodeStatus UndockRobot::onResultReceived(const WrappedResult & wr) { const auto & result = wr.result; - this->setOutput("success", result->success); - this->setOutput("error_code", result->error_code); + // this->setOutput("error_code", result->error_code); if (result->success) { return BT::NodeStatus::SUCCESS; diff --git a/panther_manager/src/robot_states_manager_node.cpp b/panther_manager/src/robot_states_manager_node.cpp new file mode 100644 index 000000000..605076024 --- /dev/null +++ b/panther_manager/src/robot_states_manager_node.cpp @@ -0,0 +1,285 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "panther_manager/robot_states_manager_node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behaviortree_ros2/ros_node_params.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "panther_utils/moving_average.hpp" + +#include +#include + +namespace panther_manager +{ + +DockingManagerNode::DockingManagerNode( + const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + RCLCPP_INFO(this->get_logger(), "Constructing node."); + + DeclareParameters(); + + const auto battery_percent_window_len = + this->get_parameter("battery.percent.window_len").as_int(); + + battery_percent_ma_ = std::make_unique>( + battery_percent_window_len, 1.0); + + const auto initial_blackboard = CreateBlackboard(); + docking_tree_manager_ = std::make_unique( + "Docking", initial_blackboard, 5557); + + RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); +} + +void DockingManagerNode::Initialize() +{ + RCLCPP_INFO(this->get_logger(), "Initializing."); + + RegisterBehaviorTree(); + docking_tree_manager_->Initialize(factory_); + + using namespace std::placeholders; + + battery_sub_ = this->create_subscription( + "battery/battery_status", 10, std::bind(&DockingManagerNode::BatteryCB, this, _1)); + e_stop_sub_ = this->create_subscription( + "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&DockingManagerNode::EStopCB, this, _1)); + robot_state_pub_ = this->create_publisher( + "robot_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + docking_srv_ = this->create_service( + "docking", std::bind(&DockingManagerNode::DockingSrvCB, this, _1, _2)); + + const float timer_freq = this->get_parameter("timer_frequency").as_double(); + const auto timer_period_ms = + std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); + + docking_tree_timer_ = this->create_wall_timer( + timer_period_ms, std::bind(&DockingManagerNode::DockingTreeTimerCB, this)); + + RCLCPP_INFO(this->get_logger(), "Initialized successfully."); +} + +void DockingManagerNode::DeclareParameters() +{ + const auto panther_manager_pkg_path = + ament_index_cpp::get_package_share_directory("panther_manager"); + const std::string default_bt_project_path = panther_manager_pkg_path + + "/behavior_trees/RobotStatesBT.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 DockingManagerNode::RegisterBehaviorTree() +{ + const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); + + 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 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(); + + BT::RosNodeParams params; + params.nh = this->shared_from_this(); + params.wait_for_server_timeout = + std::chrono::milliseconds(static_cast(service_availability_timeout * 1000)); + params.server_timeout = + std::chrono::milliseconds(static_cast(service_response_timeout * 1000)); + + behavior_tree_utils::RegisterBehaviorTree( + factory_, bt_project_path, plugin_libs, params, ros_plugin_libs); + + RCLCPP_INFO( + this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str()); +} + +std::map DockingManagerNode::CreateBlackboard() +{ + 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(); + + const std::string undefined_charging_anim_percent = ""; + const int undefined_anim_id = -1; + + const std::map docking_initial_bb = { + {"charging_anim_percent", undefined_charging_anim_percent}, + {"current_anim_id", undefined_anim_id}, + {"docking_cmd", 0}, + {"robot_state", 0}, + {"CRITICAL_BATTERY_ANIM_PERIOD", critical_battery_anim_period}, + {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent}, + {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period}, + {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent}, + // robot states + {"ROBOT_STATE_ERROR", -1}, + {"ROBOT_STATE_ESTOP", 0}, + {"ROBOT_STATE_STANDBY", 1}, + {"ROBOT_STATE_DOCKING", 2}, + {"ROBOT_STATE_SUCCESS", 3}, + // robot states + {"DOCKING_CMD_NONE", 0}, + {"DOCKING_CMD_DOCK", 1}, + {"DOCKING_CMD_UNDOCK", 2}, + // anim constants + {"E_STOP_ANIM_ID", unsigned(LEDAnimationMsg::E_STOP)}, + {"READY_ANIM_ID", unsigned(LEDAnimationMsg::READY)}, + {"ERROR_ANIM_ID", unsigned(LEDAnimationMsg::ERROR)}, + {"MANUAL_ACTION_ANIM_ID", unsigned(LEDAnimationMsg::MANUAL_ACTION)}, + {"AUTONOMOUS_ACTION_ANIM_ID", unsigned(LEDAnimationMsg::AUTONOMOUS_ACTION)}, + {"GOAL_ACHIEVED_ANIM_ID", unsigned(LEDAnimationMsg::GOAL_ACHIEVED)}, + {"LOW_BATTERY_ANIM_ID", unsigned(LEDAnimationMsg::LOW_BATTERY)}, + {"CRITICAL_BATTERY_ANIM_ID", unsigned(LEDAnimationMsg::CRITICAL_BATTERY)}, + {"BATTERY_STATE_ANIM_ID", unsigned(LEDAnimationMsg::BATTERY_STATE)}, + {"CHARGING_BATTERY_ANIM_ID", unsigned(LEDAnimationMsg::CHARGING_BATTERY)}, + // battery status constants + {"POWER_SUPPLY_STATUS_UNKNOWN", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN)}, + {"POWER_SUPPLY_STATUS_CHARGING", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING)}, + {"POWER_SUPPLY_STATUS_DISCHARGING", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING)}, + {"POWER_SUPPLY_STATUS_NOT_CHARGING", + unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_NOT_CHARGING)}, + {"POWER_SUPPLY_STATUS_FULL", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_FULL)}, + // battery health constants + {"POWER_SUPPLY_HEALTH_OVERHEAT", unsigned(BatteryStateMsg::POWER_SUPPLY_HEALTH_OVERHEAT)}, + }; + + RCLCPP_INFO(this->get_logger(), "Blackboard created."); + return docking_initial_bb; +} + +void DockingManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery_state) +{ + const auto battery_status = battery_state->power_supply_status; + const auto battery_health = battery_state->power_supply_health; + docking_tree_manager_->GetBlackboard()->set("battery_status", battery_status); + docking_tree_manager_->GetBlackboard()->set("battery_health", battery_health); + + // don't update battery percentage if unknown status or health + if ( + battery_status != BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN && + battery_health != BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN) { + battery_percent_ma_->Roll(battery_state->percentage); + } + + docking_tree_manager_->GetBlackboard()->set( + "battery_percent", battery_percent_ma_->GetAverage()); + docking_tree_manager_->GetBlackboard()->set( + "battery_percent_round", + std::to_string( + round(battery_percent_ma_->GetAverage() / update_charging_anim_step_) * + update_charging_anim_step_)); +} + +void DockingManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) +{ + docking_tree_manager_->GetBlackboard()->set("e_stop_state", e_stop->data); +} + +void DockingManagerNode::DockingSrvCB( + const SetBoolSrv::Request::SharedPtr & request, SetBoolSrv::Response::SharedPtr response) +{ + int robot_state; + if (!docking_tree_manager_->GetBlackboard()->get("robot_state", robot_state)) { + throw std::runtime_error("Cannot get robot state from blackboard."); + } + + if (robot_state != 1) { + response->success = false; + response->message = "Cannot start docking/undocking robot is not in STANDBY state."; + return; + } + + if (request->data) { + docking_tree_manager_->GetBlackboard()->set("docking_cmd", 1); + response->message = "Docking Started"; + } else { + docking_tree_manager_->GetBlackboard()->set("docking_cmd", 2); + response->message = "Undocking Started"; + } + response->success = true; +} + +void DockingManagerNode::DockingTreeTimerCB() +{ + if (!SystemReady()) { + return; + } + + docking_tree_manager_->TickOnce(); + PublishRobotState(); + + if (docking_tree_manager_->GetTreeStatus() == BT::NodeStatus::FAILURE) { + RCLCPP_WARN(this->get_logger(), "Docking behavior tree returned FAILURE status"); + } +} + +void DockingManagerNode::PublishRobotState() +{ + Int8Msg robot_state; + if (docking_tree_manager_->GetBlackboard()->get("robot_state", robot_state.data)) { + robot_state_pub_->publish(robot_state); + } +} + +bool DockingManagerNode::SystemReady() +{ + if ( + !docking_tree_manager_->GetBlackboard()->getEntry("e_stop_state") || + !docking_tree_manager_->GetBlackboard()->getEntry("battery_status")) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Waiting for required system messages to arrive."); + return false; + } + + return true; +} + +} // namespace panther_manager diff --git a/panther_manager/src/robot_states_manager_node_main.cpp b/panther_manager/src/robot_states_manager_node_main.cpp new file mode 100644 index 000000000..1a90594e0 --- /dev/null +++ b/panther_manager/src/robot_states_manager_node_main.cpp @@ -0,0 +1,38 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "panther_manager/robot_states_manager_node.hpp" + +#include +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto robot_states_manager_node = + std::make_shared("robot_states_manager"); + robot_states_manager_node->Initialize(); + + try { + rclcpp::spin(robot_states_manager_node); + } catch (const std::runtime_error & err) { + std::cerr << "[robot_states_manager] Caught exception: " << err.what() << std::endl; + } + + std::cout << "[robot_states_manager] Shutting down" << std::endl; + rclcpp::shutdown(); + return 0; +} diff --git a/panther_manager/test/plugins/action/test_dock_robot_node.cpp b/panther_manager/test/plugins/action/test_dock_robot_node.cpp index ca45c50c9..3efc84c99 100644 --- a/panther_manager/test/plugins/action/test_dock_robot_node.cpp +++ b/panther_manager/test/plugins/action/test_dock_robot_node.cpp @@ -64,7 +64,6 @@ class TestDockRobot : public panther_manager::plugin_test_utils::PluginTestUtils TEST_F(TestDockRobot, GoodLoadingDockRobotPlugin) { std::map params = { - {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, @@ -80,9 +79,11 @@ TEST_F(TestDockRobot, GoodLoadingDockRobotPlugin) TEST_F(TestDockRobot, WrongLoadingDockRobotPlugin) { std::map params = { - {"action_name", ""}, {"use_dock_id", "true"}, - {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, + {"use_dock_id", "true"}, + {"dock_id", "test_dock"}, + {"dock_type", "test_dock_type"}, + {"max_staging_time", "5.0"}, + {"navigate_to_staging_pose", "false"}, }; RegisterNodeWithParams("DockRobot"); @@ -93,7 +94,7 @@ TEST_F(TestDockRobot, WrongLoadingDockRobotPlugin) TEST_F(TestDockRobot, WrongCallDockRobotServerNotInitialized) { std::map params = { - {"action_name", "test_dock_action"}, + {"use_dock_id", "true"}, {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, @@ -116,8 +117,9 @@ TEST_F(TestDockRobot, WrongCallDockRobotServerWithNoDockID) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, - {"dock_type", "test_dock_type"}, {"max_staging_time", "5.0"}, + {"use_dock_id", "true"}, + {"dock_type", "test_dock_type"}, + {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; @@ -135,8 +137,9 @@ TEST_F(TestDockRobot, CallDockRobotServerWithoutDockID) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_dock_action"}, {"use_dock_id", "false"}, - {"dock_type", "test_dock_type"}, {"max_staging_time", "5.0"}, + {"use_dock_id", "false"}, + {"dock_type", "test_dock_type"}, + {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; @@ -154,7 +157,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockID) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_dock_action"}, + {"use_dock_id", "true"}, {"dock_id", ""}, {"dock_type", "test_dock_type"}, @@ -176,7 +179,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockType) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_dock_action"}, + {"use_dock_id", "true"}, {"dock_id", "main_dock"}, {"dock_type", ""}, @@ -197,7 +200,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseFailure) CreateActionServer(GoalResponse::REJECT, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_dock_action"}, + {"use_dock_id", "true"}, {"dock_id", "main_dock"}, {"dock_type", "test_dock_type"}, @@ -219,7 +222,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseSuccess) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_dock_action"}, + {"use_dock_id", "true"}, {"dock_id", "main_dock"}, {"dock_type", "test_dock_type"}, diff --git a/panther_manager/test/plugins/action/test_undock_robot_node.cpp b/panther_manager/test/plugins/action/test_undock_robot_node.cpp index 5820bfded..728f2eb33 100644 --- a/panther_manager/test/plugins/action/test_undock_robot_node.cpp +++ b/panther_manager/test/plugins/action/test_undock_robot_node.cpp @@ -66,7 +66,7 @@ class TestUndockRobot : public panther_manager::plugin_test_utils::PluginTestUti TEST_F(TestUndockRobot, GoodLoadingUndockRobotPlugin) { std::map params = { - {"action_name", "test_undock_action"}, + {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -79,7 +79,6 @@ TEST_F(TestUndockRobot, GoodLoadingUndockRobotPlugin) TEST_F(TestUndockRobot, WrongLoadingUndockRobotPlugin) { std::map params = { - {"action_name", ""}, {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -92,7 +91,7 @@ TEST_F(TestUndockRobot, WrongLoadingUndockRobotPlugin) TEST_F(TestUndockRobot, WrongCallUndockRobotServerNotInitialized) { std::map params = { - {"action_name", "test_undock_action"}, + {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -113,7 +112,7 @@ TEST_F(TestUndockRobot, WrongCallUndockRobotServerWithNoDockType) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_undock_action"}, + {"dock_type", ""}, {"max_undocking_time", "5.0"}, }; @@ -131,7 +130,7 @@ TEST_F(TestUndockRobot, CallUndockRobotServerFailure) CreateActionServer(GoalResponse::REJECT, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_undock_action"}, + {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -150,7 +149,7 @@ TEST_F(TestUndockRobot, CallUndockRobotServerSuccess) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"action_name", "test_undock_action"}, + {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; diff --git a/panther_manager/test/utils/plugin_test_utils.hpp b/panther_manager/test/utils/plugin_test_utils.hpp index 23d1d0c4f..d97ba8037 100644 --- a/panther_manager/test/utils/plugin_test_utils.hpp +++ b/panther_manager/test/utils/plugin_test_utils.hpp @@ -116,7 +116,6 @@ class PluginTestUtils : public testing::Test template void CreateAction( - const std::string & action_name, std::function goal)> handle_goal, @@ -126,9 +125,9 @@ class PluginTestUtils : public testing::Test std::function> goal_handle)> handle_accepted) { - server_node_ = std::make_shared("test_node_for_" + action_name); + server_node_ = std::make_shared("test_node"); action_server_ = rclcpp_action::create_server( - server_node_, action_name, handle_goal, handle_cancel, handle_accepted); + server_node_, handle_goal, handle_cancel, handle_accepted); executor_ = std::make_unique(); executor_->add_node(server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); From 3b36c1dd7b04e125e76d9fcef0281f3a6f28e479 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 16 Oct 2024 15:17:36 +0200 Subject: [PATCH 02/13] Simple working version --- .../config/docking_components.yaml | 10 +-- .../config/panther_docking_server.yaml | 2 +- panther_docking/launch/docking.launch.py | 64 +++++++++---------- .../launch/simulate_robot.launch.py | 14 ++++ panther_gazebo/package.xml | 1 + .../behavior_trees/RobotStatesBT.btproj | 6 +- .../behavior_trees/robot_states.xml | 44 ++++++++++--- .../config/robot_states_manager.yaml | 1 + .../plugins/condition/are_buttons_pressed.cpp | 3 +- 9 files changed, 94 insertions(+), 51 deletions(-) diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index 4f2b046c7..169b3a13e 100644 --- a/panther_description/config/docking_components.yaml +++ b/panther_description/config/docking_components.yaml @@ -6,11 +6,11 @@ components: rpy: 0.0 0.0 0.0 device_namespace: wireless_charger - # - type: WCH02 - # parent_link: world - # xyz: 3.0 -2.0 1.0 - # rpy: -1.57 0.0 1.57 - # device_namespace: wireless_charger + - type: WCH02 + parent_link: world + xyz: 3.0 -2.0 1.0 + rpy: -1.57 0.0 1.57 + device_namespace: wireless_charger - type: CAM01 name: camera diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 7d3ecea15..4593b7167 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -37,7 +37,7 @@ docks: ["main_dock"] main_dock: type: panther_charging_dock - frame: /main_wibotic_station_link + frame: /wibotic_station_link pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot controller: diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index d386bd31e..84fa4b440 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -13,8 +13,8 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import ( EnvironmentVariable, LaunchConfiguration, @@ -26,12 +26,13 @@ def generate_launch_description(): - use_sim = LaunchConfiguration("use_sim") - declare_use_sim_arg = DeclareLaunchArgument( - "use_sim", - default_value="False", - description="Whether simulation is used", - choices=[True, False, "True", "False", "true", "false", "1", "0"], + docking_server_config_path = LaunchConfiguration("docking_server_config_path") + declare_docking_server_config_path_arg = DeclareLaunchArgument( + "docking_server_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] + ), + description=("Path to docking server configuration file."), ) namespace = LaunchConfiguration("namespace") @@ -41,13 +42,20 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) - docking_server_config_path = LaunchConfiguration("docking_server_config_path") - declare_docking_server_config_path_arg = DeclareLaunchArgument( - "docking_server_config_path", - default_value=PathJoinSubstitution( - [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] - ), - description=("Path to docking server configuration file."), + use_docking = LaunchConfiguration("use_docking") + declare_use_docking_arg = DeclareLaunchArgument( + "use_docking", + default_value="True", + description="Enable docking server.", + choices=["True", "False", "true", "false"], + ) + + use_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used.", + choices=["True", "False", "true", "false"], ) namespaced_docking_server_config = ReplaceString( @@ -58,18 +66,21 @@ def generate_launch_description(): docking_server_node = Node( package="opennav_docking", executable="opennav_docking", + namespace=namespace, parameters=[ namespaced_docking_server_config, {"use_sim_time": use_sim}, ], - namespace=namespace, + remappings=[("~/transition_event", "~/_transition_event")], emulate_tty=True, + condition=IfCondition(use_docking), ) docking_server_activate_node = Node( package="nav2_lifecycle_manager", executable="lifecycle_manager", name="nav2_docking_lifecycle_manager", + namespace=namespace, parameters=[ { "autostart": True, @@ -79,28 +90,15 @@ def generate_launch_description(): "use_sim_time": use_sim, }, ], - namespace=namespace, - ) - - station_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("panther_docking"), - "launch", - "station.launch.py", - ] - ), - ), - launch_arguments={"namespace": namespace}.items(), + condition=IfCondition(use_docking), ) return LaunchDescription( [ - declare_use_sim_arg, - declare_namespace_arg, declare_docking_server_config_path_arg, - station_launch, + declare_namespace_arg, + declare_use_docking_arg, + declare_use_sim_arg, docking_server_node, docking_server_activate_node, ] diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index ad7252e7a..aa55478a1 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -185,6 +185,19 @@ def generate_launch_description(): emulate_tty=True, ) + docking_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("panther_docking"), + "launch", + "docking.launch.py", + ] + ), + ), + launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), + ) + return LaunchDescription( [ declare_battery_config_path_arg, @@ -200,5 +213,6 @@ def generate_launch_description(): ekf_launch, simulate_components, gz_bridge, + docking_launch, ] ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 89e7baaac..184e07c1c 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -34,6 +34,7 @@ nav2_common panther_controller panther_description + panther_docking panther_lights panther_localization panther_manager diff --git a/panther_manager/behavior_trees/RobotStatesBT.btproj b/panther_manager/behavior_trees/RobotStatesBT.btproj index f632f1431..071fb3c96 100644 --- a/panther_manager/behavior_trees/RobotStatesBT.btproj +++ b/panther_manager/behavior_trees/RobotStatesBT.btproj @@ -1,9 +1,13 @@ - + + +Topic name for sensor_msgs/Joy message type + Vector of provided input + animation ID optional parameter diff --git a/panther_manager/behavior_trees/robot_states.xml b/panther_manager/behavior_trees/robot_states.xml index 3dbbc361e..75610df4c 100644 --- a/panther_manager/behavior_trees/robot_states.xml +++ b/panther_manager/behavior_trees/robot_states.xml @@ -19,9 +19,15 @@ - + + + + @@ -36,18 +42,17 @@ _onFailure="robot_state = ROBOT_STATE_ERROR" _post="docking_cmd = DOCKING_CMD_NONE"> - - + + _skipIf="docking_cmd == DOCKING_CMD_UNDOCK"/> - + + + Topic name for sensor_msgs/Joy message type + Vector of provided input + + + bool value indicating whether to use dock ID + dock ID + type of the dock + maximum staging time + bool value indicating whether to navigate to staging pose + action name to call + + + type of the dock + maximum time to get back to the staging pose + action name to call + + diff --git a/panther_manager/config/robot_states_manager.yaml b/panther_manager/config/robot_states_manager.yaml index 8ac9a7a12..93518ae39 100644 --- a/panther_manager/config/robot_states_manager.yaml +++ b/panther_manager/config/robot_states_manager.yaml @@ -19,5 +19,6 @@ - tick_after_timeout_bt_node ros_plugin_libs: - call_set_led_animation_service_bt_node + - are_buttons_pressed_bt_node - dock_robot_bt_node - undock_robot_bt_node diff --git a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp b/panther_manager/src/plugins/condition/are_buttons_pressed.cpp index 1aad81af7..2af992406 100644 --- a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp +++ b/panther_manager/src/plugins/condition/are_buttons_pressed.cpp @@ -24,12 +24,11 @@ BT::NodeStatus AreButtonsPressed::onTick(const std::shared_ptr>("buttons", buttons_); if (!last_msg) { - RCLCPP_WARN_STREAM(this->logger(), GetLoggerPrefix(name()) << "There is no joy messages!"); return BT::NodeStatus::FAILURE; } if (last_msg->buttons.size() < buttons_.size()) { - RCLCPP_ERROR_STREAM( + RCLCPP_WARN_STREAM( this->logger(), GetLoggerPrefix(name()) << "Joy message has " << last_msg->buttons.size() << " buttons, expected at least " << buttons_.size()); return BT::NodeStatus::FAILURE; From cd181ab07cebc82d5673aa62132e1fecea2a967d Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 17 Oct 2024 19:40:23 +0200 Subject: [PATCH 03/13] Clean up --- .../config/panther_docking_server.yaml | 2 +- panther_manager/CMakeLists.txt | 19 ++-- panther_manager/CONFIGURATION.md | 1 + .../behavior_trees/RobotStatesBT.btproj | 20 +++- .../behavior_trees/robot_states.xml | 39 ++++--- .../config/robot_states_manager.yaml | 4 +- .../panther_manager/behavior_tree_utils.hpp | 25 ++--- .../plugins/action/undock_robot_node.hpp | 2 +- ...buttons_pressed.hpp => check_bool_msg.hpp} | 24 ++--- .../plugins/condition/check_joy_msg.hpp | 61 +++++++++++ .../robot_states_manager_node.hpp | 6 +- .../src/plugins/action/undock_robot_node.cpp | 2 +- ...buttons_pressed.cpp => check_bool_msg.cpp} | 18 ++-- .../src/plugins/condition/check_joy_msg.cpp | 102 ++++++++++++++++++ .../src/robot_states_manager_node.cpp | 32 +++--- .../src/robot_states_manager_node_main.cpp | 2 +- ...ons_pressed.cpp => test_check_joy_msg.cpp} | 36 +++---- 17 files changed, 288 insertions(+), 107 deletions(-) rename panther_manager/include/panther_manager/plugins/condition/{are_buttons_pressed.hpp => check_bool_msg.hpp} (62%) create mode 100644 panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp rename panther_manager/src/plugins/condition/{are_buttons_pressed.cpp => check_bool_msg.cpp} (56%) create mode 100644 panther_manager/src/plugins/condition/check_joy_msg.cpp rename panther_manager/test/plugins/condition/{test_are_buttons_pressed.cpp => test_check_joy_msg.cpp} (66%) diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 4593b7167..7d3ecea15 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -37,7 +37,7 @@ docks: ["main_dock"] main_dock: type: panther_charging_dock - frame: /wibotic_station_link + frame: /main_wibotic_station_link pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot controller: diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index 0208bb2a3..e31ba49f3 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -16,6 +16,7 @@ set(PACKAGE_DEPENDENCIES rclcpp rclcpp_action sensor_msgs + std_msgs std_srvs yaml-cpp opennav_docking_msgs) @@ -59,9 +60,13 @@ add_library(undock_robot_bt_node SHARED src/plugins/action/undock_robot_node.cpp) list(APPEND plugin_libs undock_robot_bt_node) -add_library(are_buttons_pressed_bt_node SHARED - src/plugins/condition/are_buttons_pressed.cpp) -list(APPEND plugin_libs are_buttons_pressed_bt_node) +add_library(check_joy_msg_bt_node SHARED + src/plugins/condition/check_joy_msg.cpp) +list(APPEND plugin_libs check_joy_msg_bt_node) + +add_library(check_bool_msg_bt_node SHARED + src/plugins/condition/check_bool_msg.cpp) +list(APPEND plugin_libs check_bool_msg_bt_node) add_library(tick_after_timeout_bt_node SHARED src/plugins/decorator/tick_after_timeout_node.cpp) @@ -175,10 +180,10 @@ if(BUILD_TESTING) list(APPEND plugin_tests ${PROJECT_NAME}_test_undock_robot_node) ament_add_gtest( - ${PROJECT_NAME}_test_are_buttons_pressed - test/plugins/condition/test_are_buttons_pressed.cpp - src/plugins/condition/are_buttons_pressed.cpp) - list(APPEND plugin_tests ${PROJECT_NAME}_test_are_buttons_pressed) + ${PROJECT_NAME}_test_check_joy_msg + test/plugins/condition/test_check_joy_msg.cpp + src/plugins/condition/check_joy_msg.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_check_joy_msg) ament_add_gtest( ${PROJECT_NAME}_test_tick_after_timeout_node diff --git a/panther_manager/CONFIGURATION.md b/panther_manager/CONFIGURATION.md index 3317e43a2..3235368ef 100644 --- a/panther_manager/CONFIGURATION.md +++ b/panther_manager/CONFIGURATION.md @@ -207,5 +207,6 @@ To use your customized project, you can modify the `bt_project_file` ROS paramet Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `panther_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below: - Lights tree: `10.15.20.2:5555` +- RobotState tree: `10.15.20.2:5555` - Safety tree: `10.15.20.2:6666` - Shutdown tree: `10.15.20.2:7777` diff --git a/panther_manager/behavior_trees/RobotStatesBT.btproj b/panther_manager/behavior_trees/RobotStatesBT.btproj index 071fb3c96..964181491 100644 --- a/panther_manager/behavior_trees/RobotStatesBT.btproj +++ b/panther_manager/behavior_trees/RobotStatesBT.btproj @@ -3,17 +3,27 @@ - - -Topic name for sensor_msgs/Joy message type - Vector of provided input - animation ID optional parameter indicates if animation should repeat ROS service name + + ROS service name + + + +Topic name for sensor_msgs/Joy message type + Vector of provided axis inputs + + + +Topic name for sensor_msgs/Joy message type + Max timeout to accept the msg + Vector of provided axis inputs + Vector of provided button inputs + bool value indicating whether to use dock ID dock ID diff --git a/panther_manager/behavior_trees/robot_states.xml b/panther_manager/behavior_trees/robot_states.xml index 75610df4c..c684d0fd9 100644 --- a/panther_manager/behavior_trees/robot_states.xml +++ b/panther_manager/behavior_trees/robot_states.xml @@ -1,12 +1,12 @@ - + - + - + - - + + - + + _skipIf="docking_cmd != DOCKING_CMD_UNDOCK"/> - + ROS service name + + Topic name for sensor_msgs/Joy message type - Vector of provided input + Max timeout to accept the msg + Vector of provided axis inputs + Vector of provided button inputs diff --git a/panther_manager/config/robot_states_manager.yaml b/panther_manager/config/robot_states_manager.yaml index 93518ae39..adeef4127 100644 --- a/panther_manager/config/robot_states_manager.yaml +++ b/panther_manager/config/robot_states_manager.yaml @@ -18,7 +18,7 @@ plugin_libs: - tick_after_timeout_bt_node ros_plugin_libs: - - call_set_led_animation_service_bt_node - - are_buttons_pressed_bt_node + - call_trigger_service_bt_node + - check_joy_msg_bt_node - dock_robot_bt_node - undock_robot_bt_node diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp index 5ee2331e3..655a004fa 100644 --- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp +++ b/panther_manager/include/panther_manager/behavior_tree_utils.hpp @@ -93,28 +93,25 @@ inline std::string GetLoggerPrefix(const std::string & bt_node_name) namespace BT { /** - * @brief Converts a string to a vector of integers. + * @brief Converts a string to a vector of float. * * @param str The string to convert. - * @return std::vector The vector of integers. + * @return std::vector The vector of float. * - * @throw BT::RuntimeError Throws when there is no input or cannot parse int. + * @throw BT::RuntimeError Throws when there is no input or cannot parse float. */ template <> -inline std::vector convertFromString>(StringView str) +inline std::vector convertFromString>(StringView str) { - auto parts = BT::splitString(str, ';'); - if (!parts.size()) { - throw BT::RuntimeError("invalid input"); - } else { - std::vector result; - for (auto & part : parts) { - result.push_back(convertFromString(part)); - } - - return result; + auto parts = splitString(str, ';'); + std::vector output; + output.reserve(parts.size()); + for (const StringView & part : parts) { + output.push_back(convertFromString(part)); } + return output; } + } // namespace BT #endif // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp index 157ca58da..baede6a2f 100644 --- a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp @@ -55,7 +55,7 @@ class UndockRobot : public BT::RosActionNode( "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"), - // BT::OutputPort("error_code", "Error code"), + BT::OutputPort("error_code", "Error code"), }); } }; diff --git a/panther_manager/include/panther_manager/plugins/condition/are_buttons_pressed.hpp b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp similarity index 62% rename from panther_manager/include/panther_manager/plugins/condition/are_buttons_pressed.hpp rename to panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp index 8b0534b5e..c110b970d 100644 --- a/panther_manager/include/panther_manager/plugins/condition/are_buttons_pressed.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_ -#define PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_ +#ifndef PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_ +#define PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_ #include #include @@ -22,34 +22,34 @@ #include #include -#include +#include #include "panther_manager/behavior_tree_utils.hpp" namespace panther_manager { -class AreButtonsPressed : public BT::RosTopicSubNode +class CheckBoolMsg : public BT::RosTopicSubNode { + using BoolMsg = std_msgs::msg::Bool; + public: - AreButtonsPressed( + CheckBoolMsg( const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) - : BT::RosTopicSubNode(name, conf, params) + : BT::RosTopicSubNode(name, conf, params) { + // TODO: There is no possibility to set QoS profile } - BT::NodeStatus onTick(const std::shared_ptr & last_msg); + BT::NodeStatus onTick(const BoolMsg::SharedPtr & last_msg); static BT::PortsList providedPorts() { return providedBasicPorts( - {BT::InputPort>("buttons", "state of buttons to accept a condition")}); + {BT::InputPort>("data", "state of data to accept a condition")}); } - -private: - std::vector buttons_; }; } // namespace panther_manager -#endif // PANTHER_MANAGER_PLUGINS_CONDITION_ARE_BUTTONS_PRESSED_HPP_ +#endif // PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_BOOL_MSG_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp new file mode 100644 index 000000000..dded1c29c --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ +#define PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ + +#include +#include +#include + +#include +#include + +#include + +#include "panther_manager/behavior_tree_utils.hpp" + +namespace panther_manager +{ + +class CheckJoyMsg : public BT::RosTopicSubNode +{ + using JoyMsg = sensor_msgs::msg::Joy; + +public: + CheckJoyMsg( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosTopicSubNode(name, conf, params) + { + } + + BT::NodeStatus onTick(const JoyMsg::SharedPtr & last_msg); + + static BT::PortsList providedPorts() + { + return providedBasicPorts( + {BT::InputPort>("axes", "state of axes to accept a condition"), + BT::InputPort>("buttons", "state of buttons to accept a condition"), + BT::InputPort("timeout", 0.0, "max time delay to accept a condition")}); + } + +private: + bool checkAxes(const JoyMsg::SharedPtr & last_msg); + bool checkButtons(const JoyMsg::SharedPtr & last_msg); + bool checkTimeout(const JoyMsg::SharedPtr & last_msg); +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_PLUGINS_CONDITION_CHECK_JOY_MSG_HPP_ diff --git a/panther_manager/include/panther_manager/robot_states_manager_node.hpp b/panther_manager/include/panther_manager/robot_states_manager_node.hpp index 5fce64ba3..ecc784324 100644 --- a/panther_manager/include/panther_manager/robot_states_manager_node.hpp +++ b/panther_manager/include/panther_manager/robot_states_manager_node.hpp @@ -45,12 +45,12 @@ using LEDAnimationMsg = panther_msgs::msg::LEDAnimation; * @brief This class is responsible for creating a BehaviorTree responsible for docking management, * spinning it, and updating blackboard entries based on subscribed topics. */ -class DockingManagerNode : public rclcpp::Node +class RobotStateManagerNode : public rclcpp::Node { public: - DockingManagerNode( + RobotStateManagerNode( const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ~DockingManagerNode() {} + ~RobotStateManagerNode() {} void Initialize(); diff --git a/panther_manager/src/plugins/action/undock_robot_node.cpp b/panther_manager/src/plugins/action/undock_robot_node.cpp index 89b7ac10c..e47c000a9 100644 --- a/panther_manager/src/plugins/action/undock_robot_node.cpp +++ b/panther_manager/src/plugins/action/undock_robot_node.cpp @@ -39,7 +39,7 @@ BT::NodeStatus UndockRobot::onResultReceived(const WrappedResult & wr) { const auto & result = wr.result; - // this->setOutput("error_code", result->error_code); + this->setOutput("error_code", result->error_code); if (result->success) { return BT::NodeStatus::SUCCESS; diff --git a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp b/panther_manager/src/plugins/condition/check_bool_msg.cpp similarity index 56% rename from panther_manager/src/plugins/condition/are_buttons_pressed.cpp rename to panther_manager/src/plugins/condition/check_bool_msg.cpp index 2af992406..5ece19bec 100644 --- a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp +++ b/panther_manager/src/plugins/condition/check_bool_msg.cpp @@ -12,29 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_manager/plugins/condition/are_buttons_pressed.hpp" +#include "panther_manager/plugins/condition/check_bool_msg.hpp" #include "panther_manager/behavior_tree_utils.hpp" namespace panther_manager { -BT::NodeStatus AreButtonsPressed::onTick(const std::shared_ptr & last_msg) +BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg) { - getInput>("buttons", buttons_); + bool expected_data; + getInput("data", expected_data); if (!last_msg) { return BT::NodeStatus::FAILURE; } - if (last_msg->buttons.size() < buttons_.size()) { - RCLCPP_WARN_STREAM( - this->logger(), GetLoggerPrefix(name()) << "Joy message has " << last_msg->buttons.size() - << " buttons, expected at least " << buttons_.size()); - return BT::NodeStatus::FAILURE; - } - - if (std::equal(buttons_.begin(), buttons_.end(), last_msg->buttons.begin())) { + if (expected_data == last_msg->data) { return BT::NodeStatus::SUCCESS; } @@ -44,4 +38,4 @@ BT::NodeStatus AreButtonsPressed::onTick(const std::shared_ptr expected_axes; + getInput>("axes", expected_axes); + + if (expected_axes.empty()) { + return true; + } + + if (last_msg->axes.size() < expected_axes.size()) { + RCLCPP_WARN_STREAM( + this->logger(), GetLoggerPrefix(name()) + << "Joy message has " << last_msg->axes.size() + << " axes, expected at least " << expected_axes.size()); + return false; + } + + if (std::equal(expected_axes.begin(), expected_axes.end(), last_msg->buttons.begin())) { + return true; + } + + return false; +} + +bool CheckJoyMsg::checkButtons(const JoyMsg::SharedPtr & last_msg) +{ + std::vector expected_buttons; + getInput>("buttons", expected_buttons); + + if (expected_buttons.empty()) { + return true; + } + + if (last_msg->buttons.size() < expected_buttons.size()) { + RCLCPP_WARN_STREAM( + this->logger(), GetLoggerPrefix(name()) + << "Joy message has " << last_msg->axes.size() + << " axes, expected at least " << expected_buttons.size()); + return false; + } + + if (std::equal(expected_buttons.begin(), expected_buttons.end(), last_msg->buttons.begin())) { + return true; + } + + return false; +} + +bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg) +{ + double max_timeout; + getInput("timeout", max_timeout); + + if (max_timeout <= 0.0) { + return true; + } + + if (rclcpp::Clock().now().seconds() < last_msg->header.stamp.sec + max_timeout) { + return true; + } + + return false; +} + +} // namespace panther_manager + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(panther_manager::CheckJoyMsg, "CheckJoyMsg"); diff --git a/panther_manager/src/robot_states_manager_node.cpp b/panther_manager/src/robot_states_manager_node.cpp index 605076024..71dfff713 100644 --- a/panther_manager/src/robot_states_manager_node.cpp +++ b/panther_manager/src/robot_states_manager_node.cpp @@ -34,7 +34,7 @@ namespace panther_manager { -DockingManagerNode::DockingManagerNode( +RobotStateManagerNode::RobotStateManagerNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { @@ -50,12 +50,12 @@ DockingManagerNode::DockingManagerNode( const auto initial_blackboard = CreateBlackboard(); docking_tree_manager_ = std::make_unique( - "Docking", initial_blackboard, 5557); + "RobotStates", initial_blackboard, 5557); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } -void DockingManagerNode::Initialize() +void RobotStateManagerNode::Initialize() { RCLCPP_INFO(this->get_logger(), "Initializing."); @@ -65,27 +65,27 @@ void DockingManagerNode::Initialize() using namespace std::placeholders; battery_sub_ = this->create_subscription( - "battery/battery_status", 10, std::bind(&DockingManagerNode::BatteryCB, this, _1)); + "battery/battery_status", 10, std::bind(&RobotStateManagerNode::BatteryCB, this, _1)); e_stop_sub_ = this->create_subscription( "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&DockingManagerNode::EStopCB, this, _1)); + std::bind(&RobotStateManagerNode::EStopCB, this, _1)); robot_state_pub_ = this->create_publisher( "robot_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); docking_srv_ = this->create_service( - "docking", std::bind(&DockingManagerNode::DockingSrvCB, this, _1, _2)); + "docking", std::bind(&RobotStateManagerNode::DockingSrvCB, this, _1, _2)); const float timer_freq = this->get_parameter("timer_frequency").as_double(); const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); docking_tree_timer_ = this->create_wall_timer( - timer_period_ms, std::bind(&DockingManagerNode::DockingTreeTimerCB, this)); + timer_period_ms, std::bind(&RobotStateManagerNode::DockingTreeTimerCB, this)); RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void DockingManagerNode::DeclareParameters() +void RobotStateManagerNode::DeclareParameters() { const auto panther_manager_pkg_path = ament_index_cpp::get_package_share_directory("panther_manager"); @@ -108,7 +108,7 @@ void DockingManagerNode::DeclareParameters() this->declare_parameter("timer_frequency", 10.0); } -void DockingManagerNode::RegisterBehaviorTree() +void RobotStateManagerNode::RegisterBehaviorTree() { const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); @@ -134,7 +134,7 @@ void DockingManagerNode::RegisterBehaviorTree() this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str()); } -std::map DockingManagerNode::CreateBlackboard() +std::map RobotStateManagerNode::CreateBlackboard() { update_charging_anim_step_ = this->get_parameter("battery.charging_anim_step").as_double(); const float critical_battery_anim_period = @@ -194,7 +194,7 @@ std::map DockingManagerNode::CreateBlackboard() return docking_initial_bb; } -void DockingManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery_state) +void RobotStateManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery_state) { const auto battery_status = battery_state->power_supply_status; const auto battery_health = battery_state->power_supply_health; @@ -217,12 +217,12 @@ void DockingManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery_stat update_charging_anim_step_)); } -void DockingManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) +void RobotStateManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) { docking_tree_manager_->GetBlackboard()->set("e_stop_state", e_stop->data); } -void DockingManagerNode::DockingSrvCB( +void RobotStateManagerNode::DockingSrvCB( const SetBoolSrv::Request::SharedPtr & request, SetBoolSrv::Response::SharedPtr response) { int robot_state; @@ -246,7 +246,7 @@ void DockingManagerNode::DockingSrvCB( response->success = true; } -void DockingManagerNode::DockingTreeTimerCB() +void RobotStateManagerNode::DockingTreeTimerCB() { if (!SystemReady()) { return; @@ -260,7 +260,7 @@ void DockingManagerNode::DockingTreeTimerCB() } } -void DockingManagerNode::PublishRobotState() +void RobotStateManagerNode::PublishRobotState() { Int8Msg robot_state; if (docking_tree_manager_->GetBlackboard()->get("robot_state", robot_state.data)) { @@ -268,7 +268,7 @@ void DockingManagerNode::PublishRobotState() } } -bool DockingManagerNode::SystemReady() +bool RobotStateManagerNode::SystemReady() { if ( !docking_tree_manager_->GetBlackboard()->getEntry("e_stop_state") || diff --git a/panther_manager/src/robot_states_manager_node_main.cpp b/panther_manager/src/robot_states_manager_node_main.cpp index 1a90594e0..1f7e10101 100644 --- a/panther_manager/src/robot_states_manager_node_main.cpp +++ b/panther_manager/src/robot_states_manager_node_main.cpp @@ -23,7 +23,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto robot_states_manager_node = - std::make_shared("robot_states_manager"); + std::make_shared("robot_states_manager"); robot_states_manager_node->Initialize(); try { diff --git a/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp similarity index 66% rename from panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp rename to panther_manager/test/plugins/condition/test_check_joy_msg.cpp index b563e7d59..15150394f 100644 --- a/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp +++ b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp @@ -24,13 +24,13 @@ #include -#include "panther_manager/plugins/condition/are_buttons_pressed.hpp" +#include "panther_manager/plugins/condition/check_joy_msg.hpp" #include "utils/plugin_test_utils.hpp" -class TestAreButtonsPressed : public panther_manager::plugin_test_utils::PluginTestUtils +class TestCheckJoyMsg : public panther_manager::plugin_test_utils::PluginTestUtils { public: - TestAreButtonsPressed(); + TestCheckJoyMsg(); void PublishJoyMessage(const std::vector & buttons); protected: @@ -38,36 +38,36 @@ class TestAreButtonsPressed : public panther_manager::plugin_test_utils::PluginT std::map params_ = {{"topic_name", "joy"}, {"buttons", "0;1;0"}}; }; -TestAreButtonsPressed::TestAreButtonsPressed() +TestCheckJoyMsg::TestCheckJoyMsg() { - RegisterNodeWithParams("AreButtonsPressed"); + RegisterNodeWithParams("CheckJoyMsg"); joy_publisher_ = bt_node_->create_publisher("joy", 10); } -void TestAreButtonsPressed::PublishJoyMessage(const std::vector & buttons) +void TestCheckJoyMsg::PublishJoyMessage(const std::vector & buttons) { sensor_msgs::msg::Joy msg; msg.buttons = buttons; joy_publisher_->publish(msg); } -TEST_F(TestAreButtonsPressed, LoadingAreButtonsPressedPlugin) +TEST_F(TestCheckJoyMsg, LoadingCheckJoyMsgPlugin) { - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); } -TEST_F(TestAreButtonsPressed, NoMessage) +TEST_F(TestCheckJoyMsg, NoMessage) { - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST_F(TestAreButtonsPressed, WrongMessageTooFewButtons) +TEST_F(TestCheckJoyMsg, WrongMessageTooFewButtons) { - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); PublishJoyMessage({0, 1}); @@ -76,9 +76,9 @@ TEST_F(TestAreButtonsPressed, WrongMessageTooFewButtons) EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST_F(TestAreButtonsPressed, GoodMessageWrongButtonsState) +TEST_F(TestCheckJoyMsg, GoodMessageWrongButtonsState) { - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); PublishJoyMessage({0, 0, 0}); @@ -87,9 +87,9 @@ TEST_F(TestAreButtonsPressed, GoodMessageWrongButtonsState) EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST_F(TestAreButtonsPressed, GoodMessageWithTooMuchButtonsAndGoodButtonsState) +TEST_F(TestCheckJoyMsg, GoodMessageWithTooMuchButtonsAndGoodButtonsState) { - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); PublishJoyMessage({0, 1, 0, 0, 0, 1}); @@ -98,9 +98,9 @@ TEST_F(TestAreButtonsPressed, GoodMessageWithTooMuchButtonsAndGoodButtonsState) EXPECT_EQ(status, BT::NodeStatus::SUCCESS); } -TEST_F(TestAreButtonsPressed, GoodMessageGoodButtonsState) +TEST_F(TestCheckJoyMsg, GoodMessageGoodButtonsState) { - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); PublishJoyMessage({0, 1, 0}); From 8d9f024a2a45f3005f1ff4b398a8502fc36a334f Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 18 Oct 2024 11:58:49 +0200 Subject: [PATCH 04/13] Clean up and add types enums --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- .../config/robot_states_manager.yaml | 12 -- .../panther_manager/lights_manager_node.hpp | 7 +- .../robot_states_manager_node.hpp | 32 +-- .../include/panther_manager/types.hpp | 29 +++ panther_manager/src/lights_manager_node.cpp | 18 +- .../src/robot_states_manager_node.cpp | 196 ++++++------------ .../src/robot_states_manager_node_main.cpp | 2 +- .../plugins/condition/test_check_joy_msg.cpp | 51 +++-- 10 files changed, 150 insertions(+), 201 deletions(-) create mode 100644 panther_manager/include/panther_manager/types.hpp diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index c6df86e10..b9e9b0c57 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -10,7 +10,7 @@ repositories: panther_msgs: type: git url: https://github.com/husarion/panther_msgs.git - version: fcee4d9f249a62adc113eb80be4885b08024ee9c + version: 6eb04e3a8d30e9700b4600d0ad30415bc70fd6f9 ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index d6c6932fc..b9631fd71 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -10,7 +10,7 @@ repositories: panther_msgs: type: git url: https://github.com/husarion/panther_msgs.git - version: fcee4d9f249a62adc113eb80be4885b08024ee9c + version: 6eb04e3a8d30e9700b4600d0ad30415bc70fd6f9 ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git diff --git a/panther_manager/config/robot_states_manager.yaml b/panther_manager/config/robot_states_manager.yaml index adeef4127..06d2e221f 100644 --- a/panther_manager/config/robot_states_manager.yaml +++ b/panther_manager/config/robot_states_manager.yaml @@ -2,21 +2,9 @@ robot_states_manager: ros__parameters: timer_frequency: 50.0 - battery: - percent: - window_len: 6 - threshold: - low: 0.4 - critical: 0.1 - animation_period: - low: 30.0 - critical: 15.0 - charging_anim_step: 0.1 ros_communication_timeout: availability: 1.0 response: 1.0 - plugin_libs: - - tick_after_timeout_bt_node ros_plugin_libs: - call_trigger_service_bt_node - check_joy_msg_bt_node diff --git a/panther_manager/include/panther_manager/lights_manager_node.hpp b/panther_manager/include/panther_manager/lights_manager_node.hpp index 330c233ed..288b39ad2 100644 --- a/panther_manager/include/panther_manager/lights_manager_node.hpp +++ b/panther_manager/include/panther_manager/lights_manager_node.hpp @@ -26,6 +26,7 @@ #include "std_msgs/msg/int8.hpp" #include "panther_msgs/msg/led_animation.hpp" +#include "panther_msgs/msg/robot_state.hpp" #include "panther_utils/moving_average.hpp" @@ -36,8 +37,8 @@ namespace panther_manager using BatteryStateMsg = sensor_msgs::msg::BatteryState; using BoolMsg = std_msgs::msg::Bool; -using Int8Msg = std_msgs::msg::Int8; using LEDAnimationMsg = panther_msgs::msg::LEDAnimation; +using RobotStateMsg = panther_msgs::msg::RobotState; /** * @brief This class is responsible for creating a BehaviorTree responsible for lights management, @@ -71,14 +72,14 @@ class LightsManagerNode : public rclcpp::Node private: void BatteryCB(const BatteryStateMsg::SharedPtr battery); void EStopCB(const BoolMsg::SharedPtr e_stop); - void RobotStateCB(const Int8Msg::SharedPtr robot_state); + void RobotStateCB(const RobotStateMsg::SharedPtr robot_state); void LightsTreeTimerCB(); float update_charging_anim_step_; rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; - rclcpp::Subscription::SharedPtr robot_state_sub_; + rclcpp::Subscription::SharedPtr robot_state_sub_; rclcpp::TimerBase::SharedPtr lights_tree_timer_; std::unique_ptr> battery_percent_ma_; diff --git a/panther_manager/include/panther_manager/robot_states_manager_node.hpp b/panther_manager/include/panther_manager/robot_states_manager_node.hpp index ecc784324..f971e1971 100644 --- a/panther_manager/include/panther_manager/robot_states_manager_node.hpp +++ b/panther_manager/include/panther_manager/robot_states_manager_node.hpp @@ -21,13 +21,9 @@ #include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/battery_state.hpp" #include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/int8.hpp" -#include "std_srvs/srv/set_bool.hpp" - -#include "panther_msgs/msg/led_animation.hpp" +#include "panther_msgs/msg/robot_state.hpp" #include "panther_utils/moving_average.hpp" #include @@ -35,22 +31,19 @@ namespace panther_manager { -using BatteryStateMsg = sensor_msgs::msg::BatteryState; using BoolMsg = std_msgs::msg::Bool; -using Int8Msg = std_msgs::msg::Int8; -using SetBoolSrv = std_srvs::srv::SetBool; -using LEDAnimationMsg = panther_msgs::msg::LEDAnimation; +using RobotStateMsg = panther_msgs::msg::RobotState; /** * @brief This class is responsible for creating a BehaviorTree responsible for docking management, * spinning it, and updating blackboard entries based on subscribed topics. */ -class RobotStateManagerNode : public rclcpp::Node +class RobotStatesManagerNode : public rclcpp::Node { public: - RobotStateManagerNode( + RobotStatesManagerNode( const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ~RobotStateManagerNode() {} + ~RobotStatesManagerNode() {} void Initialize(); @@ -71,23 +64,16 @@ class RobotStateManagerNode : public rclcpp::Node std::unique_ptr docking_tree_manager_; private: - void BatteryCB(const BatteryStateMsg::SharedPtr battery); void EStopCB(const BoolMsg::SharedPtr e_stop); - void DockingSrvCB( - const SetBoolSrv::Request::SharedPtr & request, SetBoolSrv::Response::SharedPtr response); - void DockingTreeTimerCB(); - - void PublishRobotState(); + void RobotStatesTimerCB(); - float update_charging_anim_step_; + void PublishRobotStateMsg(); + RobotStateMsg CreateRobotStateMsg(int8_t state_id); - rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; - rclcpp::Publisher::SharedPtr robot_state_pub_; - rclcpp::Service::SharedPtr docking_srv_; + rclcpp::Publisher::SharedPtr robot_state_pub_; rclcpp::TimerBase::SharedPtr docking_tree_timer_; - std::unique_ptr> battery_percent_ma_; BT::BehaviorTreeFactory factory_; }; diff --git a/panther_manager/include/panther_manager/types.hpp b/panther_manager/include/panther_manager/types.hpp new file mode 100644 index 000000000..4fa55c1e3 --- /dev/null +++ b/panther_manager/include/panther_manager/types.hpp @@ -0,0 +1,29 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_MANAGER_TYPES_HPP_ +#define PANTHER_MANAGER_TYPES_HPP_ + +namespace panther_manager +{ + +enum DockingCmd : unsigned { + DOCKING_CMD_NONE = 0U, + DOCKING_CMD_DOCK = 1U, + DOCKING_CMD_UNDOCK = 2U, +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_TYPES_HPP_ diff --git a/panther_manager/src/lights_manager_node.cpp b/panther_manager/src/lights_manager_node.cpp index 2f04bd85e..6869a1dd6 100644 --- a/panther_manager/src/lights_manager_node.cpp +++ b/panther_manager/src/lights_manager_node.cpp @@ -68,7 +68,7 @@ void LightsManagerNode::Initialize() e_stop_sub_ = this->create_subscription( "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::EStopCB, this, _1)); - robot_state_sub_ = this->create_subscription( + robot_state_sub_ = this->create_subscription( "robot_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::RobotStateCB, this, _1)); @@ -149,17 +149,17 @@ std::map LightsManagerNode::CreateLightsInitialBlackboard const std::map lights_initial_bb = { {"charging_anim_percent", undefined_charging_anim_percent}, {"current_anim_id", undefined_anim_id}, - {"robot_state", 0}, {"CRITICAL_BATTERY_ANIM_PERIOD", critical_battery_anim_period}, {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent}, {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period}, {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent}, // robot states - {"ROBOT_STATE_ERROR", -1}, - {"ROBOT_STATE_ESTOP", 0}, - {"ROBOT_STATE_STANDBY", 1}, - {"ROBOT_STATE_DOCKING", 2}, - {"ROBOT_STATE_SUCCESS", 3}, + {"robot_state", int(RobotStateMsg::E_STOP)}, + {"ROBOT_STATE_ERROR", int(RobotStateMsg::ERROR)}, + {"ROBOT_STATE_ESTOP", int(RobotStateMsg::E_STOP)}, + {"ROBOT_STATE_STANDBY", int(RobotStateMsg::STANDBY)}, + {"ROBOT_STATE_DOCKING", int(RobotStateMsg::DOCKING)}, + {"ROBOT_STATE_SUCCESS", int(RobotStateMsg::SUCCESS)}, // anim constants {"E_STOP_ANIM_ID", unsigned(LEDAnimationMsg::E_STOP)}, {"READY_ANIM_ID", unsigned(LEDAnimationMsg::READY)}, @@ -227,9 +227,9 @@ void LightsManagerNode::LightsTreeTimerCB() } } -void LightsManagerNode::RobotStateCB(const Int8Msg::SharedPtr robot_state) +void LightsManagerNode::RobotStateCB(const RobotStateMsg::SharedPtr robot_state) { - lights_tree_manager_->GetBlackboard()->set("robot_state", robot_state->data); + lights_tree_manager_->GetBlackboard()->set("robot_state", robot_state->state_id); } bool LightsManagerNode::SystemReady() diff --git a/panther_manager/src/robot_states_manager_node.cpp b/panther_manager/src/robot_states_manager_node.cpp index 71dfff713..0a7228b8e 100644 --- a/panther_manager/src/robot_states_manager_node.cpp +++ b/panther_manager/src/robot_states_manager_node.cpp @@ -30,11 +30,12 @@ #include #include +#include namespace panther_manager { -RobotStateManagerNode::RobotStateManagerNode( +RobotStatesManagerNode::RobotStatesManagerNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { @@ -42,12 +43,6 @@ RobotStateManagerNode::RobotStateManagerNode( DeclareParameters(); - const auto battery_percent_window_len = - this->get_parameter("battery.percent.window_len").as_int(); - - battery_percent_ma_ = std::make_unique>( - battery_percent_window_len, 1.0); - const auto initial_blackboard = CreateBlackboard(); docking_tree_manager_ = std::make_unique( "RobotStates", initial_blackboard, 5557); @@ -55,7 +50,7 @@ RobotStateManagerNode::RobotStateManagerNode( RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } -void RobotStateManagerNode::Initialize() +void RobotStatesManagerNode::Initialize() { RCLCPP_INFO(this->get_logger(), "Initializing."); @@ -64,28 +59,23 @@ void RobotStateManagerNode::Initialize() using namespace std::placeholders; - battery_sub_ = this->create_subscription( - "battery/battery_status", 10, std::bind(&RobotStateManagerNode::BatteryCB, this, _1)); e_stop_sub_ = this->create_subscription( "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&RobotStateManagerNode::EStopCB, this, _1)); - robot_state_pub_ = this->create_publisher( + std::bind(&RobotStatesManagerNode::EStopCB, this, _1)); + robot_state_pub_ = this->create_publisher( "robot_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - docking_srv_ = this->create_service( - "docking", std::bind(&RobotStateManagerNode::DockingSrvCB, this, _1, _2)); - const float timer_freq = this->get_parameter("timer_frequency").as_double(); const auto timer_period_ms = std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); docking_tree_timer_ = this->create_wall_timer( - timer_period_ms, std::bind(&RobotStateManagerNode::DockingTreeTimerCB, this)); + timer_period_ms, std::bind(&RobotStatesManagerNode::RobotStatesTimerCB, this)); RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void RobotStateManagerNode::DeclareParameters() +void RobotStatesManagerNode::DeclareParameters() { const auto panther_manager_pkg_path = ament_index_cpp::get_package_share_directory("panther_manager"); @@ -99,16 +89,10 @@ void RobotStateManagerNode::DeclareParameters() 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); + this->declare_parameter("timer_frequency", 20.0); } -void RobotStateManagerNode::RegisterBehaviorTree() +void RobotStatesManagerNode::RegisterBehaviorTree() { const auto bt_project_path = this->get_parameter("bt_project_path").as_string(); @@ -134,145 +118,58 @@ void RobotStateManagerNode::RegisterBehaviorTree() this->get_logger(), "BehaviorTree registered from path '%s'", bt_project_path.c_str()); } -std::map RobotStateManagerNode::CreateBlackboard() +std::map RobotStatesManagerNode::CreateBlackboard() { - 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(); - - const std::string undefined_charging_anim_percent = ""; - const int undefined_anim_id = -1; - const std::map docking_initial_bb = { - {"charging_anim_percent", undefined_charging_anim_percent}, - {"current_anim_id", undefined_anim_id}, - {"docking_cmd", 0}, - {"robot_state", 0}, - {"CRITICAL_BATTERY_ANIM_PERIOD", critical_battery_anim_period}, - {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent}, - {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period}, - {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent}, + // docking states + {"docking_cmd", unsigned(DockingCmd::DOCKING_CMD_NONE)}, + {"DOCKING_CMD_NONE", unsigned(DockingCmd::DOCKING_CMD_NONE)}, + {"DOCKING_CMD_DOCK", unsigned(DockingCmd::DOCKING_CMD_DOCK)}, + {"DOCKING_CMD_UNDOCK", unsigned(DockingCmd::DOCKING_CMD_UNDOCK)}, // robot states - {"ROBOT_STATE_ERROR", -1}, - {"ROBOT_STATE_ESTOP", 0}, - {"ROBOT_STATE_STANDBY", 1}, - {"ROBOT_STATE_DOCKING", 2}, - {"ROBOT_STATE_SUCCESS", 3}, - // robot states - {"DOCKING_CMD_NONE", 0}, - {"DOCKING_CMD_DOCK", 1}, - {"DOCKING_CMD_UNDOCK", 2}, - // anim constants - {"E_STOP_ANIM_ID", unsigned(LEDAnimationMsg::E_STOP)}, - {"READY_ANIM_ID", unsigned(LEDAnimationMsg::READY)}, - {"ERROR_ANIM_ID", unsigned(LEDAnimationMsg::ERROR)}, - {"MANUAL_ACTION_ANIM_ID", unsigned(LEDAnimationMsg::MANUAL_ACTION)}, - {"AUTONOMOUS_ACTION_ANIM_ID", unsigned(LEDAnimationMsg::AUTONOMOUS_ACTION)}, - {"GOAL_ACHIEVED_ANIM_ID", unsigned(LEDAnimationMsg::GOAL_ACHIEVED)}, - {"LOW_BATTERY_ANIM_ID", unsigned(LEDAnimationMsg::LOW_BATTERY)}, - {"CRITICAL_BATTERY_ANIM_ID", unsigned(LEDAnimationMsg::CRITICAL_BATTERY)}, - {"BATTERY_STATE_ANIM_ID", unsigned(LEDAnimationMsg::BATTERY_STATE)}, - {"CHARGING_BATTERY_ANIM_ID", unsigned(LEDAnimationMsg::CHARGING_BATTERY)}, - // battery status constants - {"POWER_SUPPLY_STATUS_UNKNOWN", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN)}, - {"POWER_SUPPLY_STATUS_CHARGING", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_CHARGING)}, - {"POWER_SUPPLY_STATUS_DISCHARGING", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING)}, - {"POWER_SUPPLY_STATUS_NOT_CHARGING", - unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_NOT_CHARGING)}, - {"POWER_SUPPLY_STATUS_FULL", unsigned(BatteryStateMsg::POWER_SUPPLY_STATUS_FULL)}, - // battery health constants - {"POWER_SUPPLY_HEALTH_OVERHEAT", unsigned(BatteryStateMsg::POWER_SUPPLY_HEALTH_OVERHEAT)}, + {"robot_state", int(RobotStateMsg::E_STOP)}, + {"ROBOT_STATE_ERROR", int(RobotStateMsg::ERROR)}, + {"ROBOT_STATE_ESTOP", int(RobotStateMsg::E_STOP)}, + {"ROBOT_STATE_STANDBY", int(RobotStateMsg::STANDBY)}, + {"ROBOT_STATE_DOCKING", int(RobotStateMsg::DOCKING)}, + {"ROBOT_STATE_SUCCESS", int(RobotStateMsg::SUCCESS)}, }; RCLCPP_INFO(this->get_logger(), "Blackboard created."); return docking_initial_bb; } -void RobotStateManagerNode::BatteryCB(const BatteryStateMsg::SharedPtr battery_state) -{ - const auto battery_status = battery_state->power_supply_status; - const auto battery_health = battery_state->power_supply_health; - docking_tree_manager_->GetBlackboard()->set("battery_status", battery_status); - docking_tree_manager_->GetBlackboard()->set("battery_health", battery_health); - - // don't update battery percentage if unknown status or health - if ( - battery_status != BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN && - battery_health != BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN) { - battery_percent_ma_->Roll(battery_state->percentage); - } - - docking_tree_manager_->GetBlackboard()->set( - "battery_percent", battery_percent_ma_->GetAverage()); - docking_tree_manager_->GetBlackboard()->set( - "battery_percent_round", - std::to_string( - round(battery_percent_ma_->GetAverage() / update_charging_anim_step_) * - update_charging_anim_step_)); -} - -void RobotStateManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) +void RobotStatesManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) { docking_tree_manager_->GetBlackboard()->set("e_stop_state", e_stop->data); } -void RobotStateManagerNode::DockingSrvCB( - const SetBoolSrv::Request::SharedPtr & request, SetBoolSrv::Response::SharedPtr response) -{ - int robot_state; - if (!docking_tree_manager_->GetBlackboard()->get("robot_state", robot_state)) { - throw std::runtime_error("Cannot get robot state from blackboard."); - } - - if (robot_state != 1) { - response->success = false; - response->message = "Cannot start docking/undocking robot is not in STANDBY state."; - return; - } - - if (request->data) { - docking_tree_manager_->GetBlackboard()->set("docking_cmd", 1); - response->message = "Docking Started"; - } else { - docking_tree_manager_->GetBlackboard()->set("docking_cmd", 2); - response->message = "Undocking Started"; - } - response->success = true; -} - -void RobotStateManagerNode::DockingTreeTimerCB() +void RobotStatesManagerNode::RobotStatesTimerCB() { if (!SystemReady()) { return; } docking_tree_manager_->TickOnce(); - PublishRobotState(); + PublishRobotStateMsg(); if (docking_tree_manager_->GetTreeStatus() == BT::NodeStatus::FAILURE) { RCLCPP_WARN(this->get_logger(), "Docking behavior tree returned FAILURE status"); } } -void RobotStateManagerNode::PublishRobotState() +void RobotStatesManagerNode::PublishRobotStateMsg() { - Int8Msg robot_state; - if (docking_tree_manager_->GetBlackboard()->get("robot_state", robot_state.data)) { - robot_state_pub_->publish(robot_state); + int8_t state_id; + if (docking_tree_manager_->GetBlackboard()->get("robot_state", state_id)) { + auto msg = CreateRobotStateMsg(state_id); + robot_state_pub_->publish(msg); } } -bool RobotStateManagerNode::SystemReady() +bool RobotStatesManagerNode::SystemReady() { - if ( - !docking_tree_manager_->GetBlackboard()->getEntry("e_stop_state") || - !docking_tree_manager_->GetBlackboard()->getEntry("battery_status")) { + if (!docking_tree_manager_->GetBlackboard()->getEntry("e_stop_state")) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "Waiting for required system messages to arrive."); @@ -282,4 +179,35 @@ bool RobotStateManagerNode::SystemReady() return true; } +RobotStateMsg RobotStatesManagerNode::CreateRobotStateMsg(int8_t state_id) +{ + RobotStateMsg msg; + + if (state_id < 0) { + msg.state_id = RobotStateMsg::ERROR; + msg.state_name = "ERROR"; + return msg; + } + + msg.state_id = state_id; + switch (state_id) { + case RobotStateMsg::E_STOP: + msg.state_name = "E_STOP"; + break; + case RobotStateMsg::STANDBY: + msg.state_name = "STANDBY"; + break; + case RobotStateMsg::DOCKING: + msg.state_name = "DOCKING"; + break; + case RobotStateMsg::SUCCESS: + msg.state_name = "SUCCESS"; + break; + default: + throw std::runtime_error("Invalid state_id"); + } + + return msg; +} + } // namespace panther_manager diff --git a/panther_manager/src/robot_states_manager_node_main.cpp b/panther_manager/src/robot_states_manager_node_main.cpp index 1f7e10101..66bb84353 100644 --- a/panther_manager/src/robot_states_manager_node_main.cpp +++ b/panther_manager/src/robot_states_manager_node_main.cpp @@ -23,7 +23,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto robot_states_manager_node = - std::make_shared("robot_states_manager"); + std::make_shared("robot_states_manager"); robot_states_manager_node->Initialize(); try { diff --git a/panther_manager/test/plugins/condition/test_check_joy_msg.cpp b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp index 15150394f..88a451141 100644 --- a/panther_manager/test/plugins/condition/test_check_joy_msg.cpp +++ b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp @@ -23,42 +23,55 @@ #include #include +#include #include "panther_manager/plugins/condition/check_joy_msg.hpp" #include "utils/plugin_test_utils.hpp" +using JoyMsg = sensor_msgs::msg::Joy; +using HeaderMsg = std_msgs::msg::Header; + class TestCheckJoyMsg : public panther_manager::plugin_test_utils::PluginTestUtils { public: TestCheckJoyMsg(); - void PublishJoyMessage(const std::vector & buttons); + JoyMsg CreateJoyMsg( + const HeaderMsg & header = HeaderMsg(), const std::vector & axes = {}, + const std::vector & buttons = {}); + void PublishJoyMsg(JoyMsg msg); protected: - rclcpp::Publisher::SharedPtr joy_publisher_; - std::map params_ = {{"topic_name", "joy"}, {"buttons", "0;1;0"}}; + rclcpp::Publisher::SharedPtr joy_publisher_; + std::map bb_ports_ = { + {"topic_name", "joy"}, {"axes", ""}, {"buttons", "0;1;0"}, {"timeout", "0.0"}}; }; TestCheckJoyMsg::TestCheckJoyMsg() { RegisterNodeWithParams("CheckJoyMsg"); - joy_publisher_ = bt_node_->create_publisher("joy", 10); + joy_publisher_ = bt_node_->create_publisher("joy", 10); } -void TestCheckJoyMsg::PublishJoyMessage(const std::vector & buttons) +JoyMsg TestCheckJoyMsg::CreateJoyMsg( + const HeaderMsg & header, const std::vector & axes, const std::vector & buttons) { - sensor_msgs::msg::Joy msg; + JoyMsg msg; + msg.header = header; + msg.axes = axes; msg.buttons = buttons; - joy_publisher_->publish(msg); + return msg; } +void TestCheckJoyMsg::PublishJoyMsg(JoyMsg msg) { joy_publisher_->publish(msg); } + TEST_F(TestCheckJoyMsg, LoadingCheckJoyMsgPlugin) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); } TEST_F(TestCheckJoyMsg, NoMessage) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); @@ -67,9 +80,10 @@ TEST_F(TestCheckJoyMsg, NoMessage) TEST_F(TestCheckJoyMsg, WrongMessageTooFewButtons) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - PublishJoyMessage({0, 1}); + auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 1}); + PublishJoyMsg(msg); auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); @@ -78,9 +92,10 @@ TEST_F(TestCheckJoyMsg, WrongMessageTooFewButtons) TEST_F(TestCheckJoyMsg, GoodMessageWrongButtonsState) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - PublishJoyMessage({0, 0, 0}); + auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 0, 0}); + PublishJoyMsg(msg); auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); @@ -89,9 +104,10 @@ TEST_F(TestCheckJoyMsg, GoodMessageWrongButtonsState) TEST_F(TestCheckJoyMsg, GoodMessageWithTooMuchButtonsAndGoodButtonsState) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - PublishJoyMessage({0, 1, 0, 0, 0, 1}); + auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 1, 0, 0, 0, 1}); + PublishJoyMsg(msg); auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); @@ -100,9 +116,10 @@ TEST_F(TestCheckJoyMsg, GoodMessageWithTooMuchButtonsAndGoodButtonsState) TEST_F(TestCheckJoyMsg, GoodMessageGoodButtonsState) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", params_); }); + ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - PublishJoyMessage({0, 1, 0}); + auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 1, 0}); + PublishJoyMsg(msg); auto & tree = GetTree(); auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); From 597635a13968d8f9ca9c3d05c0e869d536265e3b Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 18 Oct 2024 12:29:42 +0200 Subject: [PATCH 05/13] Simplifications --- README.md | 1 + .../behavior_trees/robot_states.xml | 16 +++++---- .../panther_manager/lights_manager_node.hpp | 1 - .../plugins/action/dock_robot_node.hpp | 6 ++-- .../plugins/action/undock_robot_node.hpp | 2 +- .../plugins/condition/check_bool_msg.hpp | 4 +-- panther_manager/launch/manager.launch.py | 2 +- .../src/plugins/condition/check_bool_msg.cpp | 11 ++---- .../src/plugins/condition/check_joy_msg.cpp | 35 ++++--------------- .../test/utils/plugin_test_utils.hpp | 5 +-- 10 files changed, 31 insertions(+), 52 deletions(-) diff --git a/README.md b/README.md index 633e44f3e..7ebf2ab0f 100644 --- a/README.md +++ b/README.md @@ -102,6 +102,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `namespace` | Add namespace to all launched nodes.
***string:*** `env(ROBOT_NAMESPACE)` | | 🤖🖥️ | `publish_robot_state` | Whether to publish the default Panther robot description.
***bool:*** `True` | | 🖥️ | `robots` | The list of the robots spawned in the simulation e.g. `robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'`
***string:*** `''` | +| 🤖🖥️ | `robot_states_bt_project_path` | Path to BehaviorTree project file, responsible for robot states management.
***string:*** [`RobotStatesBT.btproj`](./panther_manager/behavior_trees/RobotStatesBT.btproj) | | 🤖🖥️ | `safety_bt_project_path` | Path to BehaviorTree project file, responsible for safety and shutdown management.
***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj) | | 🤖🖥️ | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown.
***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml) | | 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | diff --git a/panther_manager/behavior_trees/robot_states.xml b/panther_manager/behavior_trees/robot_states.xml index c684d0fd9..dd0dd3f6d 100644 --- a/panther_manager/behavior_trees/robot_states.xml +++ b/panther_manager/behavior_trees/robot_states.xml @@ -19,14 +19,16 @@ - - - - - + - diff --git a/panther_manager/include/panther_manager/lights_manager_node.hpp b/panther_manager/include/panther_manager/lights_manager_node.hpp index 288b39ad2..a2e92ca2c 100644 --- a/panther_manager/include/panther_manager/lights_manager_node.hpp +++ b/panther_manager/include/panther_manager/lights_manager_node.hpp @@ -23,7 +23,6 @@ #include "sensor_msgs/msg/battery_state.hpp" #include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/int8.hpp" #include "panther_msgs/msg/led_animation.hpp" #include "panther_msgs/msg/robot_state.hpp" diff --git a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp index f2bed74b5..58933c068 100644 --- a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp @@ -57,8 +57,10 @@ class DockRobot : public BT::RosActionNode( "navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"), - BT::OutputPort("error_code", "Contextual error code, if any"), - BT::OutputPort("num_retries", "Number of retries attempted"), + BT::OutputPort( + "error_code", "Contextual error code, if any"), + BT::OutputPort( + "num_retries", "Number of retries attempted"), }); } }; diff --git a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp index baede6a2f..238fa5f31 100644 --- a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp @@ -55,7 +55,7 @@ class UndockRobot : public BT::RosActionNode( "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"), - BT::OutputPort("error_code", "Error code"), + BT::OutputPort("error_code", "Error code"), }); } }; diff --git a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp index c110b970d..f48506376 100644 --- a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp @@ -29,6 +29,7 @@ namespace panther_manager { +// FIXME: There is no possibility to set QoS profile. Add it in the future to subscribe e_stop. class CheckBoolMsg : public BT::RosTopicSubNode { using BoolMsg = std_msgs::msg::Bool; @@ -38,7 +39,6 @@ class CheckBoolMsg : public BT::RosTopicSubNode const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) : BT::RosTopicSubNode(name, conf, params) { - // TODO: There is no possibility to set QoS profile } BT::NodeStatus onTick(const BoolMsg::SharedPtr & last_msg); @@ -46,7 +46,7 @@ class CheckBoolMsg : public BT::RosTopicSubNode static BT::PortsList providedPorts() { return providedBasicPorts( - {BT::InputPort>("data", "state of data to accept a condition")}); + {BT::InputPort>("data", "state of data to accept a condition")}); } }; diff --git a/panther_manager/launch/manager.launch.py b/panther_manager/launch/manager.launch.py index 8b4f55f5b..2d78d058b 100644 --- a/panther_manager/launch/manager.launch.py +++ b/panther_manager/launch/manager.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): default_value=PathJoinSubstitution( [panther_manager_dir, "behavior_trees", "RobotStatesBT.btproj"] ), - description="Path to BehaviorTree project file, responsible for lights management.", + description="Path to BehaviorTree project file, responsible for robot states management.", ) lights_bt_project_path = LaunchConfiguration("lights_bt_project_path") diff --git a/panther_manager/src/plugins/condition/check_bool_msg.cpp b/panther_manager/src/plugins/condition/check_bool_msg.cpp index 5ece19bec..a6de3c376 100644 --- a/panther_manager/src/plugins/condition/check_bool_msg.cpp +++ b/panther_manager/src/plugins/condition/check_bool_msg.cpp @@ -24,15 +24,8 @@ BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg) bool expected_data; getInput("data", expected_data); - if (!last_msg) { - return BT::NodeStatus::FAILURE; - } - - if (expected_data == last_msg->data) { - return BT::NodeStatus::SUCCESS; - } - - return BT::NodeStatus::FAILURE; + return (last_msg && expected_data == last_msg->data) ? BT::NodeStatus::SUCCESS + : BT::NodeStatus::FAILURE; } } // namespace panther_manager diff --git a/panther_manager/src/plugins/condition/check_joy_msg.cpp b/panther_manager/src/plugins/condition/check_joy_msg.cpp index cad89913d..47bf89e1d 100644 --- a/panther_manager/src/plugins/condition/check_joy_msg.cpp +++ b/panther_manager/src/plugins/condition/check_joy_msg.cpp @@ -21,15 +21,9 @@ namespace panther_manager BT::NodeStatus CheckJoyMsg::onTick(const JoyMsg::SharedPtr & last_msg) { - if (!last_msg) { - return BT::NodeStatus::FAILURE; - } - - if (checkAxes(last_msg) && checkButtons(last_msg) && checkTimeout(last_msg)) { - return BT::NodeStatus::SUCCESS; - } - - return BT::NodeStatus::FAILURE; + return (last_msg && checkAxes(last_msg) && checkButtons(last_msg) && checkTimeout(last_msg)) + ? BT::NodeStatus::SUCCESS + : BT::NodeStatus::FAILURE; } bool CheckJoyMsg::checkAxes(const JoyMsg::SharedPtr & last_msg) @@ -49,11 +43,7 @@ bool CheckJoyMsg::checkAxes(const JoyMsg::SharedPtr & last_msg) return false; } - if (std::equal(expected_axes.begin(), expected_axes.end(), last_msg->buttons.begin())) { - return true; - } - - return false; + return std::equal(expected_axes.begin(), expected_axes.end(), last_msg->axes.begin()); } bool CheckJoyMsg::checkButtons(const JoyMsg::SharedPtr & last_msg) @@ -73,11 +63,7 @@ bool CheckJoyMsg::checkButtons(const JoyMsg::SharedPtr & last_msg) return false; } - if (std::equal(expected_buttons.begin(), expected_buttons.end(), last_msg->buttons.begin())) { - return true; - } - - return false; + return std::equal(expected_buttons.begin(), expected_buttons.end(), last_msg->buttons.begin()); } bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg) @@ -85,15 +71,8 @@ bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg) double max_timeout; getInput("timeout", max_timeout); - if (max_timeout <= 0.0) { - return true; - } - - if (rclcpp::Clock().now().seconds() < last_msg->header.stamp.sec + max_timeout) { - return true; - } - - return false; + return (max_timeout <= 0.0) || + (rclcpp::Clock().now().seconds() < last_msg->header.stamp.sec + max_timeout); } } // namespace panther_manager diff --git a/panther_manager/test/utils/plugin_test_utils.hpp b/panther_manager/test/utils/plugin_test_utils.hpp index d97ba8037..23d1d0c4f 100644 --- a/panther_manager/test/utils/plugin_test_utils.hpp +++ b/panther_manager/test/utils/plugin_test_utils.hpp @@ -116,6 +116,7 @@ class PluginTestUtils : public testing::Test template void CreateAction( + const std::string & action_name, std::function goal)> handle_goal, @@ -125,9 +126,9 @@ class PluginTestUtils : public testing::Test std::function> goal_handle)> handle_accepted) { - server_node_ = std::make_shared("test_node"); + server_node_ = std::make_shared("test_node_for_" + action_name); action_server_ = rclcpp_action::create_server( - server_node_, handle_goal, handle_cancel, handle_accepted); + server_node_, action_name, handle_goal, handle_cancel, handle_accepted); executor_ = std::make_unique(); executor_->add_node(server_node_); executor_thread_ = std::make_unique([this]() { executor_->spin(); }); From 6e4388f375531d68303daf98f31de962d76e2401 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 18 Oct 2024 12:35:58 +0200 Subject: [PATCH 06/13] Change ports --- panther_manager/CONFIGURATION.md | 6 ++-- panther_manager/src/safety_manager_node.cpp | 4 +-- .../plugins/action/test_dock_robot_node.cpp | 29 +++++++++---------- .../plugins/action/test_undock_robot_node.cpp | 11 +++---- 4 files changed, 24 insertions(+), 26 deletions(-) diff --git a/panther_manager/CONFIGURATION.md b/panther_manager/CONFIGURATION.md index 3235368ef..423814c6d 100644 --- a/panther_manager/CONFIGURATION.md +++ b/panther_manager/CONFIGURATION.md @@ -207,6 +207,6 @@ To use your customized project, you can modify the `bt_project_file` ROS paramet Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `panther_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below: - Lights tree: `10.15.20.2:5555` -- RobotState tree: `10.15.20.2:5555` -- Safety tree: `10.15.20.2:6666` -- Shutdown tree: `10.15.20.2:7777` +- RobotState tree: `10.15.20.2:5556` +- Safety tree: `10.15.20.2:5557` +- Shutdown tree: `10.15.20.2:5558` diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp index d204690ce..7b213c6f8 100644 --- a/panther_manager/src/safety_manager_node.cpp +++ b/panther_manager/src/safety_manager_node.cpp @@ -58,14 +58,14 @@ SafetyManagerNode::SafetyManagerNode( const auto safety_initial_blackboard = CreateSafetyInitialBlackboard(); safety_tree_manager_ = std::make_unique( - "Safety", safety_initial_blackboard, 6666); + "Safety", safety_initial_blackboard, 5557); const auto shutdown_hosts_path = this->get_parameter("shutdown_hosts_path").as_string(); const std::map shutdown_initial_blackboard = { {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path.c_str()}, }; shutdown_tree_manager_ = std::make_unique( - "Shutdown", shutdown_initial_blackboard, 7777); + "Shutdown", shutdown_initial_blackboard, 5558); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } diff --git a/panther_manager/test/plugins/action/test_dock_robot_node.cpp b/panther_manager/test/plugins/action/test_dock_robot_node.cpp index 3efc84c99..ca45c50c9 100644 --- a/panther_manager/test/plugins/action/test_dock_robot_node.cpp +++ b/panther_manager/test/plugins/action/test_dock_robot_node.cpp @@ -64,6 +64,7 @@ class TestDockRobot : public panther_manager::plugin_test_utils::PluginTestUtils TEST_F(TestDockRobot, GoodLoadingDockRobotPlugin) { std::map params = { + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, @@ -79,11 +80,9 @@ TEST_F(TestDockRobot, GoodLoadingDockRobotPlugin) TEST_F(TestDockRobot, WrongLoadingDockRobotPlugin) { std::map params = { - {"use_dock_id", "true"}, - {"dock_id", "test_dock"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, - {"navigate_to_staging_pose", "false"}, + {"action_name", ""}, {"use_dock_id", "true"}, + {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, + {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; RegisterNodeWithParams("DockRobot"); @@ -94,7 +93,7 @@ TEST_F(TestDockRobot, WrongLoadingDockRobotPlugin) TEST_F(TestDockRobot, WrongCallDockRobotServerNotInitialized) { std::map params = { - + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", "test_dock"}, {"dock_type", "test_dock_type"}, @@ -117,9 +116,8 @@ TEST_F(TestDockRobot, WrongCallDockRobotServerWithNoDockID) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"use_dock_id", "true"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, + {"dock_type", "test_dock_type"}, {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; @@ -137,9 +135,8 @@ TEST_F(TestDockRobot, CallDockRobotServerWithoutDockID) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - {"use_dock_id", "false"}, - {"dock_type", "test_dock_type"}, - {"max_staging_time", "5.0"}, + {"action_name", "test_dock_action"}, {"use_dock_id", "false"}, + {"dock_type", "test_dock_type"}, {"max_staging_time", "5.0"}, {"navigate_to_staging_pose", "false"}, }; @@ -157,7 +154,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockID) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", ""}, {"dock_type", "test_dock_type"}, @@ -179,7 +176,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithEmptyDockType) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", "main_dock"}, {"dock_type", ""}, @@ -200,7 +197,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseFailure) CreateActionServer(GoalResponse::REJECT, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", "main_dock"}, {"dock_type", "test_dock_type"}, @@ -222,7 +219,7 @@ TEST_F(TestDockRobot, CallDockRobotServerWithNavigateToStagingPoseSuccess) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_dock_action"}, {"use_dock_id", "true"}, {"dock_id", "main_dock"}, {"dock_type", "test_dock_type"}, diff --git a/panther_manager/test/plugins/action/test_undock_robot_node.cpp b/panther_manager/test/plugins/action/test_undock_robot_node.cpp index 728f2eb33..5820bfded 100644 --- a/panther_manager/test/plugins/action/test_undock_robot_node.cpp +++ b/panther_manager/test/plugins/action/test_undock_robot_node.cpp @@ -66,7 +66,7 @@ class TestUndockRobot : public panther_manager::plugin_test_utils::PluginTestUti TEST_F(TestUndockRobot, GoodLoadingUndockRobotPlugin) { std::map params = { - + {"action_name", "test_undock_action"}, {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -79,6 +79,7 @@ TEST_F(TestUndockRobot, GoodLoadingUndockRobotPlugin) TEST_F(TestUndockRobot, WrongLoadingUndockRobotPlugin) { std::map params = { + {"action_name", ""}, {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -91,7 +92,7 @@ TEST_F(TestUndockRobot, WrongLoadingUndockRobotPlugin) TEST_F(TestUndockRobot, WrongCallUndockRobotServerNotInitialized) { std::map params = { - + {"action_name", "test_undock_action"}, {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -112,7 +113,7 @@ TEST_F(TestUndockRobot, WrongCallUndockRobotServerWithNoDockType) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_undock_action"}, {"dock_type", ""}, {"max_undocking_time", "5.0"}, }; @@ -130,7 +131,7 @@ TEST_F(TestUndockRobot, CallUndockRobotServerFailure) CreateActionServer(GoalResponse::REJECT, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_undock_action"}, {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; @@ -149,7 +150,7 @@ TEST_F(TestUndockRobot, CallUndockRobotServerSuccess) GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, true, ActionResult::NONE); std::map params = { - + {"action_name", "test_undock_action"}, {"dock_type", "test_dock_type"}, {"max_undocking_time", "5.0"}, }; From 28f19d72a0729b88297ca6feeb7ee4003079bff1 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 18 Oct 2024 13:28:40 +0200 Subject: [PATCH 07/13] Add use_docking arg --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 7ebf2ab0f..6f628a764 100644 --- a/README.md +++ b/README.md @@ -105,6 +105,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `robot_states_bt_project_path` | Path to BehaviorTree project file, responsible for robot states management.
***string:*** [`RobotStatesBT.btproj`](./panther_manager/behavior_trees/RobotStatesBT.btproj) | | 🤖🖥️ | `safety_bt_project_path` | Path to BehaviorTree project file, responsible for safety and shutdown management.
***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj) | | 🤖🖥️ | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown.
***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml) | +| 🤖🖥️ | `use_docking` | Enable docking server.
***bool:*** `True` | | 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | | 🤖🖥️ | `use_sim` | Whether simulation is used.
***bool:*** `False` | | 🤖🖥️ | `user_led_animations_file` | Path to a YAML file with a description of the user-defined animations.
***string:*** `''` | From e37f87801b23de185440978b243cef6fdace539e Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 18 Oct 2024 16:19:10 +0200 Subject: [PATCH 08/13] Fix port --- panther_manager/src/robot_states_manager_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_manager/src/robot_states_manager_node.cpp b/panther_manager/src/robot_states_manager_node.cpp index 0a7228b8e..af345d04e 100644 --- a/panther_manager/src/robot_states_manager_node.cpp +++ b/panther_manager/src/robot_states_manager_node.cpp @@ -45,7 +45,7 @@ RobotStatesManagerNode::RobotStatesManagerNode( const auto initial_blackboard = CreateBlackboard(); docking_tree_manager_ = std::make_unique( - "RobotStates", initial_blackboard, 5557); + "RobotStates", initial_blackboard, 5556); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } From f7b2699f29d31801ca7cc507a147f24446e7957f Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 18 Oct 2024 22:10:00 +0200 Subject: [PATCH 09/13] Fix port --- .../panther_manager/plugins/condition/check_bool_msg.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp index f48506376..a2d953c4c 100644 --- a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp @@ -45,8 +45,7 @@ class CheckBoolMsg : public BT::RosTopicSubNode static BT::PortsList providedPorts() { - return providedBasicPorts( - {BT::InputPort>("data", "state of data to accept a condition")}); + return providedBasicPorts({BT::InputPort("data", "state of data to accept a condition")}); } }; From 2fe2078d17fcbc32dcae059d67b9ffc582e8e707 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 21 Oct 2024 13:26:33 +0200 Subject: [PATCH 10/13] Add more cases to tests --- panther_manager/CMakeLists.txt | 14 +- .../src/plugins/condition/check_joy_msg.cpp | 4 +- .../plugins/condition/test_check_bool_msg.cpp | 107 +++++++++++ .../plugins/condition/test_check_joy_msg.cpp | 179 ++++++++++++------ 4 files changed, 238 insertions(+), 66 deletions(-) create mode 100644 panther_manager/test/plugins/condition/test_check_bool_msg.cpp diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index e31ba49f3..f9d74002f 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -60,14 +60,14 @@ add_library(undock_robot_bt_node SHARED src/plugins/action/undock_robot_node.cpp) list(APPEND plugin_libs undock_robot_bt_node) -add_library(check_joy_msg_bt_node SHARED - src/plugins/condition/check_joy_msg.cpp) -list(APPEND plugin_libs check_joy_msg_bt_node) - add_library(check_bool_msg_bt_node SHARED src/plugins/condition/check_bool_msg.cpp) list(APPEND plugin_libs check_bool_msg_bt_node) +add_library(check_joy_msg_bt_node SHARED + src/plugins/condition/check_joy_msg.cpp) +list(APPEND plugin_libs check_joy_msg_bt_node) + add_library(tick_after_timeout_bt_node SHARED src/plugins/decorator/tick_after_timeout_node.cpp) list(APPEND plugin_libs tick_after_timeout_bt_node) @@ -179,6 +179,12 @@ if(BUILD_TESTING) src/plugins/action/undock_robot_node.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_undock_robot_node) + ament_add_gtest( + ${PROJECT_NAME}_test_check_bool_msg + test/plugins/condition/test_check_bool_msg.cpp + src/plugins/condition/check_bool_msg.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_check_bool_msg) + ament_add_gtest( ${PROJECT_NAME}_test_check_joy_msg test/plugins/condition/test_check_joy_msg.cpp diff --git a/panther_manager/src/plugins/condition/check_joy_msg.cpp b/panther_manager/src/plugins/condition/check_joy_msg.cpp index 47bf89e1d..f99f476a3 100644 --- a/panther_manager/src/plugins/condition/check_joy_msg.cpp +++ b/panther_manager/src/plugins/condition/check_joy_msg.cpp @@ -35,7 +35,7 @@ bool CheckJoyMsg::checkAxes(const JoyMsg::SharedPtr & last_msg) return true; } - if (last_msg->axes.size() < expected_axes.size()) { + if (last_msg->axes.size() != expected_axes.size()) { RCLCPP_WARN_STREAM( this->logger(), GetLoggerPrefix(name()) << "Joy message has " << last_msg->axes.size() @@ -55,7 +55,7 @@ bool CheckJoyMsg::checkButtons(const JoyMsg::SharedPtr & last_msg) return true; } - if (last_msg->buttons.size() < expected_buttons.size()) { + if (last_msg->buttons.size() != expected_buttons.size()) { RCLCPP_WARN_STREAM( this->logger(), GetLoggerPrefix(name()) << "Joy message has " << last_msg->axes.size() diff --git a/panther_manager/test/plugins/condition/test_check_bool_msg.cpp b/panther_manager/test/plugins/condition/test_check_bool_msg.cpp new file mode 100644 index 000000000..f4f26667e --- /dev/null +++ b/panther_manager/test/plugins/condition/test_check_bool_msg.cpp @@ -0,0 +1,107 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include "panther_manager/plugins/condition/check_bool_msg.hpp" +#include "utils/plugin_test_utils.hpp" + +using BoolMsg = std_msgs::msg::Bool; +using bt_ports = std::map; + +struct TestCase +{ + BT::NodeStatus result; + bt_ports input; + BoolMsg msg; +}; + +constexpr auto TOPIC = "bool"; +constexpr auto PLUGIN = "CheckBoolMsg"; +class TestCheckBoolMsg : public panther_manager::plugin_test_utils::PluginTestUtils +{ +public: + TestCheckBoolMsg(); + BoolMsg CreateMsg(bool data); + void PublishMsg(BoolMsg msg) { publisher_->publish(msg); } + +protected: + rclcpp::Publisher::SharedPtr publisher_; +}; + +TestCheckBoolMsg::TestCheckBoolMsg() +{ + RegisterNodeWithParams(PLUGIN); + publisher_ = bt_node_->create_publisher(TOPIC, 10); +} + +BoolMsg TestCheckBoolMsg::CreateMsg(bool data) +{ + BoolMsg msg; + msg.data = data; + return msg; +} + +TEST_F(TestCheckBoolMsg, NoTopicSet) +{ + bt_ports input = {{"topic_name", ""}, {"data", "true"}}; + ASSERT_THROW(CreateTree(PLUGIN, input), std::logic_error); +} + +TEST_F(TestCheckBoolMsg, NoMessageArrived) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestCheckBoolMsg, OnTickBehavior) +{ + std::vector test_cases = { + {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "true"}}, CreateMsg(true)}, + {BT::NodeStatus::SUCCESS, {{"topic_name", TOPIC}, {"data", "false"}}, CreateMsg(false)}, + {BT::NodeStatus::FAILURE, {{"topic_name", TOPIC}, {"data", "true"}}, CreateMsg(false)}, + {BT::NodeStatus::FAILURE, {{"topic_name", TOPIC}, {"data", "false"}}, CreateMsg(true)}}; + + for (auto & test_case : test_cases) { + CreateTree(PLUGIN, test_case.input); + PublishMsg(test_case.msg); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(); + + EXPECT_EQ(status, test_case.result); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/panther_manager/test/plugins/condition/test_check_joy_msg.cpp b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp index 88a451141..40e209e90 100644 --- a/panther_manager/test/plugins/condition/test_check_joy_msg.cpp +++ b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -30,30 +31,43 @@ using JoyMsg = sensor_msgs::msg::Joy; using HeaderMsg = std_msgs::msg::Header; +using bt_ports = std::map; +struct TestCase +{ + BT::NodeStatus result; + bt_ports input; + JoyMsg msg; +}; + +constexpr auto TOPIC = "joy"; +constexpr auto PLUGIN = "CheckJoyMsg"; class TestCheckJoyMsg : public panther_manager::plugin_test_utils::PluginTestUtils { public: TestCheckJoyMsg(); - JoyMsg CreateJoyMsg( - const HeaderMsg & header = HeaderMsg(), const std::vector & axes = {}, - const std::vector & buttons = {}); - void PublishJoyMsg(JoyMsg msg); + void PublishMsg(JoyMsg msg) { joy_publisher_->publish(msg); }; + JoyMsg CreateMsg( + const std::vector & axes = {}, const std::vector & buttons = {}, + const HeaderMsg & header = HeaderMsg()); + void SetCurrentHeaderTime(HeaderMsg & header) + { + header.stamp.sec = bt_node_->now().seconds(); + header.stamp.nanosec = bt_node_->now().nanoseconds(); + } protected: rclcpp::Publisher::SharedPtr joy_publisher_; - std::map bb_ports_ = { - {"topic_name", "joy"}, {"axes", ""}, {"buttons", "0;1;0"}, {"timeout", "0.0"}}; }; TestCheckJoyMsg::TestCheckJoyMsg() { - RegisterNodeWithParams("CheckJoyMsg"); - joy_publisher_ = bt_node_->create_publisher("joy", 10); + RegisterNodeWithParams(PLUGIN); + joy_publisher_ = bt_node_->create_publisher(TOPIC, 10); } -JoyMsg TestCheckJoyMsg::CreateJoyMsg( - const HeaderMsg & header, const std::vector & axes, const std::vector & buttons) +JoyMsg TestCheckJoyMsg::CreateMsg( + const std::vector & axes, const std::vector & buttons, const HeaderMsg & header) { JoyMsg msg; msg.header = header; @@ -62,75 +76,120 @@ JoyMsg TestCheckJoyMsg::CreateJoyMsg( return msg; } -void TestCheckJoyMsg::PublishJoyMsg(JoyMsg msg) { joy_publisher_->publish(msg); } - -TEST_F(TestCheckJoyMsg, LoadingCheckJoyMsgPlugin) +TEST_F(TestCheckJoyMsg, NoTopicSet) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); + bt_ports input = {{"topic_name", ""}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}; + ASSERT_THROW(CreateTree(PLUGIN, input), std::logic_error); } TEST_F(TestCheckJoyMsg, NoMessage) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); + bt_ports input = {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); + auto status = tree.tickWhileRunning(); EXPECT_EQ(status, BT::NodeStatus::FAILURE); } -TEST_F(TestCheckJoyMsg, WrongMessageTooFewButtons) +TEST_F(TestCheckJoyMsg, TimeoutTests) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - - auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 1}); - PublishJoyMsg(msg); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); + std::vector test_cases = { + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "1.0"}}, + CreateMsg()}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.0"}}, + CreateMsg()}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "-1.0"}}, + CreateMsg()}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.001"}}, + CreateMsg()}}; + + for (auto & test_case : test_cases) { + SCOPED_TRACE("Test case name: " + test_case.name); + + CreateTree(PLUGIN, test_case.input); + SetCurrentHeaderTime(test_case.msg.header); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + PublishMsg(test_case.msg); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(); + + EXPECT_EQ(status, test_case.result); + } } -TEST_F(TestCheckJoyMsg, GoodMessageWrongButtonsState) +TEST_F(TestCheckJoyMsg, OnTickBehavior) { - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - - auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 0, 0}); - PublishJoyMsg(msg); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestCheckJoyMsg, GoodMessageWithTooMuchButtonsAndGoodButtonsState) -{ - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - - auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 1, 0, 0, 0, 1}); - PublishJoyMsg(msg); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); -} - -TEST_F(TestCheckJoyMsg, GoodMessageGoodButtonsState) -{ - ASSERT_NO_THROW({ CreateTree("CheckJoyMsg", bb_ports_); }); - - auto msg = CreateJoyMsg(HeaderMsg(), {}, {0, 1, 0}); - PublishJoyMsg(msg); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + std::vector test_cases = { + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "1.0"}}, + CreateMsg()}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", "1"}, {"buttons", ""}, {"timeout", "1.0"}}, + CreateMsg({1})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", "1"}, {"timeout", "1.0"}}, + CreateMsg({}, {1})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 0}, {0, 0})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", "34;0;1"}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}}, + CreateMsg({34, 0, 1}, {0, 0, 10, 0})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", "34;0;1"}, {"buttons", ""}, {"timeout", "1.0"}}, + CreateMsg({34, 0, 1}, {0, 0, 10, 0})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}}, + CreateMsg({34, 0, 1}, {0, 0, 10, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "34;0;1"}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}}, + CreateMsg({33, 0, 1}, {0, 0, 10, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 0}, {0, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 0}, {0, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 0, 0}, {0, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 0}, {0, 0, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 1}, {0, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({0, 0}, {0, 1})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}, + CreateMsg({1, 1}, {1, 1})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0;0"}, {"buttons", "0;0;0"}, {"timeout", "1.0"}}, + CreateMsg({1, 1}, {1, 1, 1})}}; + + for (auto & test_case : test_cases) { + CreateTree(PLUGIN, test_case.input); + SetCurrentHeaderTime(test_case.msg.header); + PublishMsg(test_case.msg); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(); + + EXPECT_EQ(status, test_case.result); + } } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - auto result = RUN_ALL_TESTS(); - return result; } From d6e3c39fe28d34815a446c83966e60f2d4622d48 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 21 Oct 2024 15:18:19 +0200 Subject: [PATCH 11/13] update --- .../plugins/condition/test_check_joy_msg.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/panther_manager/test/plugins/condition/test_check_joy_msg.cpp b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp index 40e209e90..f286666e1 100644 --- a/panther_manager/test/plugins/condition/test_check_joy_msg.cpp +++ b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp @@ -50,11 +50,7 @@ class TestCheckJoyMsg : public panther_manager::plugin_test_utils::PluginTestUti JoyMsg CreateMsg( const std::vector & axes = {}, const std::vector & buttons = {}, const HeaderMsg & header = HeaderMsg()); - void SetCurrentHeaderTime(HeaderMsg & header) - { - header.stamp.sec = bt_node_->now().seconds(); - header.stamp.nanosec = bt_node_->now().nanoseconds(); - } + void SetCurrentMsgTime(JoyMsg & header); protected: rclcpp::Publisher::SharedPtr joy_publisher_; @@ -76,6 +72,12 @@ JoyMsg TestCheckJoyMsg::CreateMsg( return msg; } +void TestCheckJoyMsg::SetCurrentMsgTime(JoyMsg & msg) +{ + msg.header.stamp.sec = bt_node_->now().seconds(); + msg.header.stamp.nanosec = bt_node_->now().nanoseconds(); +} + TEST_F(TestCheckJoyMsg, NoTopicSet) { bt_ports input = {{"topic_name", ""}, {"axes", "0;0"}, {"buttons", "0;0"}, {"timeout", "1.0"}}; @@ -109,10 +111,8 @@ TEST_F(TestCheckJoyMsg, TimeoutTests) CreateMsg()}}; for (auto & test_case : test_cases) { - SCOPED_TRACE("Test case name: " + test_case.name); - CreateTree(PLUGIN, test_case.input); - SetCurrentHeaderTime(test_case.msg.header); + SetCurrentMsgTime(test_case.msg); std::this_thread::sleep_for(std::chrono::milliseconds(2)); PublishMsg(test_case.msg); @@ -177,7 +177,7 @@ TEST_F(TestCheckJoyMsg, OnTickBehavior) for (auto & test_case : test_cases) { CreateTree(PLUGIN, test_case.input); - SetCurrentHeaderTime(test_case.msg.header); + SetCurrentMsgTime(test_case.msg); PublishMsg(test_case.msg); auto & tree = GetTree(); From 433edce44500fefebc6833533e1af599a4ebe375 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 21 Oct 2024 18:33:54 +0200 Subject: [PATCH 12/13] Fix ports --- panther_manager/CONFIGURATION.md | 8 ++++---- panther_manager/src/lights_manager_node.cpp | 2 +- panther_manager/src/robot_states_manager_node.cpp | 2 +- panther_manager/src/safety_manager_node.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/panther_manager/CONFIGURATION.md b/panther_manager/CONFIGURATION.md index 423814c6d..0ab52d2b3 100644 --- a/panther_manager/CONFIGURATION.md +++ b/panther_manager/CONFIGURATION.md @@ -206,7 +206,7 @@ To use your customized project, you can modify the `bt_project_file` ROS paramet Groot2 also provides a real-time visualization tool that allows you to see and debug actively running trees. To use this tool with trees launched with the `panther_manager` package, you need to specify the port associated with the tree you want to visualize. The ports for each tree are listed below: -- Lights tree: `10.15.20.2:5555` -- RobotState tree: `10.15.20.2:5556` -- Safety tree: `10.15.20.2:5557` -- Shutdown tree: `10.15.20.2:5558` +- Lights tree: `10.15.20.2:5550` +- RobotState tree: `10.15.20.2:5555` +- Safety tree: `10.15.20.2:5560` +- Shutdown tree: `10.15.20.2:5565` diff --git a/panther_manager/src/lights_manager_node.cpp b/panther_manager/src/lights_manager_node.cpp index 6869a1dd6..c1c479995 100644 --- a/panther_manager/src/lights_manager_node.cpp +++ b/panther_manager/src/lights_manager_node.cpp @@ -49,7 +49,7 @@ LightsManagerNode::LightsManagerNode( battery_percent_window_len, 1.0); const auto initial_blackboard = CreateLightsInitialBlackboard(); - lights_tree_manager_ = std::make_unique("Lights", initial_blackboard, 5555); + lights_tree_manager_ = std::make_unique("Lights", initial_blackboard, 5550); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } diff --git a/panther_manager/src/robot_states_manager_node.cpp b/panther_manager/src/robot_states_manager_node.cpp index af345d04e..b9e4417c3 100644 --- a/panther_manager/src/robot_states_manager_node.cpp +++ b/panther_manager/src/robot_states_manager_node.cpp @@ -45,7 +45,7 @@ RobotStatesManagerNode::RobotStatesManagerNode( const auto initial_blackboard = CreateBlackboard(); docking_tree_manager_ = std::make_unique( - "RobotStates", initial_blackboard, 5556); + "RobotStates", initial_blackboard, 5555); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp index 7b213c6f8..258f0922a 100644 --- a/panther_manager/src/safety_manager_node.cpp +++ b/panther_manager/src/safety_manager_node.cpp @@ -58,14 +58,14 @@ SafetyManagerNode::SafetyManagerNode( const auto safety_initial_blackboard = CreateSafetyInitialBlackboard(); safety_tree_manager_ = std::make_unique( - "Safety", safety_initial_blackboard, 5557); + "Safety", safety_initial_blackboard, 5560); const auto shutdown_hosts_path = this->get_parameter("shutdown_hosts_path").as_string(); const std::map shutdown_initial_blackboard = { {"SHUTDOWN_HOSTS_FILE", shutdown_hosts_path.c_str()}, }; shutdown_tree_manager_ = std::make_unique( - "Shutdown", shutdown_initial_blackboard, 5558); + "Shutdown", shutdown_initial_blackboard, 5565); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } From 53cb60ae83aa19471af21b9f8f4dd921aaa88fa9 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 22 Oct 2024 10:29:10 +0200 Subject: [PATCH 13/13] Add dock_pose field, improve description, add dependencies --- panther_manager/CMakeLists.txt | 42 ++++++++++----- .../config/robot_states_manager.yaml | 2 +- .../panther_manager/behavior_tree_utils.hpp | 54 +++++++++++++++++++ .../action/call_set_bool_service_node.hpp | 3 +- .../call_set_led_animation_service_node.hpp | 10 ++-- .../plugins/action/dock_robot_node.hpp | 37 ++++++++----- .../action/shutdown_hosts_from_file_node.hpp | 6 +-- .../action/shutdown_single_host_node.hpp | 17 +++--- .../plugins/action/signal_shutdown_node.hpp | 2 +- .../plugins/action/undock_robot_node.hpp | 17 +++--- .../plugins/condition/check_bool_msg.hpp | 3 +- .../plugins/condition/check_joy_msg.hpp | 13 +++-- .../decorator/tick_after_timeout_node.hpp | 3 +- panther_manager/package.xml | 3 ++ 14 files changed, 156 insertions(+), 56 deletions(-) diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt index f9d74002f..9a9970339 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -11,6 +11,8 @@ set(PACKAGE_DEPENDENCIES behaviortree_cpp behaviortree_ros2 libssh + geometry_msgs + opennav_docking_msgs panther_msgs panther_utils rclcpp @@ -18,8 +20,8 @@ set(PACKAGE_DEPENDENCIES sensor_msgs std_msgs std_srvs - yaml-cpp - opennav_docking_msgs) + tf2_geometry_msgs + yaml-cpp) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) @@ -78,16 +80,19 @@ foreach(bt_plugin ${plugin_libs}) endforeach() add_executable( - safety_manager_node src/safety_manager_node_main.cpp - src/safety_manager_node.cpp src/behavior_tree_manager.cpp) + safety_manager_node + src/safety_manager_node_main.cpp src/safety_manager_node.cpp + src/behavior_tree_manager.cpp src/behavior_tree_manager.cpp) ament_target_dependencies( safety_manager_node behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) target_link_libraries(safety_manager_node ${plugin_libs}) add_executable( @@ -96,11 +101,13 @@ add_executable( ament_target_dependencies( lights_manager_node behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) target_link_libraries(lights_manager_node ${plugin_libs}) add_executable( @@ -110,11 +117,13 @@ add_executable( ament_target_dependencies( robot_states_manager_node behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) target_link_libraries(robot_states_manager_node ${plugin_libs}) install(TARGETS ${plugin_libs} DESTINATION lib) @@ -222,8 +231,9 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_behavior_tree_utils PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_utils - behaviortree_cpp behaviortree_ros2 panther_utils) + ament_target_dependencies( + ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2 + geometry_msgs panther_utils tf2_geometry_msgs) ament_add_gtest( ${PROJECT_NAME}_test_behavior_tree_manager @@ -246,10 +256,12 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_lights_manager_node behaviortree_cpp behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs + tf2_geometry_msgs std_msgs) ament_add_gtest( @@ -264,11 +276,13 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_lights_behavior_tree behaviortree_cpp behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) ament_add_gtest( ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp @@ -281,11 +295,13 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_safety_manager_node behaviortree_cpp behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs - std_msgs) + std_msgs + tf2_geometry_msgs) ament_add_gtest( ${PROJECT_NAME}_test_safety_behavior_tree @@ -299,12 +315,14 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_safety_behavior_tree behaviortree_cpp behaviortree_ros2 + geometry_msgs panther_msgs panther_utils rclcpp sensor_msgs std_msgs - std_srvs) + std_srvs + tf2_geometry_msgs) endif() ament_package() diff --git a/panther_manager/config/robot_states_manager.yaml b/panther_manager/config/robot_states_manager.yaml index 06d2e221f..e65b274d3 100644 --- a/panther_manager/config/robot_states_manager.yaml +++ b/panther_manager/config/robot_states_manager.yaml @@ -3,7 +3,7 @@ ros__parameters: timer_frequency: 50.0 ros_communication_timeout: - availability: 1.0 + availability: 2.0 response: 1.0 ros_plugin_libs: - call_trigger_service_bt_node diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp index 655a004fa..c73bc71f7 100644 --- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp +++ b/panther_manager/include/panther_manager/behavior_tree_utils.hpp @@ -26,6 +26,9 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/plugins.hpp" +#include +#include + namespace panther_manager::behavior_tree_utils { @@ -112,6 +115,57 @@ inline std::vector convertFromString>(StringView str) return output; } +/** + * @brief Converts a string to a PoseStamped message. + * + * The string format should be "roll,pitch,yaw,x,y,z,frame_id" where: + * - roll, pitch, yaw: Euler angles in radians. + * - x, y, z: Position coordinates. + * - frame_id: Coordinate frame ID (string). + * + * @param str The string to convert. + * @return geometry_msgs::msg::PoseStamped The converted PoseStamped message. + * + * @throw BT::RuntimeError Throws if the input is invalid or cannot be parsed. + */ +template <> +inline geometry_msgs::msg::PoseStamped convertFromString( + StringView str) +{ + auto parts = splitString(str, ';'); + + if (parts.size() != 7) { + throw BT::RuntimeError( + "Invalid input for PoseStamped. Expected 7 values: x;y;z;roll;pitch;yaw;frame_id"); + } + + geometry_msgs::msg::PoseStamped pose_stamped; + + try { + // Position (x, y, z) + pose_stamped.pose.position.x = convertFromString(parts[0]); + pose_stamped.pose.position.y = convertFromString(parts[1]); + pose_stamped.pose.position.z = convertFromString(parts[2]); + + // Orientation (R,P,Y -> Quaternion) + double roll = convertFromString(parts[3]); + double pitch = convertFromString(parts[4]); + double yaw = convertFromString(parts[5]); + tf2::Quaternion quaternion; + quaternion.setRPY(roll, pitch, yaw); + pose_stamped.pose.orientation = tf2::toMsg(quaternion); + + // Frame ID and current time + pose_stamped.header.frame_id = convertFromString(parts[6]); + pose_stamped.header.stamp = rclcpp::Clock().now(); + + } catch (const std::exception & e) { + throw BT::RuntimeError("Failed to convert string to PoseStamped: " + std::string(e.what())); + } + + return pose_stamped; +} + } // namespace BT #endif // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_ diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp index adafe1d46..96f40482b 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp @@ -36,7 +36,8 @@ class CallSetBoolService : public BT::RosServiceNode static BT::PortsList providedPorts() { - return providedBasicPorts({BT::InputPort("data", "true / false value")}); + return providedBasicPorts( + {BT::InputPort("data", "Boolean value to send with the service request.")}); } virtual bool setRequest(typename Request::SharedPtr & request) override; diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp index 38a915a5a..a0dcff1e1 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -36,11 +36,11 @@ class CallSetLedAnimationService : public BT::RosServiceNode("id", "animation ID"), - BT::InputPort("param", "optional parameter"), - BT::InputPort("repeating", "indicates if animation should repeat"), - }); + return providedBasicPorts( + {BT::InputPort("id", "Animation ID to trigger."), + BT::InputPort("param", "Optional animation parameter."), + BT::InputPort( + "repeating", "Specifies whether the animation should repeated continuously.")}); } virtual bool setRequest(typename Request::SharedPtr & request) override; diff --git a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp index 58933c068..7793bd216 100644 --- a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp @@ -48,20 +48,31 @@ class DockRobot : public BT::RosActionNode("use_dock_id", true, "Whether to use the dock's ID or dock pose fields"), - BT::InputPort("dock_id", "Dock ID or name to use"), - BT::InputPort("dock_type", "The dock plugin type, if using dock pose"), - BT::InputPort( - "max_staging_time", 1000.0, "Maximum time to navigate to the staging pose"), - BT::InputPort( - "navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"), + return providedBasicPorts( + {BT::InputPort( + "use_dock_id", true, "Determines whether to use the dock's ID or dock pose fields."), + BT::InputPort( + "dock_id", + "Specifies the dock's ID or name from the dock database (used if 'use_dock_id' is true)."), + BT::InputPort( + "dock_pose", + "Specifies the dock's pose (used if 'use_dock_id' is false). Format: " + "\"x;y;z;roll;pitch;yaw;frame_id\""), + BT::InputPort( + "dock_type", + "Defines the type of dock being used when docking via pose. Not needed if only one dock " + "type is available."), + BT::InputPort( + "max_staging_time", 120.0, + "Maximum time allowed (in seconds) for navigating to the dock's staging pose."), + BT::InputPort( + "navigate_to_staging_pose", true, + "Specifies whether the robot should autonomously navigate to the staging pose."), - BT::OutputPort( - "error_code", "Contextual error code, if any"), - BT::OutputPort( - "num_retries", "Number of retries attempted"), - }); + BT::OutputPort( + "error_code", "Returns an error code indicating the reason for failure, if any."), + BT::OutputPort( + "num_retries", "Reports the number of retry attempts made during the docking process.")}); } }; diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp index 949565f61..c9eaa79e0 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp @@ -39,10 +39,8 @@ class ShutdownHostsFromFile : public ShutdownHosts static BT::PortsList providedPorts() { - return { - BT::InputPort( - "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"), - }; + return {BT::InputPort( + "shutdown_hosts_file", "Absolute path to a YAML file listing the hosts to shut down.")}; } private: diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 630db6505..680e849d2 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp @@ -38,14 +38,17 @@ class ShutdownSingleHost : public ShutdownHosts static BT::PortsList providedPorts() { return { - BT::InputPort("ip", "ip of the host to shutdown"), - BT::InputPort("username", "user to log into while executing shutdown command"), - BT::InputPort("port", "SSH communication port"), - BT::InputPort("command", "command to execute on shutdown"), - BT::InputPort("timeout", "time in seconds to wait for host to shutdown"), + BT::InputPort("ip", "IP address of the host to shut down."), + BT::InputPort( + "username", "Username to use for logging in and executing the shutdown command."), + BT::InputPort("port", "SSH port used for communication (default is usually 22)."), + BT::InputPort("command", "Shutdown command to execute on the remote host."), + BT::InputPort( + "timeout", "Maximum time (in seconds) to wait for the host to shut down."), BT::InputPort( - "ping_for_success", "ping host until it is not available or timeout is reached"), - }; + "ping_for_success", + "Whether to continuously ping the host until it becomes unreachable or the timeout is " + "reached.")}; } private: diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp index 7bc9709db..abee0f743 100644 --- a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp @@ -35,7 +35,7 @@ class SignalShutdown : public BT::SyncActionNode static BT::PortsList providedPorts() { return { - BT::InputPort("reason", "", "reason to shutdown robot"), + BT::InputPort("reason", "", "Reason to shutdown robot."), }; } diff --git a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp index 238fa5f31..820ad3c20 100644 --- a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp @@ -49,14 +49,17 @@ class UndockRobot : public BT::RosActionNode( - "dock_type", "The dock plugin type, if not previous instance used for docking"), - BT::InputPort( - "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"), + return providedBasicPorts( + {BT::InputPort( + "dock_type", + "Specifies the dock plugin type to use for undocking. If empty, the previously used dock " + "type is assumed."), + BT::InputPort( + "max_undocking_time", 30.0, + "Maximum allowable time (in seconds) to undock and return to the staging pose."), - BT::OutputPort("error_code", "Error code"), - }); + BT::OutputPort( + "error_code", "Returns an error code if the undocking process fails.")}); } }; diff --git a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp index a2d953c4c..fbe52a88d 100644 --- a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp @@ -45,7 +45,8 @@ class CheckBoolMsg : public BT::RosTopicSubNode static BT::PortsList providedPorts() { - return providedBasicPorts({BT::InputPort("data", "state of data to accept a condition")}); + return providedBasicPorts( + {BT::InputPort("data", "Specifies the expected state of the data field.")}); } }; diff --git a/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp index dded1c29c..f50aafce0 100644 --- a/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp @@ -45,9 +45,16 @@ class CheckJoyMsg : public BT::RosTopicSubNode static BT::PortsList providedPorts() { return providedBasicPorts( - {BT::InputPort>("axes", "state of axes to accept a condition"), - BT::InputPort>("buttons", "state of buttons to accept a condition"), - BT::InputPort("timeout", 0.0, "max time delay to accept a condition")}); + {BT::InputPort>( + "axes", "", + "Specifies the expected state of the axes field. An empty string (\"\") means the value " + "is ignored."), + BT::InputPort>( + "buttons", "", + "Specifies the expected state of the buttons field. An empty string (\"\") means the " + "value is ignored."), + BT::InputPort( + "timeout", 0.0, "Maximum allowable time delay to accept the condition.")}); } private: diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp index 20a273e5f..7661abf95 100644 --- a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp +++ b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp @@ -31,7 +31,8 @@ class TickAfterTimeout : public BT::DecoratorNode static BT::PortsList providedPorts() { - return {BT::InputPort("timeout", "time in s to wait before ticking child again")}; + return {BT::InputPort( + "timeout", "Time in seconds to wait before ticking the child node again")}; } private: diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 6da23d973..a9cd5f765 100644 --- a/panther_manager/package.xml +++ b/panther_manager/package.xml @@ -19,15 +19,18 @@ behaviortree_cpp behaviortree_ros2 + geometry_msgs iputils-ping libboost-dev libssh-dev + opennav_docking_msgs panther_msgs panther_utils rclcpp rclcpp_action sensor_msgs std_srvs + tf2_geometry_msgs yaml-cpp ament_lint_auto