From 4189b20b8f908c76764d358e854b513b969e4966 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 8 Nov 2024 10:20:06 +0100 Subject: [PATCH 1/3] Ros2 docking manager no lights (#430) * Manage lights * Simple working version * Clean up * Clean up and add types enums * Simplifications * Change ports * Add use_docking arg * Fix port * Fix port * Add more cases to tests * update * Fix ports * Add dock_pose field, improve description, add dependencies * Docking manager without lights * Auto review * Update CONFIGURATION.md * Typo * Coderabbit suggestions * Jakub suggestions * Fix tests * Jakub suggestions --- README.md | 2 + .../src/system_monitor_node.cpp | 1 - panther_docking/launch/docking.launch.py | 64 +++--- .../launch/simulate_robot.launch.py | 14 ++ panther_gazebo/package.xml | 1 + panther_manager/CMakeLists.txt | 105 ++++------ panther_manager/CONFIGURATION.md | 9 + .../behavior_trees/DockingBT.btproj | 28 +++ panther_manager/behavior_trees/docking.xml | 62 ++++++ panther_manager/config/docking_manager.yaml | 12 ++ panther_manager/config/lights_manager.yaml | 1 + .../panther_manager/behavior_tree_utils.hpp | 78 ++++++-- .../panther_manager/docking_manager_node.hpp | 67 +++++++ .../action/call_set_bool_service_node.hpp | 3 +- .../call_set_led_animation_service_node.hpp | 10 +- .../plugins/action/dock_robot_node.hpp | 39 ++-- .../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 +- ...buttons_pressed.hpp => check_bool_msg.hpp} | 24 +-- .../plugins/condition/check_joy_msg.hpp | 68 +++++++ .../decorator/tick_after_timeout_node.hpp | 3 +- panther_manager/launch/manager.launch.py | 23 +++ panther_manager/package.xml | 3 + panther_manager/src/docking_manager_node.cpp | 122 ++++++++++++ .../src/docking_manager_node_main.cpp | 38 ++++ panther_manager/src/lights_manager_node.cpp | 28 +-- .../src/plugins/action/dock_robot_node.cpp | 3 - .../src/plugins/action/undock_robot_node.cpp | 1 - .../plugins/condition/are_buttons_pressed.cpp | 48 ----- .../src/plugins/condition/check_bool_msg.cpp | 34 ++++ .../src/plugins/condition/check_joy_msg.cpp | 92 +++++++++ panther_manager/src/safety_manager_node.cpp | 22 +-- .../condition/test_are_buttons_pressed.cpp | 119 ----------- .../plugins/condition/test_check_bool_msg.cpp | 102 ++++++++++ .../plugins/condition/test_check_joy_msg.cpp | 186 ++++++++++++++++++ .../test/test_behavior_tree_utils.cpp | 60 ++++-- 38 files changed, 1136 insertions(+), 378 deletions(-) create mode 100644 panther_manager/behavior_trees/DockingBT.btproj create mode 100644 panther_manager/behavior_trees/docking.xml create mode 100644 panther_manager/config/docking_manager.yaml create mode 100644 panther_manager/include/panther_manager/docking_manager_node.hpp rename panther_manager/include/panther_manager/plugins/condition/{are_buttons_pressed.hpp => check_bool_msg.hpp} (61%) create mode 100644 panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp create mode 100644 panther_manager/src/docking_manager_node.cpp create mode 100644 panther_manager/src/docking_manager_node_main.cpp delete mode 100644 panther_manager/src/plugins/condition/are_buttons_pressed.cpp create mode 100644 panther_manager/src/plugins/condition/check_bool_msg.cpp create mode 100644 panther_manager/src/plugins/condition/check_joy_msg.cpp delete mode 100644 panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp create mode 100644 panther_manager/test/plugins/condition/test_check_bool_msg.cpp create mode 100644 panther_manager/test/plugins/condition/test_check_joy_msg.cpp diff --git a/README.md b/README.md index 633e44f3..3e7a1f2e 100644 --- a/README.md +++ b/README.md @@ -88,6 +88,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `components_config_path` | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options).
***string:*** [`components.yaml`](./panther_description/config/components.yaml) | | 🤖🖥️ | `controller_config_path` | Path to controller configuration file. A path to custom configuration can be specified here.
***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/) | | 🤖 | `disable_manager` | Enable or disable manager_bt_node.
***bool:*** `False` | +| 🤖🖥️ | `docking_bt_project_path` | Path to BehaviorTree project file, responsible for docking management.
***string:*** [`DockingBT.btproj`](./panther_manager/behavior_trees/DockingBT.btproj) | | 🤖🖥️ | `fuse_gps` | Include GPS for data fusion.
***bool:*** `False` | | 🖥️ | `gz_bridge_config_path` | Path to the parameter_bridge configuration file.
***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml) | | 🖥️ | `gz_gui` | Run simulation with specific GUI layout.
***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config) | @@ -104,6 +105,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🖥️ | `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:*** `''` | | 🤖🖥️ | `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:*** `''` | diff --git a/panther_diagnostics/src/system_monitor_node.cpp b/panther_diagnostics/src/system_monitor_node.cpp index bc921a75..5da51464 100644 --- a/panther_diagnostics/src/system_monitor_node.cpp +++ b/panther_diagnostics/src/system_monitor_node.cpp @@ -28,7 +28,6 @@ #include "panther_utils/ros_utils.hpp" #include "panther_diagnostics/filesystem.hpp" -#include "panther_diagnostics/types.hpp" namespace panther_diagnostics { diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index d386bd31..84fa4b44 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 ad7252e7..aa55478a 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 89e7baaa..184e07c1 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/CMakeLists.txt b/panther_manager/CMakeLists.txt index 1f7bcfb9..1ba4ddde 100644 --- a/panther_manager/CMakeLists.txt +++ b/panther_manager/CMakeLists.txt @@ -11,14 +11,17 @@ set(PACKAGE_DEPENDENCIES behaviortree_cpp behaviortree_ros2 libssh + geometry_msgs + opennav_docking_msgs panther_msgs panther_utils rclcpp rclcpp_action 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) @@ -59,9 +62,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_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) @@ -75,32 +82,25 @@ endforeach() add_executable( safety_manager_node src/safety_manager_node_main.cpp src/safety_manager_node.cpp src/behavior_tree_manager.cpp) -ament_target_dependencies( - safety_manager_node - behaviortree_ros2 - panther_msgs - panther_utils - rclcpp - sensor_msgs - std_msgs) +ament_target_dependencies(safety_manager_node ${PACKAGE_DEPENDENCIES}) target_link_libraries(safety_manager_node ${plugin_libs}) add_executable( lights_manager_node src/lights_manager_node_main.cpp src/lights_manager_node.cpp src/behavior_tree_manager.cpp) -ament_target_dependencies( - lights_manager_node - behaviortree_ros2 - panther_msgs - panther_utils - rclcpp - sensor_msgs - std_msgs) +ament_target_dependencies(lights_manager_node ${PACKAGE_DEPENDENCIES}) target_link_libraries(lights_manager_node ${plugin_libs}) +add_executable( + docking_manager_node + src/docking_manager_node_main.cpp src/docking_manager_node.cpp + src/behavior_tree_manager.cpp) +ament_target_dependencies(docking_manager_node ${PACKAGE_DEPENDENCIES}) +target_link_libraries(docking_manager_node ${plugin_libs}) + install(TARGETS ${plugin_libs} DESTINATION lib) -install(TARGETS safety_manager_node lights_manager_node +install(TARGETS safety_manager_node lights_manager_node docking_manager_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY behavior_trees config launch @@ -161,10 +161,16 @@ 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_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 + 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 @@ -198,7 +204,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_utils - behaviortree_cpp behaviortree_ros2 panther_utils) + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_behavior_tree_manager @@ -217,15 +223,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_lights_manager_node PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_lights_manager_node - behaviortree_cpp - behaviortree_ros2 - panther_msgs - panther_utils - rclcpp - sensor_msgs - std_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_lights_manager_node + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_lights_behavior_tree @@ -235,15 +234,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_lights_behavior_tree PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_lights_behavior_tree - behaviortree_cpp - behaviortree_ros2 - panther_msgs - panther_utils - rclcpp - sensor_msgs - std_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_lights_behavior_tree + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp @@ -252,15 +244,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_safety_manager_node PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_safety_manager_node - behaviortree_cpp - behaviortree_ros2 - panther_msgs - panther_utils - rclcpp - sensor_msgs - std_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_safety_manager_node + ${PACKAGE_DEPENDENCIES}) ament_add_gtest( ${PROJECT_NAME}_test_safety_behavior_tree @@ -270,16 +255,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_safety_behavior_tree PUBLIC $ $) - ament_target_dependencies( - ${PROJECT_NAME}_test_safety_behavior_tree - behaviortree_cpp - behaviortree_ros2 - panther_msgs - panther_utils - rclcpp - sensor_msgs - std_msgs - std_srvs) + ament_target_dependencies(${PROJECT_NAME}_test_safety_behavior_tree + ${PACKAGE_DEPENDENCIES}) endif() ament_package() diff --git a/panther_manager/CONFIGURATION.md b/panther_manager/CONFIGURATION.md index 3317e43a..a0a5beb0 100644 --- a/panther_manager/CONFIGURATION.md +++ b/panther_manager/CONFIGURATION.md @@ -93,6 +93,14 @@ For a BehaviorTree project to work correctly, it must contain a tree with correc ### Trees +#### Docking + +A tree responsible for waiting for the joystick command and trigger `Docking`/`Undocking` action. + +

+ Docking Behavior Tree +

