From fd22e7f0a79b7caa4884e8197440d64634a8bc18 Mon Sep 17 00:00:00 2001 From: kmakd Date: Mon, 11 Aug 2025 12:46:12 +0000 Subject: [PATCH 1/6] Add twist mux controller --- ROS_API.md | 4 +++- husarion_ugv/hardware_deps.repos | 4 ++-- husarion_ugv/simulation_deps.repos | 2 +- .../config/joy2twist_lynx.yaml | 1 + .../config/joy2twist_panther.yaml | 1 + .../config/WH01_controller.yaml | 21 ++++++++++++++++++ .../config/WH02_controller.yaml | 22 +++++++++++++++++++ .../config/WH04_controller.yaml | 21 ++++++++++++++++++ .../config/WH05_controller.yaml | 21 ++++++++++++++++++ .../launch/controller.launch.py | 11 +++++++--- husarion_ugv_controller/package.xml | 1 + .../urdf/common/gazebo.urdf.xacro | 1 + husarion_ugv_gazebo/README.md | 1 + husarion_ugv_gazebo/config/robot_bridge.yaml | 12 +++++++++- .../config/teleop_with_estop.config | 2 +- .../config/enu_localization.yaml | 2 +- .../config/enu_localization_with_gps.yaml | 2 +- .../config/relative_localization.yaml | 2 +- .../relative_localization_with_gps.yaml | 2 +- 19 files changed, 120 insertions(+), 13 deletions(-) diff --git a/ROS_API.md b/ROS_API.md index 1092c40e0..a356328f0 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -67,9 +67,10 @@ Below is information about the physical robot API. For the simulation, topics an | 🤖 | 🖥️ | Topic | Description | | --- | --- | ----------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✅ | ✅ | `autonomous/cmd_vel` | Command velocity value for autonomous inputs.
[*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) | | ✅ | ✅ | `battery/battery_status` | Mean values of both batteries will be published if the robot has two batteries. Otherwise, the state of the single battery will be published.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | | ✅ | ❌ | `battery/charging_status` | Battery charging status value.
[*husarion_ugv_msgs/ChargingStatus*](husarion_ugv_msgs/msg/ChargingStatus.msg) | -| ✅ | ✅ | `cmd_vel` | Command velocity value.
[*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) | +| ✅ | ✅ | `cmd_vel` | Command velocity value for unknown inputs.
[*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) | | ✅ | ✅ | `diagnostics` | Diagnostic data.
[*diagnostic_msgs/DiagnosticArray*](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html) | | ✅ | ✅ | `dynamic_joint_states` | Information about the state of various movable joints in a robotic system.
[*control_msgs/DynamicJointState*](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg) | | ✅ | ✅ | ⚙️ `gps/filtered` ⚙️ | Filtered GPS position after fusing odometry data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | @@ -84,6 +85,7 @@ Below is information about the physical robot API. For the simulation, topics an | ✅ | ✅ | `lights/channel_1_frame` | Frame to be displayed on robot Front Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | | ✅ | ✅ | `lights/channel_2_frame` | Frame to be displayed on robot Rear Bumper Lights.
[*sensor_msgs/Image*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) | | ✅ | ✅ | `localization/set_pose` | Sets the pose of the EKF node.
[*geometry_msgs/PoseWithCovarianceStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseWithCovarianceStamped.html) | +| ✅ | ✅ | `manual/cmd_vel` | Command velocity value for manual inputs.
[*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) | | ✅ | ✅ | `odometry/filtered` | Contains information about the filtered position and orientation. When `localization_mode` is `relative`, the position and orientation are relative to the starting point. When `localization_mode` is `enu`, the orientation is relative to the east-north-up (ENU) coordinates.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | | ✅ | ✅ | `odometry/wheels` | Robot odometry calculated from wheels.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | | ✅ | ✅ | `robot_description` | Contains information about robot description from URDF file.
[*std_msgs/String*](https://docs.ros2.org/latest/api/std_msgs/msg/String.html) | diff --git a/husarion_ugv/hardware_deps.repos b/husarion_ugv/hardware_deps.repos index 5352dda59..835641dbb 100644 --- a/husarion_ugv/hardware_deps.repos +++ b/husarion_ugv/hardware_deps.repos @@ -6,7 +6,7 @@ repositories: husarion_controllers: type: git url: https://github.com/husarion/husarion_controllers - version: 632a4d248227288afd99cf11179b019d968bda7b + version: 9916ab4e087930b3ff947efec321ab9f8df531c5 husarion_components_description: type: git url: https://github.com/husarion/husarion_components_description.git @@ -14,7 +14,7 @@ repositories: joy2twist: type: git url: https://github.com/husarion/joy2twist.git - version: 8be8ae24bd0dbfc9f6d557168107f605ba61bf53 + version: 05a699aea7fa67c82db9c5c9c218b55b61626da2 launch: # There is a bug in official launch package. Track issue: https://github.com/ros2/launch/issues/894 type: git url: https://github.com/rafal-gorecki/launch.git diff --git a/husarion_ugv/simulation_deps.repos b/husarion_ugv/simulation_deps.repos index 4afcc50d5..1a5a0e03a 100644 --- a/husarion_ugv/simulation_deps.repos +++ b/husarion_ugv/simulation_deps.repos @@ -6,7 +6,7 @@ repositories: husarion_controllers: type: git url: https://github.com/husarion/husarion_controllers - version: 632a4d248227288afd99cf11179b019d968bda7b + version: 9916ab4e087930b3ff947efec321ab9f8df531c5 husarion_gz_worlds: type: git url: https://github.com/husarion/husarion_gz_worlds.git diff --git a/husarion_ugv_bringup/config/joy2twist_lynx.yaml b/husarion_ugv_bringup/config/joy2twist_lynx.yaml index 0df5a5c8d..2c840d838 100644 --- a/husarion_ugv_bringup/config/joy2twist_lynx.yaml +++ b/husarion_ugv_bringup/config/joy2twist_lynx.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + cmd_vel_topic: manual/cmd_vel cmd_vel_stamped: true linear_velocity_factor: diff --git a/husarion_ugv_bringup/config/joy2twist_panther.yaml b/husarion_ugv_bringup/config/joy2twist_panther.yaml index 5c8d3fcef..79bc18fb3 100644 --- a/husarion_ugv_bringup/config/joy2twist_panther.yaml +++ b/husarion_ugv_bringup/config/joy2twist_panther.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + cmd_vel_topic: manual/cmd_vel cmd_vel_stamped: true linear_velocity_factor: diff --git a/husarion_ugv_controller/config/WH01_controller.yaml b/husarion_ugv_controller/config/WH01_controller.yaml index c75a5faeb..89ed962a7 100644 --- a/husarion_ugv_controller/config/WH01_controller.yaml +++ b/husarion_ugv_controller/config/WH01_controller.yaml @@ -9,6 +9,8 @@ type: diff_drive_controller/DiffDriveController imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster + twist_mux_controller: + type: twist_mux_controller/TwistMuxController # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications imu_broadcaster: @@ -78,3 +80,22 @@ max_deceleration_reverse: 5.74 # rad/s^2 max_jerk: .NAN # rad/s^3 min_jerk: .NAN # rad/s^3 + + twist_mux_controller: + ros__parameters: + holonomic: false + cmd_vel_inputs: + manual: + topic: manual/cmd_vel + timeout: 0.2 + priority: 100 + autonomous: + topic: autonomous/cmd_vel + timeout: 0.2 + priority: 10 + unknown: + topic: cmd_vel + timeout: 0.2 + priority: 1 + command_interface_linear_x: drive_controller/linear/velocity + command_interface_angular_z: drive_controller/angular/velocity diff --git a/husarion_ugv_controller/config/WH02_controller.yaml b/husarion_ugv_controller/config/WH02_controller.yaml index b3a02b2ed..8b7c0cad1 100644 --- a/husarion_ugv_controller/config/WH02_controller.yaml +++ b/husarion_ugv_controller/config/WH02_controller.yaml @@ -9,6 +9,8 @@ type: mecanum_drive_controller/MecanumDriveController imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster + twist_mux_controller: + type: twist_mux_controller/TwistMuxController # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications imu_broadcaster: @@ -79,3 +81,23 @@ max_deceleration_reverse: 3.2 # rad/s^2 max_jerk: .NAN # rad/s^3 min_jerk: .NAN # rad/s^3 + + twist_mux_controller: + ros__parameters: + holonomic: true + cmd_vel_inputs: + manual: + topic: manual/cmd_vel + timeout: 0.2 + priority: 100 + autonomous: + topic: autonomous/cmd_vel + timeout: 0.2 + priority: 10 + unknown: + topic: cmd_vel + timeout: 0.2 + priority: 1 + command_interface_linear_x: drive_controller/linear/x/velocity + command_interface_linear_y: drive_controller/linear/y/velocity + command_interface_angular_z: drive_controller/angular/z/velocity diff --git a/husarion_ugv_controller/config/WH04_controller.yaml b/husarion_ugv_controller/config/WH04_controller.yaml index 5ba74c5c1..0bff462a7 100644 --- a/husarion_ugv_controller/config/WH04_controller.yaml +++ b/husarion_ugv_controller/config/WH04_controller.yaml @@ -9,6 +9,8 @@ type: diff_drive_controller/DiffDriveController imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster + twist_mux_controller: + type: twist_mux_controller/TwistMuxController # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications imu_broadcaster: @@ -79,3 +81,22 @@ max_deceleration_reverse: 3.2 # rad/s^2 max_jerk: .NAN # rad/s^3 min_jerk: .NAN # rad/s^3 + + twist_mux_controller: + ros__parameters: + holonomic: false + cmd_vel_inputs: + manual: + topic: manual/cmd_vel + timeout: 0.2 + priority: 100 + autonomous: + topic: autonomous/cmd_vel + timeout: 0.2 + priority: 10 + unknown: + topic: cmd_vel + timeout: 0.2 + priority: 1 + command_interface_linear_x: drive_controller/linear/velocity + command_interface_angular_z: drive_controller/angular/velocity diff --git a/husarion_ugv_controller/config/WH05_controller.yaml b/husarion_ugv_controller/config/WH05_controller.yaml index 2c7503e7c..92e4f3035 100644 --- a/husarion_ugv_controller/config/WH05_controller.yaml +++ b/husarion_ugv_controller/config/WH05_controller.yaml @@ -9,6 +9,8 @@ type: diff_drive_controller/DiffDriveController imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster + twist_mux_controller: + type: twist_mux_controller/TwistMuxController # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications imu_broadcaster: @@ -80,3 +82,22 @@ max_deceleration_reverse: 5.74 # rad/s^2 max_jerk: .NAN # rad/s^3 min_jerk: .NAN # rad/s^3 + + twist_mux_controller: + ros__parameters: + holonomic: false + cmd_vel_inputs: + manual: + topic: manual/cmd_vel + timeout: 0.2 + priority: 100 + autonomous: + topic: autonomous/cmd_vel + timeout: 0.2 + priority: 10 + unknown: + topic: cmd_vel + timeout: 0.2 + priority: 1 + command_interface_linear_x: drive_controller/linear/velocity + command_interface_angular_z: drive_controller/angular/velocity diff --git a/husarion_ugv_controller/launch/controller.launch.py b/husarion_ugv_controller/launch/controller.launch.py index fcac99172..cce721745 100644 --- a/husarion_ugv_controller/launch/controller.launch.py +++ b/husarion_ugv_controller/launch/controller.launch.py @@ -155,12 +155,16 @@ def generate_launch_description(): ("/diagnostics", "diagnostics"), ("drive_controller/cmd_vel", "cmd_vel"), ("drive_controller/odom", "odometry/wheels"), - ("drive_controller/transition_event", "_drive_controller/transition_event"), + ("drive_controller/transition_event", "drive_controller/_transition_event"), ("imu_broadcaster/imu", "imu/data"), - ("imu_broadcaster/transition_event", "_imu_broadcaster/transition_event"), + ("imu_broadcaster/transition_event", "imu_broadcaster/_transition_event"), ( "joint_state_broadcaster/transition_event", - "_joint_state_broadcaster/transition_event", + "joint_state_broadcaster/_transition_event", + ), + ( + "twist_mux_controller/transition_event", + "twist_mux_controller/_transition_event", ), ], arguments=[ @@ -200,6 +204,7 @@ def generate_launch_description(): "joint_state_broadcaster", "drive_controller", "imu_broadcaster", + "twist_mux_controller", "--activate-as-group", *spawner_common_args, ], diff --git a/husarion_ugv_controller/package.xml b/husarion_ugv_controller/package.xml index f4fef27b2..d4596b601 100644 --- a/husarion_ugv_controller/package.xml +++ b/husarion_ugv_controller/package.xml @@ -28,6 +28,7 @@ mecanum_drive_controller nav2_common robot_state_publisher + twist_mux_controller xacro diff --git a/husarion_ugv_description/urdf/common/gazebo.urdf.xacro b/husarion_ugv_description/urdf/common/gazebo.urdf.xacro index 546d9f4af..fde0cbcf2 100644 --- a/husarion_ugv_description/urdf/common/gazebo.urdf.xacro +++ b/husarion_ugv_description/urdf/common/gazebo.urdf.xacro @@ -43,6 +43,7 @@ imu_broadcaster/imu:=imu/data imu_broadcaster/transition_event:=imu_broadcaster/_transition_event joint_state_broadcaster/transition_event:=joint_state_broadcaster/_transition_event + twist_mux_controller/transition_event:=twist_mux_controller/_transition_event diff --git a/husarion_ugv_gazebo/README.md b/husarion_ugv_gazebo/README.md index fad8cea15..b30dd61cb 100644 --- a/husarion_ugv_gazebo/README.md +++ b/husarion_ugv_gazebo/README.md @@ -20,6 +20,7 @@ ros2 launch husarion_ugv_gazebo simulation.launch.py gz_world:= Where `` is either a path to an SDF file or the name of a built-in world in `husarion_gz_worlds`. The default world is `husarion_world`. Available worlds: + - `cave` - `empty_with_plugins` - `husarion_office` diff --git a/husarion_ugv_gazebo/config/robot_bridge.yaml b/husarion_ugv_gazebo/config/robot_bridge.yaml index 8a466b02f..8eeab0942 100644 --- a/husarion_ugv_gazebo/config/robot_bridge.yaml +++ b/husarion_ugv_gazebo/config/robot_bridge.yaml @@ -7,10 +7,20 @@ gz_type_name: gz.msgs.BatteryState direction: GZ_TO_ROS +- topic_name: //manual/cmd_vel + ros_type_name: geometry_msgs/msg/TwistStamped + gz_type_name: gz.msgs.Twist + direction: BIDIRECTIONAL + +- topic_name: //autonomous/cmd_vel + ros_type_name: geometry_msgs/msg/TwistStamped + gz_type_name: gz.msgs.Twist + direction: BIDIRECTIONAL + - topic_name: //cmd_vel ros_type_name: geometry_msgs/msg/TwistStamped gz_type_name: gz.msgs.Twist - direction: GZ_TO_ROS + direction: BIDIRECTIONAL - topic_name: //lights/channel_1_frame ros_type_name: sensor_msgs/msg/Image diff --git a/husarion_ugv_gazebo/config/teleop_with_estop.config b/husarion_ugv_gazebo/config/teleop_with_estop.config index 77cc63b51..3ee27359d 100644 --- a/husarion_ugv_gazebo/config/teleop_with_estop.config +++ b/husarion_ugv_gazebo/config/teleop_with_estop.config @@ -1194,7 +1194,7 @@ - {namespace}/cmd_vel + {namespace}/manual/cmd_vel 0 0 diff --git a/husarion_ugv_localization/config/enu_localization.yaml b/husarion_ugv_localization/config/enu_localization.yaml index d02f16037..2d0344c4a 100644 --- a/husarion_ugv_localization/config/enu_localization.yaml +++ b/husarion_ugv_localization/config/enu_localization.yaml @@ -40,7 +40,7 @@ use_control: true stamped_control: true - control_timeout: 0.5 + control_timeout: 0.2 control_config: [true, true, false, false, false, true] acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config diff --git a/husarion_ugv_localization/config/enu_localization_with_gps.yaml b/husarion_ugv_localization/config/enu_localization_with_gps.yaml index dba9206e8..8f687e894 100644 --- a/husarion_ugv_localization/config/enu_localization_with_gps.yaml +++ b/husarion_ugv_localization/config/enu_localization_with_gps.yaml @@ -50,7 +50,7 @@ use_control: true stamped_control: true - control_timeout: 0.5 + control_timeout: 0.2 control_config: [true, true, false, false, false, true] acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config diff --git a/husarion_ugv_localization/config/relative_localization.yaml b/husarion_ugv_localization/config/relative_localization.yaml index 7310f226b..55d192a93 100644 --- a/husarion_ugv_localization/config/relative_localization.yaml +++ b/husarion_ugv_localization/config/relative_localization.yaml @@ -40,7 +40,7 @@ use_control: true stamped_control: true - control_timeout: 0.5 + control_timeout: 0.2 control_config: [true, true, false, false, false, true] acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config diff --git a/husarion_ugv_localization/config/relative_localization_with_gps.yaml b/husarion_ugv_localization/config/relative_localization_with_gps.yaml index 9a8c68f75..7646df431 100644 --- a/husarion_ugv_localization/config/relative_localization_with_gps.yaml +++ b/husarion_ugv_localization/config/relative_localization_with_gps.yaml @@ -50,7 +50,7 @@ use_control: true stamped_control: true - control_timeout: 0.5 + control_timeout: 0.2 control_config: [true, true, false, false, false, true] acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config From 12a443cb79f8d05c2e31f74a681f6c2ad7144f01 Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 12 Aug 2025 08:00:07 +0000 Subject: [PATCH 2/6] Use source of velocity command for lights system --- husarion_ugv_manager/CMakeLists.txt | 10 ++ .../behavior_trees/LightsBT.btproj | 4 + .../behavior_trees/lights.xml | 38 +++-- .../config/lights_manager.yaml | 1 + .../plugins/condition/check_string_msg.hpp | 56 +++++++ .../plugins/condition/check_string_msg.cpp | 39 +++++ .../condition/test_check_string_msg.cpp | 155 ++++++++++++++++++ 7 files changed, 293 insertions(+), 10 deletions(-) create mode 100644 husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp create mode 100644 husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp create mode 100644 husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp 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/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/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/plugins/condition/check_string_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp new file mode 100644 index 000000000..9f3c77f18 --- /dev/null +++ b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp @@ -0,0 +1,39 @@ +// 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; + getInput("data", expected_data); + + 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..1a4c01c70 --- /dev/null +++ b/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp @@ -0,0 +1,155 @@ +// 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, 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; +} From c0b9e7eec378c04a054ae7a8150fabbe062bd906 Mon Sep 17 00:00:00 2001 From: kmakd Date: Thu, 14 Aug 2025 11:31:13 +0000 Subject: [PATCH 3/6] remove joy subscriber --- husarion_ugv_lights/CONFIGURATION.md | 2 +- .../husarion_ugv_manager/lights_manager_node.hpp | 4 ---- husarion_ugv_manager/src/lights_manager_node.cpp | 10 ---------- .../test/test_lights_behavior_tree.cpp | 1 + 4 files changed, 2 insertions(+), 15 deletions(-) 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/include/husarion_ugv_manager/lights_manager_node.hpp b/husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp index 16c63209a..2dfdc6637 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,7 +71,6 @@ 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; @@ -85,7 +82,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/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/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)); From bfb5bd184d591b6a8e5f135eeba85e23c8e21f7f Mon Sep 17 00:00:00 2001 From: kmakd Date: Thu, 14 Aug 2025 11:35:57 +0000 Subject: [PATCH 4/6] update docs --- husarion_ugv_manager/README.md | 1 + 1 file changed, 1 insertion(+) 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) From 06d69ea0a836527366abe4163c2a2f9a005f5490 Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 19 Aug 2025 08:23:16 +0000 Subject: [PATCH 5/6] add input check --- .../src/plugins/condition/check_string_msg.cpp | 5 ++++- .../plugins/condition/test_check_string_msg.cpp | 15 +++++++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp index 9f3c77f18..7bafdbdd8 100644 --- a/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp +++ b/husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp @@ -25,7 +25,10 @@ namespace husarion_ugv_manager BT::NodeStatus CheckStringMsg::onTick(const StringMsg::SharedPtr & last_msg) { std::string expected_data; - getInput("data", 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; 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 index 1a4c01c70..666252791 100644 --- a/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp +++ b/husarion_ugv_manager/test/plugins/condition/test_check_string_msg.cpp @@ -65,6 +65,21 @@ StringMsg TestCheckStringMsg::CreateMsg(const std::string & 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"}}; From 181c2a5091ea95ac5ff726991a3ae2aed93748ca Mon Sep 17 00:00:00 2001 From: kmakd Date: Fri, 22 Aug 2025 10:46:44 +0000 Subject: [PATCH 6/6] remove unused variable --- .../include/husarion_ugv_manager/lights_manager_node.hpp | 2 -- 1 file changed, 2 deletions(-) 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 2dfdc6637..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 @@ -73,8 +73,6 @@ class LightsManagerNode : public rclcpp::Node void EStopCB(const BoolMsg::SharedPtr e_stop); void LightsTreeTimerCB(); - static constexpr std::size_t kDeadManButtonIndex = 4; - float update_charging_anim_step_; std::shared_ptr param_listener_;