diff --git a/husarion_ugv_lights/CONFIGURATION.md b/husarion_ugv_lights/CONFIGURATION.md index 7dff19d82..056854bab 100644 --- a/husarion_ugv_lights/CONFIGURATION.md +++ b/husarion_ugv_lights/CONFIGURATION.md @@ -116,7 +116,7 @@ Animation of type `husarion_ugv_lights::MovingImageAnimation`, returns frames to MovingImageAnimation -### Defining Animations +## Defining Animations Users can define their own LED animations using basic animation types. Similar to basic ones, user animations are parsed using YAML file and loaded on node start. For `ImageAnimation` you can use basic images from the `animations` folder and change their color with the `color` key ([see ImageAnimation](#imageanimation)). Follow the example below to add custom animations. diff --git a/husarion_ugv_manager/CMakeLists.txt b/husarion_ugv_manager/CMakeLists.txt index f3b784a0a..7d959975f 100644 --- a/husarion_ugv_manager/CMakeLists.txt +++ b/husarion_ugv_manager/CMakeLists.txt @@ -58,6 +58,10 @@ 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_string_msg_bt_node SHARED + src/plugins/condition/check_string_msg.cpp) +list(APPEND plugin_libs check_string_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) @@ -163,6 +167,12 @@ if(BUILD_TESTING) src/plugins/condition/check_joy_msg.cpp) list(APPEND plugin_tests ${PROJECT_NAME}_test_check_joy_msg) + ament_add_gtest( + ${PROJECT_NAME}_test_check_string_msg + test/plugins/condition/test_check_string_msg.cpp + src/plugins/condition/check_string_msg.cpp) + list(APPEND plugin_tests ${PROJECT_NAME}_test_check_string_msg) + ament_add_gtest( ${PROJECT_NAME}_test_tick_after_timeout_node test/plugins/decorator/test_tick_after_timeout_node.cpp diff --git a/husarion_ugv_manager/README.md b/husarion_ugv_manager/README.md index f28e83c61..54f98cd05 100644 --- a/husarion_ugv_manager/README.md +++ b/husarion_ugv_manager/README.md @@ -31,6 +31,7 @@ Node responsible for managing Bumper Lights animation scheduling. - `battery/battery_status` [*sensor_msgs/BatteryState*]: State of the internal Battery. - `hardware/e_stop` [*std_msgs/Bool*]: State of emergency stop. +- `twist_mux_controller/source` [*std_msgs/String]: Source of the command velocity. #### Service Clients (for Default Trees) diff --git a/husarion_ugv_manager/behavior_trees/LightsBT.btproj b/husarion_ugv_manager/behavior_trees/LightsBT.btproj index 61bba3ef7..4da54aa84 100644 --- a/husarion_ugv_manager/behavior_trees/LightsBT.btproj +++ b/husarion_ugv_manager/behavior_trees/LightsBT.btproj @@ -9,6 +9,10 @@ indicates if animation should repeat ROS service name + + Topic name + Specifies the expected state of the data field + time in s to wait before ticking child again diff --git a/husarion_ugv_manager/behavior_trees/lights.xml b/husarion_ugv_manager/behavior_trees/lights.xml index ca1101203..fb23a752d 100644 --- a/husarion_ugv_manager/behavior_trees/lights.xml +++ b/husarion_ugv_manager/behavior_trees/lights.xml @@ -107,22 +107,35 @@ - - + + - + + + + + + indicates if animation should repeat ROS service name + + Topic name + Specifies the expected state of the data field + time in s to wait before ticking child again diff --git a/husarion_ugv_manager/config/lights_manager.yaml b/husarion_ugv_manager/config/lights_manager.yaml index b4b208801..5bd09e88c 100644 --- a/husarion_ugv_manager/config/lights_manager.yaml +++ b/husarion_ugv_manager/config/lights_manager.yaml @@ -19,3 +19,4 @@ - tick_after_timeout_bt_node ros_plugin_libs: - call_set_led_animation_service_bt_node + - check_string_msg_bt_node diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp index 16c63209a..a24629abe 100644 --- a/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp +++ b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp @@ -23,7 +23,6 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" -#include "sensor_msgs/msg/joy.hpp" #include "std_msgs/msg/bool.hpp" #include "husarion_ugv_msgs/msg/led_animation.hpp" @@ -39,7 +38,6 @@ namespace husarion_ugv_manager using BatteryStateMsg = sensor_msgs::msg::BatteryState; using BoolMsg = std_msgs::msg::Bool; using LEDAnimationMsg = husarion_ugv_msgs::msg::LEDAnimation; -using JoyMsg = sensor_msgs::msg::Joy; /** * @brief This class is responsible for creating a BehaviorTree responsible for lights management, @@ -73,11 +71,8 @@ class LightsManagerNode : public rclcpp::Node private: void BatteryCB(const BatteryStateMsg::SharedPtr battery); void EStopCB(const BoolMsg::SharedPtr e_stop); - void JoyCB(const JoyMsg::SharedPtr joy); void LightsTreeTimerCB(); - static constexpr std::size_t kDeadManButtonIndex = 4; - float update_charging_anim_step_; std::shared_ptr param_listener_; @@ -85,7 +80,6 @@ class LightsManagerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr e_stop_sub_; - rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::TimerBase::SharedPtr lights_tree_timer_; std::unique_ptr> battery_percent_ma_; diff --git a/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp new file mode 100644 index 000000000..074dc0b51 --- /dev/null +++ b/husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp @@ -0,0 +1,56 @@ +// Copyright 2025 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 HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_STRING_MSG_HPP_ +#define HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_STRING_MSG_HPP_ + +#include +#include + +#include +#include + +#include + +#include "husarion_ugv_manager/behavior_tree_utils.hpp" + +namespace husarion_ugv_manager +{ + +// FIXME: There is no possibility to set QoS profile. Add it in the future to subscribe e_stop. +class CheckStringMsg : public BT::RosTopicSubNode +{ + using StringMsg = std_msgs::msg::String; + +public: + CheckStringMsg( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) + : BT::RosTopicSubNode(name, conf, params) + { + } + + BT::NodeStatus onTick(const StringMsg::SharedPtr & last_msg) override; + + bool latchLastMessage() const override; + + static BT::PortsList providedPorts() + { + return providedBasicPorts( + {BT::InputPort("data", "Specifies the expected state of the data field.")}); + } +}; + +} // namespace husarion_ugv_manager + +#endif // HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_STRING_MSG_HPP_ diff --git a/husarion_ugv_manager/src/lights_manager_node.cpp b/husarion_ugv_manager/src/lights_manager_node.cpp index 2bd54f9f9..c5e9d0f1d 100644 --- a/husarion_ugv_manager/src/lights_manager_node.cpp +++ b/husarion_ugv_manager/src/lights_manager_node.cpp @@ -24,7 +24,6 @@ #include "behaviortree_ros2/ros_node_params.hpp" #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/joy.hpp" #include "husarion_ugv_utils/moving_average.hpp" @@ -72,8 +71,6 @@ 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)); - joy_sub_ = this->create_subscription( - "joy", 10, std::bind(&LightsManagerNode::JoyCB, this, _1)); const double timer_freq = this->params_.timer_frequency; const auto timer_period = std::chrono::duration(1.0 / timer_freq); @@ -126,7 +123,6 @@ std::map LightsManagerNode::CreateLightsInitialBlackboard {"current_anim_id", undefined_anim_id}, {"current_battery_anim_id", undefined_anim_id}, {"current_error_anim_id", undefined_anim_id}, - {"drive_state", false}, {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent}, {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period}, {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent}, @@ -190,12 +186,6 @@ void LightsManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop) lights_tree_manager_->GetBlackboard()->set("e_stop_state", e_stop->data); } -void LightsManagerNode::JoyCB(const JoyMsg::SharedPtr joy) -{ - lights_tree_manager_->GetBlackboard()->set( - "drive_state", joy->buttons[kDeadManButtonIndex]); -} - void LightsManagerNode::LightsTreeTimerCB() { if (!SystemReady()) { diff --git a/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp new file mode 100644 index 000000000..7bafdbdd8 --- /dev/null +++ b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp @@ -0,0 +1,42 @@ +// Copyright 2025 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 "husarion_ugv_manager/plugins/condition/check_string_msg.hpp" + +#include +#include + +#include "husarion_ugv_manager/behavior_tree_utils.hpp" + +namespace husarion_ugv_manager +{ + +BT::NodeStatus CheckStringMsg::onTick(const StringMsg::SharedPtr & last_msg) +{ + std::string expected_data; + if (!getInput("data", expected_data)) { + RCLCPP_ERROR_STREAM(this->logger(), GetLoggerPrefix(name()) << "Failed to get input [data]"); + return BT::NodeStatus::FAILURE; + } + + return (last_msg && last_msg->data == expected_data) ? BT::NodeStatus::SUCCESS + : BT::NodeStatus::FAILURE; +} + +bool CheckStringMsg::latchLastMessage() const { return true; } + +} // namespace husarion_ugv_manager + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(husarion_ugv_manager::CheckStringMsg, "CheckStringMsg"); diff --git a/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp b/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp new file mode 100644 index 000000000..666252791 --- /dev/null +++ b/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp @@ -0,0 +1,170 @@ +// Copyright 2025 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 "husarion_ugv_manager/plugins/condition/check_string_msg.hpp" +#include "utils/plugin_test_utils.hpp" + +using StringMsg = std_msgs::msg::String; +using bt_ports = std::map; + +struct TestCase +{ + BT::NodeStatus result; + bt_ports input; + StringMsg msg; +}; + +constexpr auto TOPIC = "string"; +constexpr auto PLUGIN = "CheckStringMsg"; + +class TestCheckStringMsg : public husarion_ugv_manager::plugin_test_utils::PluginTestUtils +{ +public: + TestCheckStringMsg(); + StringMsg CreateMsg(const std::string & data); + void PublishMsg(StringMsg msg) { publisher_->publish(msg); } + +protected: + rclcpp::Publisher::SharedPtr publisher_; +}; + +TestCheckStringMsg::TestCheckStringMsg() +{ + RegisterNodeWithParams(PLUGIN); + publisher_ = bt_node_->create_publisher(TOPIC, 10); +} + +StringMsg TestCheckStringMsg::CreateMsg(const std::string & data) +{ + StringMsg msg; + msg.data = data; + return msg; +} + +TEST_F(TestCheckStringMsg, NoInputData) +{ + bt_ports input = {{"topic_name", TOPIC}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); + + auto & tree = GetTree(); + auto status = tree.tickOnce(); + + // publish empty msg, to make sure that node fails because there is no data and not because it + // reads empty string + PublishMsg(CreateMsg("")); + status = tree.tickWhileRunning(); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestCheckStringMsg, NoMessageArrived) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "name"}}; + ASSERT_NO_THROW({ CreateTree(PLUGIN, input); }); + + auto & tree = GetTree(); + auto status = tree.tickWhileRunning(); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestCheckStringMsg, SuccessOnExactMatch) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "name"}}; + CreateTree(PLUGIN, input); + auto & tree = GetTree(); + auto status = tree.tickOnce(); + + PublishMsg(CreateMsg("name")); + status = tree.tickWhileRunning(); + + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); +} + +TEST_F(TestCheckStringMsg, SuccessOnMatchWithSpaces) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "name with spaces"}}; + CreateTree(PLUGIN, input); + auto & tree = GetTree(); + auto status = tree.tickOnce(); + + PublishMsg(CreateMsg("name with spaces")); + status = tree.tickWhileRunning(); + + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); +} + +TEST_F(TestCheckStringMsg, SuccessOnMatchWithDots) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "name.with.dots"}}; + CreateTree(PLUGIN, input); + auto & tree = GetTree(); + auto status = tree.tickOnce(); + + PublishMsg(CreateMsg("name.with.dots")); + status = tree.tickWhileRunning(); + + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); +} + +TEST_F(TestCheckStringMsg, SuccessOnMatchWithMixedLetters) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "MiXeDlEtTeRs"}}; + CreateTree(PLUGIN, input); + auto & tree = GetTree(); + auto status = tree.tickOnce(); + + PublishMsg(CreateMsg("MiXeDlEtTeRs")); + status = tree.tickWhileRunning(); + + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); +} + +TEST_F(TestCheckStringMsg, SuccessWhenValueChanges) +{ + bt_ports input = {{"topic_name", TOPIC}, {"data", "name"}}; + CreateTree(PLUGIN, input); + auto & tree = GetTree(); + auto status = tree.tickOnce(); + + PublishMsg(CreateMsg("name")); + status = tree.tickWhileRunning(); + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); + + PublishMsg(CreateMsg("name changed")); + status = tree.tickWhileRunning(); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); + + PublishMsg(CreateMsg("name")); + status = tree.tickWhileRunning(); + EXPECT_EQ(status, BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + auto result = RUN_ALL_TESTS(); + return result; +} diff --git a/husarion_ugv_manager/test/test_lights_behavior_tree.cpp b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp index 0d43cfbb2..dc2b6cefa 100644 --- a/husarion_ugv_manager/test/test_lights_behavior_tree.cpp +++ b/husarion_ugv_manager/test/test_lights_behavior_tree.cpp @@ -131,6 +131,7 @@ std::vector TestLightsBehaviorTree::CreateTestParameters() co std::vector ros_plugin_libs; ros_plugin_libs.push_back("call_set_led_animation_service_bt_node"); + ros_plugin_libs.push_back("check_string_msg_bt_node"); std::vector params; params.push_back(rclcpp::Parameter("bt_project_path", bt_project_path));