+ #### Lights A tree responsible for scheduling animations displayed on the Bumper Lights based on the Husarion Panther robot's system state. @@ -206,6 +214,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: +- Docking tree: `10.15.20.2:4444` - Lights 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/DockingBT.btproj b/panther_manager/behavior_trees/DockingBT.btproj new file mode 100644 index 00000000..2752c0d3 --- /dev/null +++ b/panther_manager/behavior_trees/DockingBT.btproj @@ -0,0 +1,28 @@ + + + + + + + +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 + 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/behavior_trees/docking.xml b/panther_manager/behavior_trees/docking.xml new file mode 100644 index 00000000..1118dc10 --- /dev/null +++ b/panther_manager/behavior_trees/docking.xml @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + 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 + 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/docking_manager.yaml b/panther_manager/config/docking_manager.yaml new file mode 100644 index 00000000..28879a8b --- /dev/null +++ b/panther_manager/config/docking_manager.yaml @@ -0,0 +1,12 @@ +/**: + docking_manager: + ros__parameters: + timer_frequency: 50.0 + bt_server_port: 4444 + ros_communication_timeout: + availability: 2.0 + response: 1.0 + ros_plugin_libs: + - check_joy_msg_bt_node + - dock_robot_bt_node + - undock_robot_bt_node diff --git a/panther_manager/config/lights_manager.yaml b/panther_manager/config/lights_manager.yaml index cea2a5e4..f9033f68 100644 --- a/panther_manager/config/lights_manager.yaml +++ b/panther_manager/config/lights_manager.yaml @@ -2,6 +2,7 @@ lights_manager: ros__parameters: timer_frequency: 10.0 + bt_server_port: 5555 battery: percent: window_len: 6 diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp index 5ee2331e..db7f3212 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 { @@ -93,28 +96,75 @@ 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; } + +/** + * @brief Converts a string to a PoseStamped message. + * + * The string format should be "x;y;z;roll;pitch;yaw;frame_id" where: + * - x, y, z: Position coordinates. + * - roll, pitch, yaw: Rotation around axes XYZ. + * - 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) +{ + constexpr std::size_t expected_parts_size = 7; + + auto parts = splitString(str, ';'); + if (parts.size() != expected_parts_size) { + throw BT::RuntimeError( + "Invalid input for PoseStamped. Expected " + std::to_string(expected_parts_size) + + " values: x;y;z;roll;pitch;yaw;frame_id"); + } + + geometry_msgs::msg::PoseStamped pose_stamped; + + try { + pose_stamped.pose.position.x = convertFromString(parts[0]); + pose_stamped.pose.position.y = convertFromString(parts[1]); + pose_stamped.pose.position.z = convertFromString(parts[2]); + + const auto roll = convertFromString(parts[3]); + const auto pitch = convertFromString(parts[4]); + const auto yaw = convertFromString(parts[5]); + tf2::Quaternion quaternion; + quaternion.setRPY(roll, pitch, yaw); + pose_stamped.pose.orientation = tf2::toMsg(quaternion); + + 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/docking_manager_node.hpp b/panther_manager/include/panther_manager/docking_manager_node.hpp new file mode 100644 index 00000000..7c311b75 --- /dev/null +++ b/panther_manager/include/panther_manager/docking_manager_node.hpp @@ -0,0 +1,67 @@ +// 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_DOCKING_MANAGER_NODE_HPP_ +#define PANTHER_MANAGER_DOCKING_MANAGER_NODE_HPP_ + +#include +#include + +#include +#include + +#include + +#include "panther_manager/behavior_tree_manager.hpp" +#include "panther_utils/moving_average.hpp" + +namespace panther_manager +{ + +using BoolMsg = std_msgs::msg::Bool; + +/** + * @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() = default; + + /** + * @brief Initializes the docking manager, setting up parameters and behavior tree. + * @throws std::runtime_error if initialization fails + */ + void Initialize(); + +protected: + void DeclareParameters(); + void RegisterBehaviorTree(); + + std::unique_ptr docking_tree_manager_; + +private: + void TimerCB(); + + rclcpp::TimerBase::SharedPtr docking_tree_timer_; + + BT::BehaviorTreeFactory factory_; +}; + +} // namespace panther_manager + +#endif // PANTHER_MANAGER_DOCKING_MANAGER_NODE_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 adafe1d4..96f40482 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 38a915a5..a0dcff1e 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 2e4a5c03..7793bd21 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,22 +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( - "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", "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 949565f6..c9eaa79e 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 630db650..006ac53a 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", "A 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 7bc9709d..5eedaf7c 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", "", "A reason to shutdown a 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 ade7380f..24589118 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,16 +49,15 @@ 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."), + BT::InputPort( + "max_undocking_time", 30.0, + "Maximum allowable time (in seconds) to undock and return to the staging pose."), - BT::OutputPort( - "success", "If the action was successful"), - 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/are_buttons_pressed.hpp b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp similarity index 61% 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 8b0534b5..fbe52a88 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 +// 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; + 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) { } - 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", "Specifies the expected state of the data field.")}); } - -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 00000000..a7966482 --- /dev/null +++ b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp @@ -0,0 +1,68 @@ +// 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", "", + "Specifies the expected state of the axes field. An empty string (\"\") means the values " + "are ignored."), + BT::InputPort>( + "buttons", "", + "Specifies the expected state of the buttons field. An empty string (\"\") means values " + "are ignored."), + BT::InputPort( + "timeout", 0.0, "Maximum allowable time delay to accept the 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/plugins/decorator/tick_after_timeout_node.hpp b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp index 20a273e5..af58ec3b 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/launch/manager.launch.py b/panther_manager/launch/manager.launch.py index 14f4c8c7..c2ab3b18 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", "DockingBT.btproj"] + ), + description="Path to BehaviorTree project file, responsible for docking 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", ) + docking_manager_node = Node( + package="panther_manager", + executable="docking_manager_node", + name="docking_manager", + parameters=[ + PathJoinSubstitution([panther_manager_dir, "config", "docking_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, + docking_manager_node, lights_manager_node, safety_manager_node, ] diff --git a/panther_manager/package.xml b/panther_manager/package.xml index 6da23d97..a9cd5f76 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 diff --git a/panther_manager/src/docking_manager_node.cpp b/panther_manager/src/docking_manager_node.cpp new file mode 100644 index 00000000..8cd485f3 --- /dev/null +++ b/panther_manager/src/docking_manager_node.cpp @@ -0,0 +1,122 @@ +// 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/docking_manager_node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#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 std::map empty_bb = {}; + const int bt_server_port = this->get_parameter("bt_server_port").as_int(); + + docking_tree_manager_ = std::make_unique( + "Docking", empty_bb, bt_server_port); + + 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; + + const auto timer_freq = this->get_parameter("timer_frequency").as_double(); + const auto timer_period = std::chrono::duration(1.0 / timer_freq); + + docking_tree_timer_ = this->create_wall_timer( + timer_period, std::bind(&DockingManagerNode::TimerCB, this)); +} + +void DockingManagerNode::DeclareParameters() +{ + const auto panther_manager_pkg_path = + ament_index_cpp::get_package_share_directory("panther_manager"); + const auto default_bt_project_path = panther_manager_pkg_path + + "/behavior_trees/DockingBT.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("timer_frequency", 20.0); + this->declare_parameter("bt_server_port", 4444); +} + +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(); + auto wait_for_server_timeout_s = std::chrono::duration(service_availability_timeout); + params.wait_for_server_timeout = + std::chrono::duration_cast(wait_for_server_timeout_s); + auto server_timeout_s = std::chrono::duration(service_response_timeout); + params.server_timeout = std::chrono::duration_cast(server_timeout_s); + + behavior_tree_utils::RegisterBehaviorTree( + factory_, bt_project_path, plugin_libs, params, ros_plugin_libs); + + RCLCPP_INFO_STREAM( + this->get_logger(), "BehaviorTree registered from path '" << bt_project_path << "'"); +} + +void DockingManagerNode::TimerCB() +{ + docking_tree_manager_->TickOnce(); + + if (docking_tree_manager_->GetTreeStatus() == BT::NodeStatus::FAILURE) { + RCLCPP_WARN(this->get_logger(), "Docking behavior tree returned FAILURE status"); + } +} + +} // namespace panther_manager diff --git a/panther_manager/src/docking_manager_node_main.cpp b/panther_manager/src/docking_manager_node_main.cpp new file mode 100644 index 00000000..926a9c44 --- /dev/null +++ b/panther_manager/src/docking_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/docking_manager_node.hpp" + +#include +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto docking_manager_node = + std::make_shared("docking_manager"); + docking_manager_node->Initialize(); + + try { + rclcpp::spin(docking_manager_node); + } catch (const std::runtime_error & err) { + std::cerr << "[docking_manager] Caught exception: " << err.what() << std::endl; + } + + std::cout << "[docking_manager] Shutting down" << std::endl; + rclcpp::shutdown(); + return 0; +} diff --git a/panther_manager/src/lights_manager_node.cpp b/panther_manager/src/lights_manager_node.cpp index 53ff612b..0a31e075 100644 --- a/panther_manager/src/lights_manager_node.cpp +++ b/panther_manager/src/lights_manager_node.cpp @@ -48,8 +48,10 @@ LightsManagerNode::LightsManagerNode( battery_percent_ma_ = std::make_unique>( battery_percent_window_len, 1.0); + const auto bt_server_port = this->get_parameter("bt_server_port").as_int(); const auto initial_blackboard = CreateLightsInitialBlackboard(); - lights_tree_manager_ = std::make_unique("Lights", initial_blackboard, 5555); + lights_tree_manager_ = std::make_unique( + "Lights", initial_blackboard, bt_server_port); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } @@ -69,12 +71,11 @@ void LightsManagerNode::Initialize() "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&LightsManagerNode::EStopCB, this, _1)); - const float timer_freq = this->get_parameter("timer_frequency").as_double(); - const auto timer_period_ms = - std::chrono::milliseconds(static_cast(1.0f / timer_freq * 1000)); + const auto timer_freq = this->get_parameter("timer_frequency").as_double(); + const auto timer_period = std::chrono::duration(1.0 / timer_freq); lights_tree_timer_ = this->create_wall_timer( - timer_period_ms, std::bind(&LightsManagerNode::LightsTreeTimerCB, this)); + timer_period, std::bind(&LightsManagerNode::LightsTreeTimerCB, this)); RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } @@ -83,8 +84,8 @@ void LightsManagerNode::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/PantherLightsBT.btproj"; + const auto default_bt_project_path = panther_manager_pkg_path + + "/behavior_trees/PantherLightsBT.btproj"; const std::vector default_plugin_libs = {}; this->declare_parameter("bt_project_path", default_bt_project_path); @@ -100,6 +101,7 @@ void LightsManagerNode::DeclareParameters() 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("bt_server_port", 5555); } void LightsManagerNode::RegisterBehaviorTree() @@ -116,16 +118,17 @@ void LightsManagerNode::RegisterBehaviorTree() BT::RosNodeParams params; params.nh = this->shared_from_this(); + auto wait_for_server_timeout_s = std::chrono::duration(service_availability_timeout); 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)); + std::chrono::duration_cast(wait_for_server_timeout_s); + auto server_timeout_s = std::chrono::duration(service_response_timeout); + params.server_timeout = std::chrono::duration_cast(server_timeout_s); 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()); + RCLCPP_INFO_STREAM( + this->get_logger(), "BehaviorTree registered from path '" << bt_project_path << "'"); } std::map LightsManagerNode::CreateLightsInitialBlackboard() @@ -170,7 +173,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."); diff --git a/panther_manager/src/plugins/action/dock_robot_node.cpp b/panther_manager/src/plugins/action/dock_robot_node.cpp index 065476be..8f5a60cf 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 89c8cc10..e47c000a 100644 --- a/panther_manager/src/plugins/action/undock_robot_node.cpp +++ b/panther_manager/src/plugins/action/undock_robot_node.cpp @@ -39,7 +39,6 @@ BT::NodeStatus UndockRobot::onResultReceived(const WrappedResult & wr) { const auto & result = wr.result; - this->setOutput("success", result->success); this->setOutput("error_code", result->error_code); if (result->success) { diff --git a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp b/panther_manager/src/plugins/condition/are_buttons_pressed.cpp deleted file mode 100644 index 1aad81af..00000000 --- a/panther_manager/src/plugins/condition/are_buttons_pressed.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// 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/plugins/condition/are_buttons_pressed.hpp" - -#include "panther_manager/behavior_tree_utils.hpp" - -namespace panther_manager -{ - -BT::NodeStatus AreButtonsPressed::onTick(const std::shared_ptr & last_msg) -{ - getInput>("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( - 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())) { - return BT::NodeStatus::SUCCESS; - } - - return BT::NodeStatus::FAILURE; -} - -} // namespace panther_manager - -#include "behaviortree_ros2/plugins.hpp" -CreateRosNodePlugin(panther_manager::AreButtonsPressed, "AreButtonsPressed"); diff --git a/panther_manager/src/plugins/condition/check_bool_msg.cpp b/panther_manager/src/plugins/condition/check_bool_msg.cpp new file mode 100644 index 00000000..09cd7f0d --- /dev/null +++ b/panther_manager/src/plugins/condition/check_bool_msg.cpp @@ -0,0 +1,34 @@ +// 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/plugins/condition/check_bool_msg.hpp" + +#include "panther_manager/behavior_tree_utils.hpp" + +namespace panther_manager +{ + +BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg) +{ + bool expected_data; + getInput("data", expected_data); + + return (last_msg && last_msg->data == expected_data) ? BT::NodeStatus::SUCCESS + : BT::NodeStatus::FAILURE; +} + +} // namespace panther_manager + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(panther_manager::CheckBoolMsg, "CheckBoolMsg"); diff --git a/panther_manager/src/plugins/condition/check_joy_msg.cpp b/panther_manager/src/plugins/condition/check_joy_msg.cpp new file mode 100644 index 00000000..d6b51552 --- /dev/null +++ b/panther_manager/src/plugins/condition/check_joy_msg.cpp @@ -0,0 +1,92 @@ +// 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/plugins/condition/check_joy_msg.hpp" + +#include "panther_manager/behavior_tree_utils.hpp" + +namespace panther_manager +{ + +BT::NodeStatus CheckJoyMsg::onTick(const JoyMsg::SharedPtr & last_msg) +{ + return (last_msg && checkTimeout(last_msg) && checkAxes(last_msg) && checkButtons(last_msg)) + ? BT::NodeStatus::SUCCESS + : BT::NodeStatus::FAILURE; +} + +bool CheckJoyMsg::checkAxes(const JoyMsg::SharedPtr & last_msg) +{ + std::vector 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; + } + + return std::equal( + expected_axes.begin(), expected_axes.end(), last_msg->axes.begin(), + [](float a, float b) { return std::fabs(a - b) <= std::numeric_limits::epsilon(); }); +} + +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->buttons.size() + << " axes, expected at least " << expected_buttons.size()); + return false; + } + + return std::equal(expected_buttons.begin(), expected_buttons.end(), last_msg->buttons.begin()); +} + +bool CheckJoyMsg::checkTimeout(const JoyMsg::SharedPtr & last_msg) +{ + double max_timeout; + getInput("timeout", max_timeout); + + rclcpp::Time now; + { + auto node = node_.lock(); + if (!node) { + return false; + } + now = node->now(); + } + + const auto time_diff = now - last_msg->header.stamp; + return (max_timeout <= 0.0) || time_diff < rclcpp::Duration::from_seconds(max_timeout); +} + +} // namespace panther_manager + +#include "behaviortree_ros2/plugins.hpp" +CreateRosNodePlugin(panther_manager::CheckJoyMsg, "CheckJoyMsg"); diff --git a/panther_manager/src/safety_manager_node.cpp b/panther_manager/src/safety_manager_node.cpp index d204690c..c11058b3 100644 --- a/panther_manager/src/safety_manager_node.cpp +++ b/panther_manager/src/safety_manager_node.cpp @@ -101,12 +101,11 @@ void SafetyManagerNode::Initialize() // Timers // ------------------------------- - 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)); + const auto timer_freq = this->get_parameter("timer_frequency").as_double(); + const auto timer_period = std::chrono::duration(1.0 / timer_freq); safety_tree_timer_ = this->create_wall_timer( - timer_period_ms, std::bind(&SafetyManagerNode::SafetyTreeTimerCB, this)); + timer_period, std::bind(&SafetyManagerNode::SafetyTreeTimerCB, this)); RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } @@ -115,8 +114,8 @@ void SafetyManagerNode::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/PantherSafetyBT.btproj"; + const auto default_bt_project_path = panther_manager_pkg_path + + "/behavior_trees/PantherSafetyBT.btproj"; const std::vector default_plugin_libs = {}; this->declare_parameter("bt_project_path", default_bt_project_path); @@ -151,16 +150,17 @@ void SafetyManagerNode::RegisterBehaviorTree() BT::RosNodeParams params; params.nh = this->shared_from_this(); + auto wait_for_server_timeout_s = std::chrono::duration(service_availability_timeout); 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)); + std::chrono::duration_cast(wait_for_server_timeout_s); + auto server_timeout_s = std::chrono::duration(service_response_timeout); + params.server_timeout = std::chrono::duration_cast(server_timeout_s); 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()); + RCLCPP_INFO_STREAM( + this->get_logger(), "BehaviorTree registered from path '" << bt_project_path << "'"); } std::map SafetyManagerNode::CreateSafetyInitialBlackboard() diff --git a/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp b/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp deleted file mode 100644 index b563e7d5..00000000 --- a/panther_manager/test/plugins/condition/test_are_buttons_pressed.cpp +++ /dev/null @@ -1,119 +0,0 @@ -// 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 "panther_manager/plugins/condition/are_buttons_pressed.hpp" -#include "utils/plugin_test_utils.hpp" - -class TestAreButtonsPressed : public panther_manager::plugin_test_utils::PluginTestUtils -{ -public: - TestAreButtonsPressed(); - void PublishJoyMessage(const std::vector & buttons); - -protected: - rclcpp::Publisher::SharedPtr joy_publisher_; - std::map params_ = {{"topic_name", "joy"}, {"buttons", "0;1;0"}}; -}; - -TestAreButtonsPressed::TestAreButtonsPressed() -{ - RegisterNodeWithParams("AreButtonsPressed"); - joy_publisher_ = bt_node_->create_publisher("joy", 10); -} - -void TestAreButtonsPressed::PublishJoyMessage(const std::vector & buttons) -{ - sensor_msgs::msg::Joy msg; - msg.buttons = buttons; - joy_publisher_->publish(msg); -} - -TEST_F(TestAreButtonsPressed, LoadingAreButtonsPressedPlugin) -{ - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); -} - -TEST_F(TestAreButtonsPressed, NoMessage) -{ - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestAreButtonsPressed, WrongMessageTooFewButtons) -{ - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); - - PublishJoyMessage({0, 1}); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestAreButtonsPressed, GoodMessageWrongButtonsState) -{ - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); - - PublishJoyMessage({0, 0, 0}); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::FAILURE); -} - -TEST_F(TestAreButtonsPressed, GoodMessageWithTooMuchButtonsAndGoodButtonsState) -{ - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); - - PublishJoyMessage({0, 1, 0, 0, 0, 1}); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - EXPECT_EQ(status, BT::NodeStatus::SUCCESS); -} - -TEST_F(TestAreButtonsPressed, GoodMessageGoodButtonsState) -{ - ASSERT_NO_THROW({ CreateTree("AreButtonsPressed", params_); }); - - PublishJoyMessage({0, 1, 0}); - - auto & tree = GetTree(); - auto status = tree.tickWhileRunning(std::chrono::milliseconds(100)); - 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/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 00000000..c4cfc7cd --- /dev/null +++ b/panther_manager/test/plugins/condition/test_check_bool_msg.cpp @@ -0,0 +1,102 @@ +// 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, 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 new file mode 100644 index 00000000..fe12ecec --- /dev/null +++ b/panther_manager/test/plugins/condition/test_check_joy_msg.cpp @@ -0,0 +1,186 @@ +// 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 + +#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; +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(); + void PublishMsg(JoyMsg msg) { joy_publisher_->publish(msg); }; + JoyMsg CreateMsg( + const std::vector & axes = {}, const std::vector & buttons = {}, + const HeaderMsg & header = HeaderMsg()); + void SetCurrentMsgTime(JoyMsg & header); + +protected: + rclcpp::Publisher::SharedPtr joy_publisher_; +}; + +TestCheckJoyMsg::TestCheckJoyMsg() +{ + RegisterNodeWithParams(PLUGIN); + joy_publisher_ = bt_node_->create_publisher(TOPIC, 10); +} + +JoyMsg TestCheckJoyMsg::CreateMsg( + const std::vector & axes, const std::vector & buttons, const HeaderMsg & header) +{ + JoyMsg msg; + msg.header = header; + msg.axes = axes; + msg.buttons = buttons; + return msg; +} + +void TestCheckJoyMsg::SetCurrentMsgTime(JoyMsg & msg) { msg.header.stamp = bt_node_->now(); } + +TEST_F(TestCheckJoyMsg, NoMessageArrived) +{ + 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(); + EXPECT_EQ(status, BT::NodeStatus::FAILURE); +} + +TEST_F(TestCheckJoyMsg, TimeoutTests) +{ + std::vector test_cases = { + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.5"}}, + CreateMsg()}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.0"}}, + CreateMsg()}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "-0.5"}}, + CreateMsg()}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", ""}, {"timeout", "0.001"}}, + CreateMsg()}}; + + for (auto & test_case : test_cases) { + CreateTree(PLUGIN, test_case.input); + SetCurrentMsgTime(test_case.msg); + 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, OnTickBehavior) +{ + 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", "0.1;-0.2;1.0"}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}}, + CreateMsg({0.1, -0.2, 1.0}, {0, 0, 10, 0})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", "0.1;-0.2;1.0"}, {"buttons", ""}, {"timeout", "1.0"}}, + CreateMsg({0.1, -0.2, 1.0}, {0, 0, 10, 0})}, + {BT::NodeStatus::SUCCESS, + {{"topic_name", TOPIC}, {"axes", ""}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}}, + CreateMsg({0.1, -0.2, 1.0}, {0, 0, 10, 0})}, + {BT::NodeStatus::FAILURE, + {{"topic_name", TOPIC}, {"axes", "0.1;-0.2;1.0"}, {"buttons", "0;0;10;0"}, {"timeout", "1.0"}}, + CreateMsg({0.2, -0.2, 1.0}, {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); + SetCurrentMsgTime(test_case.msg); + 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/test_behavior_tree_utils.cpp b/panther_manager/test/test_behavior_tree_utils.cpp index a65c0a07..24d1c7de 100644 --- a/panther_manager/test/test_behavior_tree_utils.cpp +++ b/panther_manager/test/test_behavior_tree_utils.cpp @@ -26,6 +26,8 @@ #include "behaviortree_ros2/ros_node_params.hpp" #include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + #include "panther_manager/behavior_tree_utils.hpp" #include "panther_utils/test/test_utils.hpp" @@ -121,33 +123,59 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS) rclcpp::shutdown(); } -TEST(TestConvertFromString, GoodVectorInt) +TEST(TestConvertFromStringPoseStamped, GoodInput) { - std::string str = "1;2;3"; - auto result = BT::convertFromString>(str); + constexpr double time_threshold = 0.1; + constexpr float epsilon = 1e-3; + + auto str = "1;2;3;0.1;0.2;0.1;pose"; + auto result = BT::convertFromString(str); + + auto time_diff = rclcpp::Clock().now() - rclcpp::Time(result.header.stamp, RCL_SYSTEM_TIME); + EXPECT_LE(time_diff.seconds(), time_threshold); + EXPECT_EQ(result.header.frame_id, "pose"); - EXPECT_EQ(result.size(), 3); - EXPECT_EQ(result[0], 1); - EXPECT_EQ(result[1], 2); - EXPECT_EQ(result[2], 3); + EXPECT_NEAR(result.pose.position.x, 1, epsilon); + EXPECT_NEAR(result.pose.position.y, 2, epsilon); + EXPECT_NEAR(result.pose.position.z, 3, epsilon); + + EXPECT_NEAR(result.pose.orientation.x, 0.0447, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.1021, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0447, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 0.9928, epsilon); } -TEST(TestConvertFromString, EmptyVectorInt) +TEST(TestConvertFromStringPoseStamped, WrongInput) { - std::string str = ""; - EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); + auto str = ""; + EXPECT_THROW(BT::convertFromString(str), BT::RuntimeError); + str = "1;2;3;0.1;0.2;0.1;"; + EXPECT_THROW(BT::convertFromString(str), BT::RuntimeError); + str = "1;2;3;0.1;0.2;0.1;pose;0.1"; + EXPECT_THROW(BT::convertFromString(str), BT::RuntimeError); + str = "pose;1;2;3;0.1;0.2;0.1;"; + EXPECT_THROW(BT::convertFromString(str), BT::RuntimeError); } -TEST(TestConvertFromString, InvalidVectorIntWithFloat) +TEST(TestConvertFromStringVectorOfDouble, GoodInput) { - std::string str = "1.0;2;3;4"; - EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); + constexpr float epsilon = 1e-6; + + auto str = "1;2;3;0.1;0.2;0.1"; + auto result = BT::convertFromString>(str); + + EXPECT_NEAR(result[0], 1, epsilon); + EXPECT_NEAR(result[1], 2, epsilon); + EXPECT_NEAR(result[2], 3, epsilon); + EXPECT_NEAR(result[3], 0.1, epsilon); + EXPECT_NEAR(result[4], 0.2, epsilon); + EXPECT_NEAR(result[5], 0.1, epsilon); } -TEST(TestConvertFromString, InvalidVectorIntWithLetter) +TEST(TestConvertFromStringVectorOfFloat, WrongInput) { - std::string str = "a;2;3;4"; - EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); + auto str = "1;2;3;0.1;a;0.2"; + EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); } int main(int argc, char ** argv) From 5fc756025b667c57a2101deaab9dd3f8dd948dd6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Fri, 8 Nov 2024 12:09:03 +0100 Subject: [PATCH 2/3] ROS2 docking fix joy (#439) * Fixed utils test, fixed joy Signed-off-by: Jakub Delicat * Revert "Fixed utils test, fixed joy" This reverts commit 3bd5e305576b1ef8267fc09bc3c573d0ce21c3ed. * Fixed bt utils test, changed dock jot input Signed-off-by: Jakub Delicat * Joy input and typo Signed-off-by: Jakub Delicat --------- Signed-off-by: Jakub Delicat --- panther_manager/behavior_trees/docking.xml | 4 ++-- panther_manager/src/plugins/condition/check_joy_msg.cpp | 2 +- panther_manager/test/test_behavior_tree_utils.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/panther_manager/behavior_trees/docking.xml b/panther_manager/behavior_trees/docking.xml index 1118dc10..9d3b46bc 100644 --- a/panther_manager/behavior_trees/docking.xml +++ b/panther_manager/behavior_trees/docking.xml @@ -8,7 +8,7 @@ topic_name="joy" timeout="2.0" axes="" - buttons="0;0;0;1;1;1"/> + buttons="0;0;0;1;1;1;0;0;0;0;0;0"/> + buttons="1;0;0;0;1;1;0;0;0;0;0;0"/> logger(), GetLoggerPrefix(name()) << "Joy message has " << last_msg->buttons.size() - << " axes, expected at least " << expected_buttons.size()); + << " buttons, expected at least " << expected_buttons.size()); return false; } diff --git a/panther_manager/test/test_behavior_tree_utils.cpp b/panther_manager/test/test_behavior_tree_utils.cpp index 24d1c7de..c12f17b9 100644 --- a/panther_manager/test/test_behavior_tree_utils.cpp +++ b/panther_manager/test/test_behavior_tree_utils.cpp @@ -175,7 +175,7 @@ TEST(TestConvertFromStringVectorOfDouble, GoodInput) TEST(TestConvertFromStringVectorOfFloat, WrongInput) { auto str = "1;2;3;0.1;a;0.2"; - EXPECT_THROW(BT::convertFromString>(str), BT::RuntimeError); + EXPECT_THROW(BT::convertFromString>(str), std::invalid_argument); } int main(int argc, char ** argv) From 4ee2f683316c693caeeac13c3ddbe303047ac8ef Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Fri, 8 Nov 2024 17:16:26 +0100 Subject: [PATCH 3/3] Charging Dock Plugin based on dock pose subscriber (#431) * Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat * Changed GetDockPose approach Signed-off-by: Jakub Delicat * Cleanup charger Signed-off-by: Jakub Delicat * Added unit tests Signed-off-by: Jakub Delicat * Update docs: Signed-off-by: Jakub Delicat * Fix upside down Signed-off-by: Jakub Delicat * Added updateAndPublishStagingPose method Signed-off-by: Jakub Delicat * Added suggestins | added reading docking_server parameter is dock pose publisher Signed-off-by: Jakub Delicat * Remove redundant tests Signed-off-by: Jakub Delicat * Added aprilros launch to simulation Signed-off-by: Jakub Delicat * update vsc and changed dock name Signed-off-by: Jakub Delicat --------- Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- .../config/docking_components.yaml | 6 +- panther_docking/CMakeLists.txt | 13 +- panther_docking/README.md | 44 +- .../config/panther_docking_server.yaml | 34 +- .../panther_docking/panther_charging_dock.hpp | 57 +-- panther_docking/launch/docking.launch.py | 71 ++- panther_docking/launch/station.launch.py | 17 +- panther_docking/package.xml | 1 + .../src/dock_pose_publisher_node.cpp | 103 +++++ panther_docking/src/panther_charging_dock.cpp | 183 +++----- .../test/test_panther_charging_dock.cpp | 418 ------------------ .../test/unit/test_panther_charging_dock.cpp | 202 +++++++++ panther_gazebo/config/apriltag.yaml | 23 + panther_gazebo/launch/apriltag.launch.py | 62 +++ .../launch/simulate_robot.launch.py | 14 + panther_gazebo/package.xml | 1 + panther_manager/behavior_trees/docking.xml | 2 +- 19 files changed, 583 insertions(+), 672 deletions(-) create mode 100644 panther_docking/src/dock_pose_publisher_node.cpp delete mode 100644 panther_docking/test/test_panther_charging_dock.cpp create mode 100644 panther_docking/test/unit/test_panther_charging_dock.cpp create mode 100644 panther_gazebo/config/apriltag.yaml create mode 100644 panther_gazebo/launch/apriltag.launch.py diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index c6df86e1..0e267fba 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: 99e0189970293bb0110aad0a3b609bc659ecc663 + version: b29f41ac00ab1a6fbac3e1d03602575094de277a ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index d6c6932f..24751fd7 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: 99e0189970293bb0110aad0a3b609bc659ecc663 + version: b29f41ac00ab1a6fbac3e1d03602575094de277a ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index 169b3a13..940eb137 100644 --- a/panther_description/config/docking_components.yaml +++ b/panther_description/config/docking_components.yaml @@ -9,12 +9,12 @@ components: - type: WCH02 parent_link: world xyz: 3.0 -2.0 1.0 - rpy: -1.57 0.0 1.57 + rpy: 1.57 0.0 -1.57 device_namespace: wireless_charger - type: CAM01 name: camera - parent_link: lights_channel_1_link - xyz: 0.05 0.0 0.0 + parent_link: front_bumper_link + xyz: 0.0 0.0 -0.06 rpy: 0.0 0.0 0.0 device_namespace: camera diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index a82908ed..6295b903 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -35,10 +35,15 @@ pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) add_library(panther_charging_dock SHARED src/panther_charging_dock.cpp) ament_target_dependencies(panther_charging_dock ${PACKAGE_DEPENDENCIES}) + # TODO @delihus how to link the library what is not a name of a package target_link_libraries(panther_charging_dock /opt/ros/humble/lib/libpose_filter.so) +target_include_directories( + panther_charging_dock PUBLIC $ + $) + install(TARGETS panther_charging_dock LIBRARY DESTINATION lib) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) @@ -47,13 +52,19 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock - test/test_panther_charging_dock.cpp) + test/unit/test_panther_charging_dock.cpp) target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock panther_charging_dock) ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock ${PACKAGE_DEPENDENCIES}) + endif() +add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp) +ament_target_dependencies(dock_pose_publisher ${PACKAGE_DEPENDENCIES}) + +install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME}) + ament_export_include_directories(include) ament_export_libraries(panther_charging_dock) diff --git a/panther_docking/README.md b/panther_docking/README.md index 228f1359..5aa076fa 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -13,37 +13,41 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht ## ROS Nodes +- `DockPosePublisherNode`: A node listens to `tf` and republishes position of `dock_pose` in the fixed frame. - `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. -### PantherChargingDock +### DockPosePublisherNode #### Publishes - `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. -- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station. #### Subscribers -- `battery/charging_status` [*panther_msgs/ChargingStatus*]: A charging status of Panther robot. -- `hardware/io_state` [*panther_msgs/IOState*]: States of GPIOs of Panther robot. The `PantherChargingDock` subscribes this topic to check if a robot charges. +- `tf` [*tf2_msgs/TFMessage*]: Tf tree with a detected dock transform. + +#### Parameters -#### Service Clients +- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. +- `.type` [*string*, default: **panther_charging_dock**]: It checks if this dock with name `dock_name` is a type of `panther_charging_dock`. +- `.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose` -- `hardware/charger_enable` [*std_srvs/SetBoot*]: This service client enables charging in a robot. +### PantherChargingDock + +#### Publishes + +- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station. + +#### Subscribers + +- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. #### Parameters -- `~.base_frame` [*string*, default: **base_link**]: A base frame id of a robot. -- `~.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id. -- `~.external_detection_translation_x` [*double*, default: **0.0**]: A translation over an X axis between a detected frame and a dock pose. -- `~.external_detection_translation_y` [*double*, default: **0.0**]: A translation over an Y axis between a detected frame and a dock pose. -- `~.external_detection_translation_x` [*double*, default: **0.0**]: A translation over a Z axis between a detected frame and a dock pose. -- `~.external_detection_rotation_roll` [*double*, default: **0.0**]: A rotation over an X axis between a detected frame and a dock pose. -- `~.external_detection_rotation_pitch` [*double*, default: **0.0**]: A rotation over an Y axis between a detected frame and a dock pose. -- `~.external_detection_rotation_yaw` [*double*, default: **0.0**]: A rotation over a Z axis between a detected frame and a dock pose. -- `~.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations. -- `~.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed. -- `~.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed. -- `~.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. -- `~.staging_yaw_offset` [*double*, default: **0.0**]: A staging pose is defined by offsetting a yaw angle. -- `~.enable_charger_service_call_timeout` [*double*, default: **0.2**]: A timeout for calling enable charging service. A robot is unable to dock if excised. +- `base_frame` [*string*, default: **base_link**]: A base frame id of a robot. +- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. +- `.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id. +- `.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed. +- `.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed. +- `.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. +- `.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations. diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 7d3ecea1..8bc198f1 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -16,33 +16,21 @@ dock_plugins: ["panther_charging_dock"] panther_charging_dock: plugin: panther_docking::PantherChargingDock - base_frame: "/base_link" - docking_distance_threshold: 0.15 - docking_yaw_threshold: 0.15 - staging_x_offset: -0.8 - staging_yaw_offset: 0.0 + external_detection_timeout: 0.1 + docking_distance_threshold: 0.08 + docking_yaw_threshold: 0.1 + staging_x_offset: -0.5 + filter_coef: 0.1 - # TODO: @delihus Try to remove this parameters by using docking station description in the ros_components_description - # Transform between april tag frame and dock pose. An april tag Z+ faces always a camera - external_detection_timeout: 0.3 - external_detection_translation_x: 0.6 - external_detection_translation_y: 0.0 - external_detection_translation_z: 0.0 - external_detection_rotation_roll: 0.0 - external_detection_rotation_pitch: 0.0 - external_detection_rotation_yaw: 3.14 - - enable_charger_service_call_timeout: 1.0 - - docks: ["main_dock"] - main_dock: + docks: ["main"] + main: type: panther_charging_dock - frame: /main_wibotic_station_link + frame: /main_wibotic_receiver_requested_pose_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: k_phi: 1.0 k_delta: 2.0 - v_linear_min: 0.05 - v_linear_max: 0.2 - v_angular_max: 0.3 + v_linear_min: 0.0 + v_linear_max: 0.3 + v_angular_max: 0.15 diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index fdef74de..ca4b37c0 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -139,56 +140,12 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); - /** - * @brief Offset the staging pose. - * - * This method offsets the staging dock pose by values described in a configuration. - * - * @param dock_pose The dock pose to offset. - * - * @return The offset staging pose. - */ - PoseStampedMsg offsetStagingPoseToDockPose(const PoseStampedMsg & dock_pose); + void updateAndPublishStagingPose(); - /** - * @brief Offset the detected dock pose. - * - * This method offsets the detected dock pose by values described in a configuration. - * - * @param detected_dock_pose The detected dock pose to offset. - * - * @return The offset detected dock pose. - */ - PoseStampedMsg offsetDetectedDockPose(const PoseStampedMsg & detected_dock_pose); - - /** - * @brief Get the dock pose in a detection dock frame. - * - * This method retrieves the dock pose in a detection dock frame. - * - * @param frame The detection frame to get the dock pose in. - * @return The dock pose in the detection frame. - */ - PoseStampedMsg getDockPose(const std::string & frame); - - /** - * @brief Method to update the dock pose and publish it. - * - * This method makes all necessary transformations to update the dock pose and publishes it. - * - */ - void updateDockPoseAndPublish(); - - /** - * @brief Update the staging pose and publish it. - * - * This method makes all necessary transformations to update the staging pose and publishes it. - * - * @param frame The frame to publish the staging pose in. - */ - void updateStagingPoseAndPublish(const std::string & frame); + void setDockPose(const PoseStampedMsg::SharedPtr pose); std::string base_frame_name_; + std::string fixed_frame_name_; std::string dock_frame_; rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; @@ -198,16 +155,12 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock tf2_ros::Buffer::SharedPtr tf2_buffer_; rclcpp::Publisher::SharedPtr staging_pose_pub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Subscription::SharedPtr dock_pose_sub_; PoseStampedMsg dock_pose_; PoseStampedMsg staging_pose_; double external_detection_timeout_; - tf2::Quaternion external_detection_rotation_; - double external_detection_translation_x_; - double external_detection_translation_y_; - double external_detection_translation_z_; std::shared_ptr pose_filter_; diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 84fa4b44..44bd8952 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -13,13 +13,11 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import ( - EnvironmentVariable, - LaunchConfiguration, - PathJoinSubstitution, -) +from launch.actions import DeclareLaunchArgument # , IncludeLaunchDescription +from launch.conditions import IfCondition # , UnlessCondition + +# from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from nav2_common.launch import ReplaceString @@ -35,14 +33,6 @@ def generate_launch_description(): description=("Path to docking server configuration file."), ) - namespace = LaunchConfiguration("namespace") - declare_namespace_arg = DeclareLaunchArgument( - "namespace", - default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), - description="Add namespace to all launched nodes.", - ) - - use_docking = LaunchConfiguration("use_docking") declare_use_docking_arg = DeclareLaunchArgument( "use_docking", default_value="True", @@ -50,12 +40,16 @@ def generate_launch_description(): choices=["True", "False", "true", "false"], ) + namespace = LaunchConfiguration("namespace") + use_docking = LaunchConfiguration("use_docking") 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"], + + log_level = LaunchConfiguration("log_level") + declare_log_level = DeclareLaunchArgument( + "log_level", + default_value="info", + description="Logging level", + choices=["debug", "info", "warning", "error"], ) namespaced_docking_server_config = ReplaceString( @@ -71,6 +65,7 @@ def generate_launch_description(): namespaced_docking_server_config, {"use_sim_time": use_sim}, ], + arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], remappings=[("~/transition_event", "~/_transition_event")], emulate_tty=True, condition=IfCondition(use_docking), @@ -93,13 +88,43 @@ def generate_launch_description(): condition=IfCondition(use_docking), ) + dock_pose_publisher = Node( + package="panther_docking", + executable="dock_pose_publisher", + parameters=[ + namespaced_docking_server_config, + {"use_sim_time": use_sim}, + ], + name="dock_pose_publisher", + namespace=namespace, + emulate_tty=True, + arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], + condition=IfCondition(use_docking), + ) + + # FIXME: This launch does not work with the simulation. It can be caused by different versions of opencv + # station_launch = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # PathJoinSubstitution( + # [ + # FindPackageShare("panther_docking"), + # "launch", + # "station.launch.py", + # ] + # ), + # ), + # launch_arguments={"namespace": namespace}.items(), + # condition=UnlessCondition(use_sim), + # ) + return LaunchDescription( [ - declare_docking_server_config_path_arg, - declare_namespace_arg, declare_use_docking_arg, - declare_use_sim_arg, + declare_docking_server_config_path_arg, + declare_log_level, + # station_launch, docking_server_node, docking_server_activate_node, + dock_pose_publisher, ] ) diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py index f1303cdf..f25dc5b9 100644 --- a/panther_docking/launch/station.launch.py +++ b/panther_docking/launch/station.launch.py @@ -17,6 +17,7 @@ import imageio from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition from launch.substitutions import ( Command, EnvironmentVariable, @@ -33,7 +34,7 @@ def generate_apriltag_and_get_path(tag_id): tag_generator = TagGenerator2("tag36h11") - tag_image = tag_generator.generate(tag_id, scale=100) + tag_image = tag_generator.generate(tag_id, scale=1000) path = f"/tmp/tag_{tag_id}.png" @@ -45,6 +46,7 @@ def launch_setup(context, *args, **kwargs): namespace = LaunchConfiguration("namespace").perform(context) apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context)) apriltag_size = LaunchConfiguration("apriltag_size").perform(context) + use_docking = LaunchConfiguration("use_docking").perform(context) apriltag_image_path = generate_apriltag_and_get_path(apriltag_id) @@ -85,12 +87,20 @@ def launch_setup(context, *args, **kwargs): remappings=[("robot_description", "station_description")], namespace=namespace, emulate_tty=True, + condition=IfCondition(use_docking), ) return [station_state_pub_node] def generate_launch_description(): + declare_use_docking_arg = DeclareLaunchArgument( + "use_docking", + default_value="True", + description="Enable docking server.", + choices=["True", "False", "true", "false"], + ) + declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), @@ -99,18 +109,19 @@ def generate_launch_description(): declare_apriltag_id = DeclareLaunchArgument( "apriltag_id", - default_value="1", + default_value="0", description="ID of a generated apriltag on the station", ) declare_apriltag_size = DeclareLaunchArgument( "apriltag_size", - default_value="0.15", + default_value="0.06", description="Size in meters of a generated apriltag on the station", ) return LaunchDescription( [ + declare_use_docking_arg, declare_namespace_arg, declare_apriltag_id, declare_apriltag_size, diff --git a/panther_docking/package.xml b/panther_docking/package.xml index edee3f62..f665b0c6 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -28,6 +28,7 @@ nav2_lifecycle_manager python3-imageio + xacro ament_cmake ament_cmake_gtest diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp new file mode 100644 index 00000000..2784ebf0 --- /dev/null +++ b/panther_docking/src/dock_pose_publisher_node.cpp @@ -0,0 +1,103 @@ +// Copyright (c) 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 + +class DockPosePublisherNode : public rclcpp::Node +{ +public: + DockPosePublisherNode() : Node("pose_publisher_node") + { + declare_parameter("publish_rate", 10.0); + declare_parameter("docks", std::vector({"main"})); + declare_parameter("fixed_frame", "odom"); + + const auto fixed_frame = get_parameter("fixed_frame").as_string(); + const auto docks = get_parameter("docks").as_string_array(); + + std::string dock_pose_frame_id; + for (const auto & dock : docks) { + declare_parameter(dock + ".type", "panther_charging_dock"); + declare_parameter(dock + ".frame", "main_wibotic_receiver_requested_pose_link"); + + const auto dock_type = get_parameter(dock + ".type").as_string(); + if (dock_type == "panther_charging_dock") { + dock_pose_frame_id = get_parameter(dock + ".frame").as_string(); + } + } + + const auto publish_rate = get_parameter("publish_rate").as_double(); + const auto publish_period = std::chrono::duration(1.0 / publish_rate); + + source_frame_ = dock_pose_frame_id; + target_frame_ = fixed_frame; + + pose_publisher_ = this->create_publisher( + "docking/dock_pose", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + timer_ = this->create_wall_timer( + publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); + } + +private: + void publishPose() + { + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform( + target_frame_, source_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_DEBUG(this->get_logger(), "Could not get transform: %s", ex.what()); + return; + } + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = this->now(); + pose_msg.header.frame_id = target_frame_; + pose_msg.pose.position.x = transform_stamped.transform.translation.x; + pose_msg.pose.position.y = transform_stamped.transform.translation.y; + pose_msg.pose.position.z = 0.0; + pose_msg.pose.orientation = transform_stamped.transform.rotation; + + pose_publisher_->publish(pose_msg); + } + + std::string target_frame_; + std::string source_frame_; + rclcpp::Publisher::SharedPtr pose_publisher_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index dc0bf779..f6bc0c8e 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -51,41 +51,34 @@ void PantherChargingDock::configure( void PantherChargingDock::cleanup() { - dock_pose_pub_.reset(); + dock_pose_sub_.reset(); staging_pose_pub_.reset(); } void PantherChargingDock::activate() { auto node = node_.lock(); - dock_pose_pub_ = node->create_publisher("docking/dock_pose", 1); + dock_pose_sub_ = node->create_subscription( + "docking/dock_pose", 1, + std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1)); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); } void PantherChargingDock::deactivate() { - dock_pose_pub_.reset(); + dock_pose_sub_.reset(); staging_pose_pub_.reset(); } void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { nav2_util::declare_parameter_if_not_declared( - node, name_ + ".base_frame", rclcpp::ParameterValue("base_link")); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_timeout", rclcpp::ParameterValue(0.2)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_translation_x", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_translation_y", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_translation_z", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_rotation_yaw", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_rotation_pitch", rclcpp::ParameterValue(0.0)); + node, "base_frame", rclcpp::ParameterValue("base_link")); + + nav2_util::declare_parameter_if_not_declared(node, "fixed_frame", rclcpp::ParameterValue("odom")); + nav2_util::declare_parameter_if_not_declared( - node, name_ + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); + node, name_ + ".external_detection_timeout", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( node, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); @@ -94,8 +87,6 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod nav2_util::declare_parameter_if_not_declared( node, name_ + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( node, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); @@ -103,78 +94,78 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { - node->get_parameter(name_ + ".base_frame", base_frame_name_); + node->get_parameter("base_frame", base_frame_name_); + node->get_parameter("fixed_frame", fixed_frame_name_); + node->get_parameter(name_ + ".external_detection_timeout", external_detection_timeout_); - node->get_parameter( - name_ + ".external_detection_translation_x", external_detection_translation_x_); - node->get_parameter( - name_ + ".external_detection_translation_y", external_detection_translation_y_); - node->get_parameter( - name_ + ".external_detection_translation_z", external_detection_translation_z_); - - double yaw, pitch, roll; - node->get_parameter(name_ + ".external_detection_rotation_yaw", yaw); - node->get_parameter(name_ + ".external_detection_rotation_pitch", pitch); - node->get_parameter(name_ + ".external_detection_rotation_roll", roll); - - external_detection_rotation_.setRPY(roll, pitch, yaw); node->get_parameter(name_ + ".docking_distance_threshold", docking_distance_threshold_); node->get_parameter(name_ + ".docking_yaw_threshold", docking_yaw_threshold_); node->get_parameter(name_ + ".staging_x_offset", staging_x_offset_); - node->get_parameter(name_ + ".staging_yaw_offset", staging_yaw_offset_); node->get_parameter(name_ + ".filter_coef", pose_filter_coef_); } +// When there is no pose actual position of robot is a staging pose PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { - std::string stage_frame = frame; - // The pose in the argument is a pose defined in yaml file or passed in docking action. - // In our approach to docking we use local "base_link" frame as a reference frame. - // The pose is empty when the docking action is called from the action server. - // When a robot is undocking it passes odometry frame as a reference frame. - // It needed to be split to two cases to handle both situations. + RCLCPP_DEBUG_STREAM(logger_, "Getting staging pose in frame: " << frame); + + // When there is no global pose to reach thanks to nav2 if (pose == geometry_msgs::msg::Pose()) { - dock_frame_ = frame; - stage_frame = base_frame_name_; - } + if (dock_pose_.header.frame_id.empty()) { + throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); + } - if (dock_frame_.empty()) { - throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); + updateAndPublishStagingPose(); } - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(stage_frame); return staging_pose_; } bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) { - try { - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(base_frame_name_); - pose = dock_pose_; - } catch (const opennav_docking_core::DockingException & e) { - RCLCPP_ERROR_STREAM(logger_, "An occurred error while getting refined pose: " << e.what()); + RCLCPP_DEBUG(logger_, "Getting refined pose"); + rclcpp::Time request_detection_time; + + if (dock_pose_.header.frame_id.empty()) { + throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); + } + + { + auto node = node_.lock(); + request_detection_time = node->now(); + } + + auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); + auto duration = rclcpp::Time(request_detection_time) - rclcpp::Time(dock_pose_.header.stamp); + if (duration > timeout) { + RCLCPP_WARN_STREAM( + logger_, "Lost detection or did not detect: timeout exceeded: " << duration.seconds()); return false; } + pose = dock_pose_; + updateAndPublishStagingPose(); + return true; } bool PantherChargingDock::isDocked() { - updateDockPoseAndPublish(); + RCLCPP_DEBUG(logger_, "Checking if docked"); + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = base_frame_name_; + + robot_pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, robot_pose, fixed_frame_name_); - geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = dock_pose_.header.frame_id; return panther_utils::tf2_utils::ArePosesNear( - origin, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); + robot_pose, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); } bool PantherChargingDock::isCharging() { + RCLCPP_DEBUG(logger_, "Checking if charging"); try { return isDocked(); } catch (const opennav_docking_core::FailedToDetectDock & e) { @@ -186,80 +177,20 @@ bool PantherChargingDock::disableCharging() { return true; } bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } -PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDockPose( - const PoseStampedMsg & dock_pose) +void PantherChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) { - tf2::Transform staging_offset_transform; - staging_offset_transform.setOrigin(tf2::Vector3(staging_x_offset_, 0.0, 0.0)); - - tf2::Quaternion staging_yaw_rotation; - staging_yaw_rotation.setRPY(0, 0, staging_yaw_offset_); - staging_offset_transform.setRotation(staging_yaw_rotation); - - return panther_utils::tf2_utils::OffsetPose(dock_pose, staging_offset_transform); -} - -PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetDetectedDockPose( - const PoseStampedMsg & detected_dock_pose) -{ - tf2::Transform offset; - offset.setOrigin(tf2::Vector3( - external_detection_translation_x_, external_detection_translation_y_, - external_detection_translation_z_)); - offset.setRotation(external_detection_rotation_); - - return panther_utils::tf2_utils::OffsetPose(detected_dock_pose, offset); + auto filtered_pose = pose_filter_->update(*pose); + dock_pose_ = filtered_pose; } -PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std::string & frame) +void PantherChargingDock::updateAndPublishStagingPose() { - PoseStampedMsg filtered_offset_detected_dock_pose; - try { - PoseStampedMsg pose; - pose.header.frame_id = frame; - { - auto node = node_.lock(); - pose.header.stamp = node->get_clock()->now(); - } - auto offset_detected_dock_pose = offsetDetectedDockPose(pose); - - filtered_offset_detected_dock_pose = pose_filter_->update(offset_detected_dock_pose); - filtered_offset_detected_dock_pose = panther_utils::tf2_utils::TransformPose( - tf2_buffer_, filtered_offset_detected_dock_pose, base_frame_name_, - external_detection_timeout_); - - filtered_offset_detected_dock_pose.pose.position.z = 0.0; - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "An exception occurred while getting dock pose: " + std::string(e.what())); - } + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + staging_pose_ = dock_pose_; + staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose_.pose.position.y += sin(yaw) * staging_x_offset_; - return filtered_offset_detected_dock_pose; -} - -void PantherChargingDock::updateDockPoseAndPublish() -{ - try { - dock_pose_ = getDockPose(dock_frame_); - dock_pose_pub_->publish(dock_pose_); - } catch (const std::runtime_error & e) { - throw opennav_docking_core::FailedToDetectDock( - "An exception occurred while updating dock pose: " + std::string(e.what())); - } -} - -void PantherChargingDock::updateStagingPoseAndPublish(const std::string & frame) -{ - try { - auto new_staging_pose = offsetStagingPoseToDockPose(dock_pose_); - staging_pose_ = panther_utils::tf2_utils::TransformPose( - tf2_buffer_, new_staging_pose, frame, external_detection_timeout_); - dock_pose_.pose.position.z = 0.0; - staging_pose_pub_->publish(staging_pose_); - } catch (const std::runtime_error & e) { - throw opennav_docking_core::FailedToStage( - "An exception occurred while transforming staging pose: " + std::string(e.what())); - } + staging_pose_pub_->publish(staging_pose_); } } // namespace panther_docking diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp deleted file mode 100644 index 71656ee3..00000000 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ /dev/null @@ -1,418 +0,0 @@ -// Copyright (c) 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 -#include -#include -#include - -#include "panther_docking/panther_charging_dock.hpp" -#include "panther_utils/test/ros_test_utils.hpp" -#include "panther_utils/tf2_utils.hpp" - -static constexpr char kBaseFrame[] = "base_link"; -static constexpr char kDockFrame[] = "test_dock"; -static constexpr char kOdomFrame[] = "odom"; - -class PantherChargingDockWrapper : public panther_docking::PantherChargingDock -{ -public: - using SharedPtr = std::shared_ptr; - using UniquePtr = std::unique_ptr; - PantherChargingDockWrapper() : panther_docking::PantherChargingDock() {} - - geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose( - const geometry_msgs::msg::PoseStamped & dock_pose) - { - return panther_docking::PantherChargingDock::offsetStagingPoseToDockPose(dock_pose); - } - - geometry_msgs::msg::PoseStamped offsetDetectedDockPose( - const geometry_msgs::msg::PoseStamped & detected_dock_pose) - { - return panther_docking::PantherChargingDock::offsetDetectedDockPose(detected_dock_pose); - } - - geometry_msgs::msg::PoseStamped getDockPose(const std::string & frame) - { - return panther_docking::PantherChargingDock::getDockPose(frame); - } - void updateDockPoseAndPublish() - { - panther_docking::PantherChargingDock::updateDockPoseAndPublish(); - } - - void updateStagingPoseAndPublish(const std::string & frame) - { - panther_docking::PantherChargingDock::updateStagingPoseAndPublish(frame); - } -}; - -class TestPantherChargingDock : public testing::Test -{ -public: - TestPantherChargingDock(); - ~TestPantherChargingDock(); - -protected: - void ConfigureAndActivateDock(); - void SetTransform( - const std::string & frame_id, const std::string & child_frame_id, - const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform); - void SetDetectionOffsets(); - void SetStagingOffsets(); - - rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - tf2_ros::Buffer::SharedPtr tf2_buffer_; - - PantherChargingDockWrapper::UniquePtr charging_dock_; - - rclcpp::Subscription::SharedPtr dock_pose_sub_; - rclcpp::Subscription::SharedPtr staging_pose_sub_; - - geometry_msgs::msg::PoseStamped::SharedPtr dock_pose_; - geometry_msgs::msg::PoseStamped::SharedPtr staging_pose_; - - geometry_msgs::msg::Transform base_link_to_odom_transform_; - geometry_msgs::msg::Transform dock_to_base_transform_; - geometry_msgs::msg::Transform dock_to_base_at_external_detection_transform_; - - bool charging_status_; - - double external_detection_translation_x_ = 0.3; - double external_detection_translation_y_ = 0.1; - double external_detection_translation_z_ = 0.0; - double external_detection_roll_ = 0.0; - double external_detection_pitch_ = 0.0; - double external_detection_yaw_ = 1.57; - double staging_x_offset_ = 0.5; - double staging_yaw_offset_ = 1.57; -}; - -TestPantherChargingDock::TestPantherChargingDock() -{ - node_ = std::make_shared("panther_charging_dock_test"); - - tf2_buffer_ = std::make_shared(node_->get_clock()); - - dock_pose_sub_ = node_->create_subscription( - "docking/dock_pose", 10, - [&](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { dock_pose_ = msg; }); - - staging_pose_sub_ = node_->create_subscription( - "docking/staging_pose", 10, - [&](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { staging_pose_ = msg; }); - - // Silence error about dedicated thread's being necessary - tf2_buffer_->setUsingDedicatedThread(true); - - charging_dock_ = std::make_unique(); - - tf2::Quaternion rotation; - base_link_to_odom_transform_.translation.x = 0.3; - base_link_to_odom_transform_.translation.y = 0.2; - base_link_to_odom_transform_.translation.z = 0.1; - base_link_to_odom_transform_.rotation.x = 0.0; - base_link_to_odom_transform_.rotation.y = 0.0; - base_link_to_odom_transform_.rotation.z = 0.0; - base_link_to_odom_transform_.rotation.w = 1.0; - - dock_to_base_transform_.translation.x = 1.0; - dock_to_base_transform_.translation.y = 2.0; - dock_to_base_transform_.translation.z = 3.0; - rotation.setRPY(0.0, 0.0, 1.57); - dock_to_base_transform_.rotation = tf2::toMsg(rotation); - - dock_to_base_at_external_detection_transform_.translation.x = -external_detection_translation_y_; - dock_to_base_at_external_detection_transform_.translation.y = external_detection_translation_x_; - dock_to_base_at_external_detection_transform_.translation.z = external_detection_translation_z_; - rotation.setRPY(0.0, 0.0, -external_detection_yaw_); - dock_to_base_at_external_detection_transform_.rotation = tf2::toMsg(rotation); -} - -TestPantherChargingDock::~TestPantherChargingDock() -{ - if (charging_dock_) { - charging_dock_->deactivate(); - charging_dock_->cleanup(); - } - - if (executor_) { - executor_->cancel(); - } -} - -void TestPantherChargingDock::ConfigureAndActivateDock() -{ - charging_dock_->configure(node_, kDockFrame, tf2_buffer_); - charging_dock_->activate(); -} - -void TestPantherChargingDock::SetTransform( - const std::string & frame_id, const std::string & child_frame_id, - const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform) -{ - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.header.stamp = stamp; - transform_stamped.header.frame_id = frame_id; - transform_stamped.child_frame_id = child_frame_id; - transform_stamped.transform = transform; - - tf2_buffer_->setTransform(transform_stamped, "unittest", true); -} - -void TestPantherChargingDock::SetDetectionOffsets() -{ - node_->declare_parameter( - "test_dock.external_detection_translation_x", - rclcpp::ParameterValue(external_detection_translation_x_)); - node_->declare_parameter( - "test_dock.external_detection_translation_y", - rclcpp::ParameterValue(external_detection_translation_y_)); - node_->declare_parameter( - "test_dock.external_detection_translation_z", - rclcpp::ParameterValue(external_detection_translation_z_)); - node_->declare_parameter( - "test_dock.external_detection_rotation_roll", rclcpp::ParameterValue(external_detection_roll_)); - node_->declare_parameter( - "test_dock.external_detection_rotation_pitch", - rclcpp::ParameterValue(external_detection_pitch_)); - node_->declare_parameter( - "test_dock.external_detection_rotation_yaw", rclcpp::ParameterValue(external_detection_yaw_)); -} - -void TestPantherChargingDock::SetStagingOffsets() -{ - node_->declare_parameter("test_dock.staging_x_offset", rclcpp::ParameterValue(staging_x_offset_)); - node_->declare_parameter( - "test_dock.staging_yaw_offset", rclcpp::ParameterValue(staging_yaw_offset_)); -} - -TEST_F(TestPantherChargingDock, OffsetStagingPoseToDockPose) -{ - SetStagingOffsets(); - ConfigureAndActivateDock(); - - geometry_msgs::msg::PoseStamped pose; - pose = charging_dock_->offsetStagingPoseToDockPose(pose); - EXPECT_NEAR(pose.pose.position.x, staging_x_offset_, 0.01); - EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), staging_yaw_offset_, 0.01); -} - -TEST_F(TestPantherChargingDock, OffsetDetectedDockPose) -{ - SetDetectionOffsets(); - ConfigureAndActivateDock(); - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = kDockFrame; - pose = charging_dock_->offsetDetectedDockPose(pose); - - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header = pose.header; - expected_pose.pose.position.x = external_detection_translation_x_; - expected_pose.pose.position.y = external_detection_translation_y_; - expected_pose.pose.position.z = external_detection_translation_z_; - tf2::Quaternion expected_rotation; - expected_rotation.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, GetDockPose) -{ - SetDetectionOffsets(); - - ConfigureAndActivateDock(); - - EXPECT_THROW({ charging_dock_->getDockPose("wrong_dock_pose"); }, std::runtime_error); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - - geometry_msgs::msg::PoseStamped pose; - ASSERT_NO_THROW({ pose = charging_dock_->getDockPose(kDockFrame); };); - - ASSERT_EQ(pose.header.frame_id, kBaseFrame); - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header.frame_id = pose.header.frame_id; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_; - expected_pose.pose.position.z = 0.0; - tf2::Quaternion expected_rotation; - // 1.57 is from transformation from base_link to test_dock - expected_rotation.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); - - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) -{ - SetDetectionOffsets(); - SetStagingOffsets(); - - ConfigureAndActivateDock(); - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - SetTransform(kOdomFrame, kBaseFrame, node_->now(), base_link_to_odom_transform_); - - ASSERT_NO_THROW({ - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - charging_dock_->updateDockPoseAndPublish(); - charging_dock_->updateStagingPoseAndPublish(kBaseFrame); - }); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, dock_pose_, std::chrono::milliseconds(100))); - - ASSERT_EQ(dock_pose_->header.frame_id, kBaseFrame); - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header = dock_pose_->header; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_; - expected_pose.pose.position.z = 0.0; - tf2::Quaternion expected_rotation; - expected_rotation.setRPY(0.0, 0.0, external_detection_yaw_ + 1.57); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(*dock_pose_, expected_pose, 0.01, 0.01)); - - ASSERT_EQ(staging_pose_->header.frame_id, kBaseFrame); - expected_pose.header = staging_pose_->header; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_ - staging_x_offset_; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_; - expected_pose.pose.position.z = 0.0; - // 1.57 is from transformation from base_link to test_dock - expected_rotation.setRPY(0.0, 0.0, -1.57); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(*staging_pose_, expected_pose, 0.01, 0.01)); - - ASSERT_NO_THROW({ charging_dock_->updateStagingPoseAndPublish(kOdomFrame); }); - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); - - ASSERT_EQ(staging_pose_->header.frame_id, kOdomFrame); - expected_pose.header = staging_pose_->header; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_ - staging_x_offset_ + 0.3; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_ + 0.2; - expected_pose.pose.position.z = 0.1; - - // It is 4.71 but tf2::getYaw moves by 2pi what is 6.28 - expected_rotation.setRPY(0.0, 0.0, -1.57); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(*staging_pose_, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, GetStagingPose) -{ - SetStagingOffsets(); - ConfigureAndActivateDock(); - - geometry_msgs::msg::PoseStamped pose; - ASSERT_THROW( - { charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); }, - opennav_docking_core::FailedToDetectDock); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - ASSERT_NO_THROW( - { pose = charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); }); - ASSERT_EQ(pose.header.frame_id, kBaseFrame); - - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header.frame_id = pose.header.frame_id; - expected_pose.pose.position.x = 1.0; - expected_pose.pose.position.y = 2.0 + staging_x_offset_; - expected_pose.pose.position.z = 0.0; - - tf2::Quaternion expected_rotation; - expected_rotation.setRPY(0.0, 0.0, 1.57 + staging_yaw_offset_); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, GetRefinedPose) -{ - SetDetectionOffsets(); - ConfigureAndActivateDock(); - - geometry_msgs::msg::PoseStamped pose; - ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - - ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); - - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - ASSERT_TRUE(charging_dock_->getRefinedPose(pose)); - - ASSERT_EQ(pose.header.frame_id, kBaseFrame); - tf2::Quaternion external_detection_rotation_; - // 1.57 is from transformation from base_link to test_dock - external_detection_rotation_.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); - geometry_msgs::msg::PoseStamped refined_pose; - refined_pose.header = pose.header; - refined_pose.pose.position.x = 1.0 - external_detection_translation_y_; - refined_pose.pose.position.y = 2.0 + external_detection_translation_x_; - refined_pose.pose.position.z = 0.0; - refined_pose.pose.orientation = tf2::toMsg(external_detection_rotation_); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, refined_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, IsDocked) -{ - SetDetectionOffsets(); - ConfigureAndActivateDock(); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - SetTransform(kOdomFrame, kBaseFrame, node_->now(), base_link_to_odom_transform_); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - ASSERT_FALSE(charging_dock_->isDocked()); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_at_external_detection_transform_); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - - EXPECT_TRUE(charging_dock_->isDocked()); -} - -TEST_F(TestPantherChargingDock, FailedConfigureCannotLockNode) -{ - rclcpp_lifecycle::LifecycleNode::SharedPtr node; - ASSERT_THROW({ charging_dock_->configure(node, kDockFrame, tf2_buffer_); }, std::runtime_error); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - - auto run_tests = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - return run_tests; -} diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp new file mode 100644 index 00000000..435e7bdb --- /dev/null +++ b/panther_docking/test/unit/test_panther_charging_dock.cpp @@ -0,0 +1,202 @@ +// Copyright (c) 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 + +static constexpr char kBaseFrame[] = "base_link"; +static constexpr char kOdomFrame[] = "odom"; + +class PantherChargingDockWrapper : public panther_docking::PantherChargingDock +{ +public: + void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) + { + panther_docking::PantherChargingDock::setDockPose(pose); + } +}; + +class TestPantherChargingDock : public ::testing::Test +{ +protected: + TestPantherChargingDock(); + void SetTransform( + const std::string & frame_id, const std::string & child_frame_id, + const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr dock_; + rclcpp::Publisher::SharedPtr dock_pose_pub; + tf2_ros::Buffer::SharedPtr tf_buffer_; +}; + +TestPantherChargingDock::TestPantherChargingDock() +{ + node_ = std::make_shared("test_node"); + tf_buffer_ = std::make_shared(node_->get_clock()); + + // Silence error about dedicated thread's being necessary + tf_buffer_->setUsingDedicatedThread(true); + + dock_ = std::make_shared(); + dock_pose_pub = node_->create_publisher("dock_pose", 10); + + node_->configure(); + node_->activate(); +} + +void TestPantherChargingDock::SetTransform( + const std::string & frame_id, const std::string & child_frame_id, + const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = stamp; + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform = transform; + + tf_buffer_->setTransform(transform_stamped, "unittest", true); +} + +TEST_F(TestPantherChargingDock, FailConfigureNoNode) +{ + node_.reset(); + ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, FailConfigureNoTfBuffer) +{ + tf_buffer_.reset(); + ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, GetStagingPoseLocal) +{ + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); + + geometry_msgs::msg::PoseStamped::SharedPtr dock_pose = + std::make_shared(); + dock_pose->pose.position.x = 1.0; + dock_pose->pose.position.y = 1.0; + dock_pose->pose.position.z = 0.0; + dock_pose->pose.orientation.w = 1.0; + + dock_->setDockPose(dock_pose); + geometry_msgs::msg::PoseStamped pose; + + geometry_msgs::msg::PoseStamped staging_pose; + ASSERT_THROW( + { staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); }, + opennav_docking_core::FailedToDetectDock); + + dock_pose->header.frame_id = kOdomFrame; + dock_->setDockPose(dock_pose); + + staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); + + ASSERT_FLOAT_EQ(staging_pose.pose.position.x, 0.3); + ASSERT_FLOAT_EQ(staging_pose.pose.position.y, 1.0); + ASSERT_FLOAT_EQ(staging_pose.pose.position.z, 0.0); +} + +// TODO: fill after nav2 tests +// TEST_F(TestPantherChargingDock, GetStagingPoseGlobal){ +// } + +TEST_F(TestPantherChargingDock, GetRefinedPose) +{ + node_->declare_parameter("dock.external_detection_timeout", 0.5); + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); + + geometry_msgs::msg::PoseStamped::SharedPtr dock_pose = + std::make_shared(); + dock_pose->pose.position.x = 1.0; + dock_pose->pose.position.y = 1.0; + dock_pose->pose.position.z = 0.0; + dock_pose->pose.orientation.w = 1.0; + + dock_->setDockPose(dock_pose); + + geometry_msgs::msg::PoseStamped pose; + + ASSERT_THROW({ dock_->getRefinedPose(pose); }, opennav_docking_core::FailedToDetectDock); + + dock_pose->header.frame_id = kOdomFrame; + dock_->setDockPose(dock_pose); + ASSERT_FALSE(dock_->getRefinedPose(pose)); + + dock_pose->header.stamp = node_->now(); + dock_->setDockPose(dock_pose); + ASSERT_TRUE(dock_->getRefinedPose(pose)); + + ASSERT_FLOAT_EQ(pose.pose.position.x, 1.0); + ASSERT_FLOAT_EQ(pose.pose.position.y, 1.0); + ASSERT_FLOAT_EQ(pose.pose.position.z, 0.0); +} + +TEST_F(TestPantherChargingDock, IsDocked) +{ + node_->declare_parameter("dock.external_detection_timeout", 0.5); + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); + + auto transform = geometry_msgs::msg::Transform(); + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + + SetTransform(kOdomFrame, kBaseFrame, node_->now(), transform); + geometry_msgs::msg::PoseStamped::SharedPtr dock_pose = + std::make_shared(); + dock_pose->header.frame_id = kOdomFrame; + dock_pose->header.stamp = node_->now(); + dock_pose->pose.position.x = transform.translation.x - 0.1; + dock_pose->pose.position.y = transform.translation.y; + dock_pose->pose.position.z = transform.translation.z; + dock_->setDockPose(dock_pose); + + ASSERT_FALSE(dock_->isDocked()); + + dock_pose->pose.position.x = transform.translation.x; + dock_pose->pose.position.y = transform.translation.y; + dock_pose->pose.position.z = transform.translation.z; + // Set dock pose 10 times to ensure that filter stabilize the pose + for (std::size_t i = 0; i < 10; i++) { + dock_->setDockPose(dock_pose); + } + + ASSERT_TRUE(dock_->isDocked()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + + auto run_tests = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return run_tests; +} diff --git a/panther_gazebo/config/apriltag.yaml b/panther_gazebo/config/apriltag.yaml new file mode 100644 index 00000000..920c6f99 --- /dev/null +++ b/panther_gazebo/config/apriltag.yaml @@ -0,0 +1,23 @@ +--- +/**: + ros__parameters: + image_transport: raw + family: 36h11 + size: 0.06 + profile: false + + + max_hamming: 0 + detector: + threads: 1 + decimate: 2.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + + + tag: + ids: [0] + frames: [/main_apriltag_link] + sizes: [0.06] diff --git a/panther_gazebo/launch/apriltag.launch.py b/panther_gazebo/launch/apriltag.launch.py new file mode 100644 index 00000000..deeaa24e --- /dev/null +++ b/panther_gazebo/launch/apriltag.launch.py @@ -0,0 +1,62 @@ +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + use_docking = LaunchConfiguration("use_docking") + use_sim = LaunchConfiguration("use_sim") + namespace = LaunchConfiguration("namespace") + + apriltag_config_path = LaunchConfiguration("apriltag_config_path") + apriltag_config_path_arg = DeclareLaunchArgument( + "apriltag_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_gazebo"), "config", "apriltag.yaml"] + ), + description=("Path to apriltag configuration file."), + ) + + namespaced_apriltag_config_path = ReplaceString( + source_file=apriltag_config_path, + replacements={"": namespace, "//": "/"}, + ) + + apriltag_node = Node( + package="apriltag_ros", + executable="apriltag_node", + parameters=[{"use_sim_time": use_sim}, namespaced_apriltag_config_path], + namespace=namespace, + emulate_tty=True, + remappings={ + "camera_info": "camera/color/camera_info", + "image_rect": "camera/color/image_raw", + "detections": "docking/april_tags", + }.items(), + condition=IfCondition(use_docking), + ) + + return LaunchDescription( + [ + apriltag_config_path_arg, + apriltag_node, + ] + ) diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index aa55478a..8f7fa280 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -198,6 +198,19 @@ def generate_launch_description(): launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), ) + apriltag_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("panther_gazebo"), + "launch", + "apriltag.launch.py", + ] + ), + ), + launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), + ) + return LaunchDescription( [ declare_battery_config_path_arg, @@ -214,5 +227,6 @@ def generate_launch_description(): simulate_components, gz_bridge, docking_launch, + apriltag_launch, ] ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 184e07c1..ecadf84c 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -27,6 +27,7 @@ std_msgs std_srvs + apriltag_ros controller_manager husarion_gz_worlds launch diff --git a/panther_manager/behavior_trees/docking.xml b/panther_manager/behavior_trees/docking.xml index 9d3b46bc..042e221b 100644 --- a/panther_manager/behavior_trees/docking.xml +++ b/panther_manager/behavior_trees/docking.xml @@ -11,7 +11,7 @@ buttons="0;0;0;1;1;1;0;0;0;0;0;0"/>