From 25da047db3313f70b0d19382d86c27155ba75b55 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 25 Jul 2024 18:18:41 +0200 Subject: [PATCH 01/23] Namespace refactor --- panther_description/urdf/gazebo.urdf.xacro | 14 ++------------ panther_description/urdf/panther_macro.urdf.xacro | 7 +------ 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index b8b2afdbb..4d315fd29 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -6,12 +6,7 @@ - - - - - - + @@ -65,12 +60,7 @@ - - - - - - + diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index 7ff4192e2..01c2764bf 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -16,12 +16,7 @@ imu_rot_y:=0.0 namespace:=''"> - - - - - - + From 05eb4839a20a14c77515a57e0e87147eb44cb4c6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 15 Aug 2024 00:08:41 +0000 Subject: [PATCH 02/23] added half tested panther_docking package Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 55 +++ panther_docking/README.md | 3 + .../config/panther_docking_server.yaml | 49 ++ .../panther_docking/panther_charging_dock.hpp | 252 ++++++++++ .../launch/apriltag_linux_cam.launch.py | 75 +++ panther_docking/launch/docking.launch.py | 91 ++++ panther_docking/package.xml | 36 ++ panther_docking/plugin.xml | 8 + panther_docking/src/panther_charging_dock.cpp | 451 ++++++++++++++++++ .../test/test_panther_charging_dock.cpp | 448 +++++++++++++++++ .../panther_utils/common_utilities.hpp | 14 + 11 files changed, 1482 insertions(+) create mode 100644 panther_docking/CMakeLists.txt create mode 100644 panther_docking/README.md create mode 100644 panther_docking/config/panther_docking_server.yaml create mode 100644 panther_docking/include/panther_docking/panther_charging_dock.hpp create mode 100644 panther_docking/launch/apriltag_linux_cam.launch.py create mode 100644 panther_docking/launch/docking.launch.py create mode 100644 panther_docking/package.xml create mode 100644 panther_docking/plugin.xml create mode 100644 panther_docking/src/panther_charging_dock.cpp create mode 100644 panther_docking/test/test_panther_charging_dock.cpp diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt new file mode 100644 index 000000000..d2d860e30 --- /dev/null +++ b/panther_docking/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(panther_docking) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(PACKAGE_DEPENDENCIES + ament_cmake + rclcpp + panther_msgs + geometry_msgs + sensor_msgs + std_srvs + realtime_tools + opennav_docking_core + opennav_docking + pluginlib + tf2_ros + panther_utils) + +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +include_directories(include) + +set(library_name ${PROJECT_NAME}_battery_state_dock) + +add_library(${library_name} SHARED src/panther_charging_dock.cpp) + +# TODO @delihus how to link the library what is not a name of a package +target_link_libraries(${library_name} /opt/ros/humble/lib/libpose_filter.so) + +target_include_directories(${library_name} + PUBLIC ${CMAKE_INSTALL_PREFIX}/include) + +pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) + + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock test/test_panther_charging_dock.cpp) + target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock ${library_name} ) + ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock ${PACKAGE_DEPENDENCIES}) +endif() + +install(DIRECTORY include/ DESTINATION include/) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_libraries(panther_charging_dock ${library_name}) +ament_target_dependencies(${library_name} ${PACKAGE_DEPENDENCIES}) +ament_package() diff --git a/panther_docking/README.md b/panther_docking/README.md new file mode 100644 index 000000000..88583c682 --- /dev/null +++ b/panther_docking/README.md @@ -0,0 +1,3 @@ +ros2 action send_goal /panther/dock_robot opennav_docking_msgs/action/DockRobot "{dock_id: main_dock, navigate_to_staging_pose: false}" +ros2 launch panther_bringup bringup.launch.py namespace:=panther components_config_path:=$(pwd)/gazebo_components.yaml +PANTHER_ROBOT_VERSION=1.21 ros2 launch panther_docking docking.launch.py namespace:=panther diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml new file mode 100644 index 000000000..cb62a177f --- /dev/null +++ b/panther_docking/config/panther_docking_server.yaml @@ -0,0 +1,49 @@ +/**: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 20.0 + undock_linear_tolerance: 0.08 + undock_angular_tolerance: 0.08 + max_retries: 3 + base_frame: "/base_link" + fixed_frame: "/odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + + dock_plugins: ["panther_docking_dock"] + panther_docking_dock: + plugin: panther_docking::PantherChargingDock + base_frame: "/base_link" + docking_distance_threshold: 0.15 + docking_yaw_threshold: 0.15 + staging_x_offset: -0.5 + staging_yaw_offset: 0.0 + + # 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.0 + external_detection_translation_y: -0.175 # Distance between the detection and ground + external_detection_translation_z: 0.8 # Distance between the detection and the front of the robot + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: 0.0 + external_detection_rotation_yaw: 1.57 + filter_coef: 0.1 + + enable_charger_service_call_timeout: 1.0 + + docks: ["main_dock"] + main_dock: + type: "panther_docking_dock" + frame: /main_dock + 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 diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp new file mode 100644 index 000000000..049ca6fd0 --- /dev/null +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -0,0 +1,252 @@ +// 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_CHARGING_DOCK_HPP_ +#define PANTHER_CHARGING_DOCK_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "opennav_docking/pose_filter.hpp" +#include "opennav_docking_core/charging_dock.hpp" +#include "opennav_docking_core/docking_exceptions.hpp" +#include "panther_msgs/msg/charging_status.hpp" +#include "panther_msgs/msg/io_state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/realtime_box.h" +#include "sensor_msgs/msg/battery_state.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" + +namespace panther_docking +{ + +/** + * @class PantherChargingDock + * @brief Abstract interface for a charging dock for the docking framework + */ +class PantherChargingDock : public opennav_docking_core::ChargingDock +{ +public: + using Ptr = std::shared_ptr; + + PantherChargingDock() + { + } + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent, const std::string& name, + std::shared_ptr tf) override final; + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() override final; + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() override final; + + /** + * @brief Method to deactivate Behavior and any threads involved in execution. + */ + virtual void deactivate() override final; + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose(const geometry_msgs::msg::Pose& pose, + const std::string& frame) override final; + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped& pose) override final; + + /** + * @brief Have we made contact with dock? This can be implemented in a variety + * of ways: by establishing communications with the dock, by monitoring the + * the drive motor efforts, etc. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + virtual bool isDocked() override final; + + /** + * @brief Are we charging? If a charge dock requires any sort of negotiation + * to begin charging, that should happen inside this function as this function + * will be called repeatedly after the docking loop to check if successful. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + virtual bool isCharging() override final; + + /** + * @brief Undocking while current is still flowing can damage a charge dock + * so some charge docks provide the ability to disable charging before the + * robot physically disconnects. The undocking action will not command the + * robot to move until this returns true. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + virtual bool disableCharging() override final; + + /** + * @brief Similar to isCharging() but called when undocking. + */ + virtual bool hasStoppedCharging() override final; + + /** + * @brief Get the name of the dock + */ + std::string getName(); + +protected: + /** + * @brief Method calls enable/disable service of the charger + * + * @param state The state to set the charger to + */ + void setChargerState(bool state); + + /** + * @brief Get the pose from a transform between two frames. + * + * This method retrieves the pose by transforming a pose from the child frame to the parent frame using a transform. + * + * @param frame_id The ID of the parent frame. + * @param child_frame_id The ID of the child frame. + * @return The pose in the parent frame. + */ + geometry_msgs::msg::PoseStamped getPoseFromTransform(const std::string& frame_id, const std::string& child_frame_id); + + /** + * @brief Transform a pose to a target frame. + * + * @param pose The pose to transform. + * @param target_frame The target frame to transform the pose to. + * + * @return The transformed pose. + */ + geometry_msgs::msg::PoseStamped transformPose(const geometry_msgs::msg::PoseStamped& pose, + const std::string& target_frame); + + /** + * @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. + */ + geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose(const geometry_msgs::msg::PoseStamped& dock_pose); + + /** + * @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. + */ + geometry_msgs::msg::PoseStamped offsetDetectedDockPose(const geometry_msgs::msg::PoseStamped& 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. + */ + geometry_msgs::msg::PoseStamped 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); + + std::string name_; + std::string base_frame_name_; + std::string dock_frame_; + float panther_version_; + + rclcpp::Logger logger_{ rclcpp::get_logger("PantherChargingDock") }; + rclcpp::Clock steady_clock_{ RCL_STEADY_TIME }; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr tf2_buffer_; + rclcpp::Subscription::SharedPtr charging_status_sub_; + rclcpp::Subscription::SharedPtr io_state_sub_; + rclcpp::Publisher::SharedPtr staging_pose_pub_; + rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Client::SharedPtr charger_enable_client_; + + realtime_tools::RealtimeBox> charging_status_box_{ nullptr }; + realtime_tools::RealtimeBox> io_state_box_{ nullptr }; + + geometry_msgs::msg::PoseStamped dock_pose_; + geometry_msgs::msg::PoseStamped 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 filter_; + + double docking_distance_threshold_; + double docking_yaw_threshold_; + + double staging_x_offset_; + double staging_yaw_offset_; + + double enable_charger_service_call_timeout_; +}; + +} // namespace panther_docking + +#endif // PANTHER_CHARGING_DOCK_HPP_ diff --git a/panther_docking/launch/apriltag_linux_cam.launch.py b/panther_docking/launch/apriltag_linux_cam.launch.py new file mode 100644 index 000000000..561e54988 --- /dev/null +++ b/panther_docking/launch/apriltag_linux_cam.launch.py @@ -0,0 +1,75 @@ +# 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.substitutions import ( + EnvironmentVariable, + 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_sim = LaunchConfiguration("use_sim") + declare_use_sim_arg = DeclareLaunchArgument( + "use_sim", + default_value="False", + description="Whether simulation is used", + choices=["True", "False"], + ) + + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes.", + ) + + apriltag_config_path = LaunchConfiguration("apriltag_config_path") + apriltag_config_path_arg = DeclareLaunchArgument( + "apriltag_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_docking"), "config", "apriltag.yaml"] + ), + description=("Path to apriltag configuration file."), + ) + + namespaced_apriltag_config_path = ReplaceString( + source_file=apriltag_config_path, + replacements={"": namespace, "//": "/"}, + ) + + return LaunchDescription( + [ + declare_use_sim_arg, + declare_namespace_arg, + apriltag_config_path_arg, + 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(), + ), + ] + ) diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py new file mode 100644 index 000000000..4f9fde75c --- /dev/null +++ b/panther_docking/launch/docking.launch.py @@ -0,0 +1,91 @@ +# 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.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0") + + 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"], + ) + + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Add namespace to all launched nodes.", + ) + + docking_server_path = LaunchConfiguration("docking_server_path") + declare_docking_server_path_arg = DeclareLaunchArgument( + "docking_server_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] + ), + description=("Path to docking server configuration file."), + ) + + namespaced_docking_server_config = ReplaceString( + source_file=docking_server_path, + replacements={"": namespace, "//": "/"}, + ) + + return LaunchDescription( + [ + declare_use_sim_arg, + declare_namespace_arg, + declare_docking_server_path_arg, + Node( + package="opennav_docking", + executable="opennav_docking", + parameters=[ + {"panther_version": panther_version}, + namespaced_docking_server_config, + {"use_sim_time": True}, + ], + namespace=namespace, + emulate_tty=True, + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="nav2_docking_lifecycle_manager", + parameters=[ + { + "autostart": True, + "node_names": [ + "docking_server", + ], + "use_sim_time": use_sim, + }, + ], + namespace=namespace, + ), + ] + ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml new file mode 100644 index 000000000..ae3377b8f --- /dev/null +++ b/panther_docking/package.xml @@ -0,0 +1,36 @@ + + + + panther_docking + 2.1.0 + Integration Panther with Wibotic charger and Opennav Docking framework + + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/panther_ros + https://github.com/husarion/panther_ros/issues + + Jakub Delicat + + geometry_msgs + nav2_util + opennav_docking + panther_msgs + panther_utils + pluginlib + rclcpp + realtime_tools + sensor_msgs + std_srvs + tf2_ros + + ament_cmake + ament_cmake_gtest + + + ament_cmake + + + diff --git a/panther_docking/plugin.xml b/panther_docking/plugin.xml new file mode 100644 index 000000000..ef3b789db --- /dev/null +++ b/panther_docking/plugin.xml @@ -0,0 +1,8 @@ + + + + A dock plugin interface for Panther robot + + + diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp new file mode 100644 index 000000000..c3660888e --- /dev/null +++ b/panther_docking/src/panther_charging_dock.cpp @@ -0,0 +1,451 @@ +// 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 "nav2_util/node_utils.hpp" +#include "panther_docking/panther_charging_dock.hpp" +#include "panther_utils/common_utilities.hpp" + +namespace panther_docking +{ + +void PantherChargingDock::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent, const std::string& name, + std::shared_ptr tf) +{ + name_ = name; + + if (!tf) + { + throw std::runtime_error("PantherChargingDock requires a TF buffer"); + } + + tf2_buffer_ = tf; + + node_ = parent.lock(); + if (!node_) + { + throw std::runtime_error("Failed to lock node"); + } + + nav2_util::declare_parameter_if_not_declared(node_, "panther_version", rclcpp::ParameterValue(1.0)); + + 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.0)); + 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)); + nav2_util::declare_parameter_if_not_declared(node_, name + ".external_detection_rotation_roll", + rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared(node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared(node_, name + ".docking_distance_threshold", + rclcpp::ParameterValue(0.05)); + nav2_util::declare_parameter_if_not_declared(node_, name + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); + + 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 + ".enable_charger_service_call_timeout", + rclcpp::ParameterValue(0.2)); + + node_->get_parameter("panther_version", panther_version_); + + node_->get_parameter(name + ".base_frame", base_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_.setEuler(yaw, pitch, roll); + 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 + ".enable_charger_service_call_timeout", enable_charger_service_call_timeout_); + + double filter_coef; + node_->get_parameter(name + ".filter_coef", filter_coef); + filter_ = std::make_unique(filter_coef, external_detection_timeout_); +} + +void PantherChargingDock::cleanup() +{ + dock_pose_pub_.reset(); + staging_pose_pub_.reset(); + + if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + charging_status_sub_.reset(); + io_state_sub_.reset(); + charger_enable_client_.reset(); + } +} + +void PantherChargingDock::activate() + +{ + dock_pose_pub_ = node_->create_publisher("docking/dock_pose", 1); + staging_pose_pub_ = node_->create_publisher("docking/staging_pose", 1); + + if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + charging_status_sub_ = node_->create_subscription( + "battery/charging_status", rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + charging_status_box_.set(std::move(msg)); + }); + io_state_sub_ = node_->create_subscription( + "hardware/io_state", rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { io_state_box_.set(std::move(msg)); }); + + charger_enable_client_ = node_->create_client("hardware/charger_enable"); + + using namespace std::chrono_literals; + if (!charger_enable_client_->wait_for_service(1s)) + { + RCLCPP_WARN_STREAM(logger_, + "Service \"hardware/charger_enable\" not available. Make sure if Panther ROS 2 Stack is " + "running."); + } + } +} + +void PantherChargingDock::deactivate() +{ + dock_pose_pub_.reset(); + staging_pose_pub_.reset(); + + if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + charging_status_sub_.reset(); + io_state_sub_.reset(); + charger_enable_client_.reset(); + } +} + +geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose(const geometry_msgs::msg::Pose& pose, + const std::string& frame) +{ + // When the pose if default the robot is docking so the frame is the dock frame + if (pose == geometry_msgs::msg::Pose()) + { + dock_frame_ = frame; + updateDockPoseAndPublish(); + updateStagingPoseAndPublish(base_frame_name_); + } + else + { + if (dock_frame_.empty()) + { + throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); + } + updateDockPoseAndPublish(); + updateStagingPoseAndPublish(frame); + } + + return staging_pose_; +} +bool PantherChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped& pose) +{ + updateDockPoseAndPublish(); + updateStagingPoseAndPublish(base_frame_name_); + pose = dock_pose_; + + return true; +} + +bool PantherChargingDock::isDocked() +{ + updateDockPoseAndPublish(); + const double d = std::hypot(dock_pose_.pose.position.x, dock_pose_.pose.position.y); + const double yaw_diff = std::abs(tf2::getYaw(dock_pose_.pose.orientation)); + + return d < docking_distance_threshold_ && yaw_diff < docking_yaw_threshold_; +} + +bool PantherChargingDock::isCharging() +{ + std::shared_ptr charging_status_msg; + + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + try + { + return isDocked(); + } + catch (const opennav_docking_core::FailedToDetectDock& e) + { + return false; + } + } + + // TODO: This might be used whe is undocking but we not want to enable charging when the robot is undocking + try + { + setChargerState(true); + } + catch (const std::runtime_error& e) + { + throw opennav_docking_core::FailedToCharge("An exception occurred while enabling charging: " + + std::string(e.what())); + } + + // CAUTION: The conteroller frequency can be higher than the message frequency + charging_status_box_.get(charging_status_msg); + + if (!charging_status_msg) + { + throw opennav_docking_core::FailedToCharge("Did not receive charging_status_msg message"); + } + + return charging_status_msg->charging; +} + +bool PantherChargingDock::disableCharging() +{ + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + return true; + } + + try + { + setChargerState(false); + } + catch (const std::runtime_error& e) + { + RCLCPP_ERROR_STREAM(logger_, "An exception occurred while disabling charging: " << e.what()); + return false; + } + + return true; +} + +bool PantherChargingDock::hasStoppedCharging() +{ + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + return !isCharging(); + } + + std::shared_ptr charging_status_msg; + charging_status_box_.get(charging_status_msg); + if (!charging_status_msg) + { + throw opennav_docking_core::FailedToCharge("Did not receive charging_status_msg message"); + } + + if (charging_status_msg->charging) + { + throw opennav_docking_core::FailedToCharge("Charging status is still true"); + } + + return true; +} + +std::string PantherChargingDock::getName() +{ + return name_; +} + +void PantherChargingDock::setChargerState(bool state) +{ + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) + { + throw std::runtime_error("This version of Panther does not support charger control"); + } + + auto request = std::make_shared(); + request->data = state; + + auto result = charger_enable_client_->async_send_request(request); + + const auto timeout = std::chrono::duration(enable_charger_service_call_timeout_); + // This doubles spinning the node because the node is spinning in docking_server + if (rclcpp::spin_until_future_complete(node_, result, timeout) != rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("Failed to call charger enable service"); + } + + if (!result.get()->success) + { + throw std::runtime_error("Failed to enable charger"); + } +} + +geometry_msgs::msg::PoseStamped PantherChargingDock::getPoseFromTransform(const std::string& frame_id, + const std::string& child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform; + try + { + auto timeout = tf2::durationFromSec(external_detection_timeout_); + + transform = tf2_buffer_->lookupTransform(frame_id, child_frame_id, node_->get_clock()->now(), timeout); + } + catch (tf2::TransformException& ex) + { + throw std::runtime_error("An exception occurred while getting pose from transform: " + std::string(ex.what())); + } + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = transform.header.frame_id; + pose.header.stamp = transform.header.stamp; + pose.pose.position.x = transform.transform.translation.x; + pose.pose.position.y = transform.transform.translation.y; + pose.pose.position.z = transform.transform.translation.z; + pose.pose.orientation.x = transform.transform.rotation.x; + pose.pose.orientation.y = transform.transform.rotation.y; + pose.pose.orientation.z = transform.transform.rotation.z; + return pose; +} + +geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose(const geometry_msgs::msg::PoseStamped& pose, + const std::string& target_frame) +{ + geometry_msgs::msg::PoseStamped transformed_pose; + + if (pose.header.frame_id.empty() || target_frame.empty()) + { + throw std::runtime_error("Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + + "\", target frame: \"" + target_frame + "\""); + } + + if (!tf2_buffer_->canTransform(pose.header.frame_id, target_frame, pose.header.stamp, + rclcpp::Duration::from_seconds(external_detection_timeout_))) + { + throw std::runtime_error("Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + + std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); + } + + tf2_buffer_->transform(pose, transformed_pose, target_frame); + + return transformed_pose; +} + +geometry_msgs::msg::PoseStamped +PantherChargingDock::offsetStagingPoseToDockPose(const geometry_msgs::msg::PoseStamped& dock_pose) +{ + tf2::Transform dock_pose_transform; + tf2::fromMsg(dock_pose.pose, dock_pose_transform); + + 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); + + tf2::Transform staging_pose_transform = dock_pose_transform * staging_offset_transform; + + auto staging_pose = dock_pose; + staging_pose.header = dock_pose.header; + staging_pose.pose.position.x = staging_pose_transform.getOrigin().getX(); + staging_pose.pose.position.y = staging_pose_transform.getOrigin().getY(); + staging_pose.pose.position.z = staging_pose_transform.getOrigin().getZ(); + + staging_pose.pose.orientation = tf2::toMsg(staging_pose_transform.getRotation()); + + return staging_pose; +} + +geometry_msgs::msg::PoseStamped +PantherChargingDock::offsetDetectedDockPose(const geometry_msgs::msg::PoseStamped& detected_dock_pose) +{ + geometry_msgs::msg::PoseStamped just_orientation; + just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation = detected_dock_pose.pose.orientation; + + tf2::doTransform(just_orientation, just_orientation, transform); + + tf2::Quaternion orientation(just_orientation.pose.orientation.x, just_orientation.pose.orientation.y, + just_orientation.pose.orientation.z, just_orientation.pose.orientation.w); + + geometry_msgs::msg::PoseStamped offset_detected_dock_pose = detected_dock_pose; + offset_detected_dock_pose.pose.orientation = tf2::toMsg(orientation); + offset_detected_dock_pose.header = detected_dock_pose.header; + offset_detected_dock_pose.pose.position = detected_dock_pose.pose.position; + offset_detected_dock_pose.pose.position.x += external_detection_translation_x_; + offset_detected_dock_pose.pose.position.y += external_detection_translation_y_; + offset_detected_dock_pose.pose.position.z += external_detection_translation_z_; + + return offset_detected_dock_pose; +} + +geometry_msgs::msg::PoseStamped PantherChargingDock::getDockPose(const std::string& frame) +{ + geometry_msgs::msg::PoseStamped offset_detected_dock_pose; + try + { + offset_detected_dock_pose = offsetDetectedDockPose(getPoseFromTransform(frame, frame)); + auto filtered_offset_detected_dock_pose = filter_->update(offset_detected_dock_pose); + } + catch (const std::runtime_error& e) + { + throw std::runtime_error("An exception occurred while getting dock pose: " + std::string(e.what())); + } + + return offset_detected_dock_pose; +} + +void PantherChargingDock::updateDockPoseAndPublish() +{ + try + { + auto new_dock_pose = getDockPose(dock_frame_); + dock_pose_ = transformPose(new_dock_pose, base_frame_name_); + dock_pose_.pose.position.z = 0.0; + 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_ = transformPose(new_staging_pose, frame); + 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())); + } +} + +} // namespace panther_docking + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(panther_docking::PantherChargingDock, opennav_docking_core::ChargingDock) diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp new file mode 100644 index 000000000..130bc256a --- /dev/null +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -0,0 +1,448 @@ +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "panther_docking/panther_charging_dock.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/static_transform_broadcaster.h" + +class PantherChargingDockWrapper : public panther_docking::PantherChargingDock +{ +public: + PantherChargingDockWrapper() : panther_docking::PantherChargingDock() + { + } + + void setChargerState(bool state) + { + panther_docking::PantherChargingDock::setChargerState(state); + } + + geometry_msgs::msg::PoseStamped getPoseFromTransform(const std::string& frame_id, const std::string& child_frame_id) + { + return panther_docking::PantherChargingDock::getPoseFromTransform(frame_id, child_frame_id); + } + + geometry_msgs::msg::PoseStamped transformPose(const geometry_msgs::msg::PoseStamped& pose, + const std::string& target_frame) + { + return panther_docking::PantherChargingDock::transformPose(pose, target_frame); + } + 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(double panther_version); + void CreateChargerEnableService(bool success); + void CreateChargerStatusPublisher(); + void PublishChargerStatus(bool charging); + void PublishBaseLinkTransform(); + void PublishDockTransform(); + void EnableSpinningNode(); + + std::shared_ptr executor_; + std::shared_ptr node_; + std::shared_ptr tf2_buffer_; + std::shared_ptr charging_dock_; + std::shared_ptr tf2_static_broadcaster_; + rclcpp::Service::SharedPtr charger_enable_service_; + rclcpp::Publisher::SharedPtr charger_state_pub_; + + std::shared_ptr spin_thread_; + bool charging_status_; +}; + +TestPantherChargingDock::TestPantherChargingDock() +{ + rclcpp::init(0, nullptr); + node_ = std::make_shared("panther_charging_dock_test"); +} + +TestPantherChargingDock::~TestPantherChargingDock() +{ + if (charging_dock_) + { + charging_dock_->deactivate(); + charging_dock_->cleanup(); + } + + if (executor_) + { + executor_->cancel(); + } + + if (spin_thread_) + { + spin_thread_->join(); + } + + rclcpp::shutdown(); +} + +void TestPantherChargingDock::ConfigureAndActivateDock(double panther_version) +{ + node_->declare_parameter("panther_version", rclcpp::ParameterValue(panther_version)); + + tf2_buffer_ = std::make_shared(node_->get_clock()); + // tf2_buffer_->setUsingDedicatedThread(true); + + tf2_static_broadcaster_ = std::make_shared(node_); + charging_dock_ = std::make_shared(); + + charging_dock_->configure(node_, "test_dock", tf2_buffer_); + charging_dock_->activate(); +} + +void TestPantherChargingDock::CreateChargerEnableService(bool success) +{ + charger_enable_service_ = node_->create_service( + "hardware/charger_enable", [success, this](const std::shared_ptr request, + std::shared_ptr response) { + response->success = success; + if (success) + { + this->charging_status_ = request->data; + } + else + { + this->charging_status_ = false; + } + + if (this->charger_state_pub_) + { + PublishChargerStatus(this->charging_status_); + } + }); +} + +void TestPantherChargingDock::CreateChargerStatusPublisher() +{ + charger_state_pub_ = + node_->create_publisher("battery/charging_status", rclcpp::QoS(1)); +} + +void TestPantherChargingDock::PublishChargerStatus(bool charging) +{ + panther_msgs::msg::ChargingStatus msg; + msg.charging = charging; + charger_state_pub_->publish(msg); +} + +void TestPantherChargingDock::PublishBaseLinkTransform() +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = 0.3; + transform.transform.translation.y = 0.2; + transform.transform.translation.z = 0.1; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf2_static_broadcaster_->sendTransform(transform); +} + +void TestPantherChargingDock::PublishDockTransform() +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "base_link"; + transform.child_frame_id = "test_dock"; + transform.transform.translation.x = 1.0; + transform.transform.translation.y = 2.0; + transform.transform.translation.z = 3.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf2_static_broadcaster_->sendTransform(transform); +} + +void TestPantherChargingDock::EnableSpinningNode() +{ + executor_ = std::make_shared(); + executor_->add_node(node_->get_node_base_interface()); + spin_thread_ = std::make_shared([this]() { this->executor_->spin(); }); +} + +TEST_F(TestPantherChargingDock, SetChargerStateOlderFailure) +{ + ConfigureAndActivateDock(1.06); + EXPECT_THROW({ charging_dock_->setChargerState(true); }, std::runtime_error); + EXPECT_THROW({ charging_dock_->setChargerState(false); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, SetChargerStateFailure) +{ + ConfigureAndActivateDock(1.21); + CreateChargerEnableService(false); + EXPECT_THROW({ charging_dock_->setChargerState(true); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, SetChargerStateSuccess) +{ + ConfigureAndActivateDock(1.21); + CreateChargerEnableService(true); + CreateChargerStatusPublisher(); + + EXPECT_NO_THROW({ charging_dock_->setChargerState(true); }); + EXPECT_TRUE(charging_status_); + EXPECT_NO_THROW({ charging_dock_->setChargerState(false); }); + EXPECT_FALSE(charging_status_); +} + +TEST_F(TestPantherChargingDock, GetPoseFromTransformFailure) +{ + ConfigureAndActivateDock(1.21); + EXPECT_THROW({ charging_dock_->getPoseFromTransform("world", "base_link"); }, std::runtime_error); +} + +// tf2_buffer_ cannot read transformations +// TEST_F(TestPantherChargingDock, GetPoseFromTransformSuccess) +// { +// ConfigureAndActivateDock(1.21); +// CreateChargerEnableService(true); +// EnableSpinningNode(); +// PublishBaseLinkTransform(); + +// geometry_msgs::msg::PoseStamped pose; +// ASSERT_NO_THROW({ pose = charging_dock_->getPoseFromTransform("base_link", "world"); };); + +// EXPECT_NEAR(pose.pose.position.x, 0.3, 0.01); +// EXPECT_NEAR(pose.pose.position.y, 0.2, 0.01); +// EXPECT_NEAR(pose.pose.position.z, 0.1, 0.01); +// EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); +// } + +TEST_F(TestPantherChargingDock, TransformPoseFailure) +{ + ConfigureAndActivateDock(1.21); + geometry_msgs::msg::PoseStamped pose; + EXPECT_THROW({ charging_dock_->transformPose(pose, ""); }, std::runtime_error); + EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); + + pose.header.frame_id = "odom"; + EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); +} + +// tf2_buffer_ cannot read transformations +// TEST_F(TestPantherChargingDock, TransformPoseSuccess) +// { +// ConfigureAndActivateDock(1.21); +// CreateChargerEnableService(true); +// EnableSpinningNode(); +// PublishBaseLinkTransform(); +// geometry_msgs::msg::PoseStamped pose; +// EXPECT_THROW({ charging_dock_->transformPose(pose, ""); }, std::runtime_error); +// EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); + +// pose.header.frame_id = "odom"; +// EXPECT_NO_THROW({ pose = charging_dock_->transformPose(pose, "base_link"); }); +// } + +TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) +{ + const double x_offset = 0.5; + const double yaw_offset = 1.57; + node_->declare_parameter("test_dock.staging_x_offset", rclcpp::ParameterValue(x_offset)); + node_->declare_parameter("test_dock.staging_yaw_offset", rclcpp::ParameterValue(yaw_offset)); + ConfigureAndActivateDock(1.21); + geometry_msgs::msg::PoseStamped pose; + + auto new_pose = charging_dock_->offsetStagingPoseToDockPose(pose); + EXPECT_NEAR(new_pose.pose.position.x, x_offset, 0.01); + EXPECT_NEAR(new_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(new_pose.pose.orientation), yaw_offset, 0.01); +} + +TEST_F(TestPantherChargingDock, offsetDetectedDockPose) +{ + const double external_detection_translation_x_ = 0.3; + const double external_detection_translation_y_ = 0.1; + const double external_detection_translation_z_ = 0.0; + const double roll = 0.0; + const double pitch = 0.0; + const double yaw = 1.57; + + 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(roll)); + node_->declare_parameter("test_dock.external_detection_rotation_pitch", rclcpp::ParameterValue(pitch)); + node_->declare_parameter("test_dock.external_detection_rotation_yaw", rclcpp::ParameterValue(yaw)); + + ConfigureAndActivateDock(1.21); + geometry_msgs::msg::PoseStamped pose; + + auto new_pose = charging_dock_->offsetDetectedDockPose(pose); + + tf2::Quaternion external_detection_rotation_; + external_detection_rotation_.setEuler(yaw, pitch, roll); + + tf2::Quaternion offset_rotation; + tf2::fromMsg(new_pose.pose.orientation, offset_rotation); + EXPECT_NEAR(new_pose.pose.position.x, external_detection_translation_x_, 0.01); + EXPECT_NEAR(new_pose.pose.position.y, external_detection_translation_y_, 0.01); + EXPECT_NEAR(new_pose.pose.position.z, 0.0, 0.01); + EXPECT_EQ(offset_rotation, external_detection_rotation_); +} + +TEST_F(TestPantherChargingDock, GetDockPoseFailure) +{ + ConfigureAndActivateDock(1.21); + EXPECT_THROW({ charging_dock_->getDockPose("test_dock"); }, std::runtime_error); +} + +// tf2_buffer_ cannot read transformations +// TEST_F(TestPantherChargingDock, GetDockPoseSuccess) +// { +// const double external_detection_translation_x_ = 0.3; +// const double external_detection_translation_y_ = 0.1; +// const double external_detection_translation_z_ = 0.0; +// const double roll = 0.0; +// const double pitch = 0.0; +// const double yaw = 1.57; + +// 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(roll)); +// node_->declare_parameter("test_dock.external_detection_rotation_pitch", rclcpp::ParameterValue(pitch)); +// node_->declare_parameter("test_dock.external_detection_rotation_yaw", rclcpp::ParameterValue(yaw)); + +// ConfigureAndActivateDock(1.21); +// CreateChargerEnableService(true); +// EnableSpinningNode(); +// PublishBaseLinkTransform(); + +// auto new_pose = charging_dock_->getDockPose("test_dock"); + +// tf2::Quaternion external_detection_rotation_; +// external_detection_rotation_.setEuler(yaw, pitch, roll); + +// tf2::Quaternion offset_rotation; +// tf2::fromMsg(new_pose.pose.orientation, offset_rotation); +// EXPECT_NEAR(new_pose.pose.position.x, external_detection_translation_x_, 0.01); +// EXPECT_NEAR(new_pose.pose.position.y, external_detection_translation_y_, 0.01); +// EXPECT_NEAR(new_pose.pose.position.z, 0.0, 0.01); +// EXPECT_EQ(offset_rotation, external_detection_rotation_); +// } + +// TEST_F(TestPantherChargingDock, ConfigureAndActivateOlder) +// { +// ConfigureAndActivateDock(1.06); +// EXPECT_FALSE(charging_dock_->isCharging()); +// EXPECT_TRUE(charging_dock_->disableCharging()); +// EXPECT_TRUE(charging_dock_->hasStoppedCharging()); +// } + +// TEST_F(TestPantherChargingDock, ChargingStateNoServiceFailure) +// { +// ConfigureAndActivateDock(1.21); +// EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); +// EXPECT_FALSE(charging_dock_->disableCharging()); +// EXPECT_THROW({ charging_dock_->hasStoppedCharging(); }, opennav_docking_core::FailedToCharge); +// } + +// TEST_F(TestPantherChargingDock, ChargingStateFailRespondFailure) +// { +// ConfigureAndActivateDock(1.21); +// CreateChargerStatusPublisher(); +// CreateChargerEnableService(false); + +// EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); +// EXPECT_FALSE(charging_dock_->disableCharging()); +// EXPECT_TRUE(charging_dock_->hasStoppedCharging()); +// } + +// TEST_F(TestPantherChargingDock, ChargingStateSuccess) +// { +// ConfigureAndActivateDock(1.21); +// CreateChargerStatusPublisher(); +// CreateChargerEnableService(true); + +// EXPECT_TRUE(charging_dock_->isCharging()); +// EXPECT_TRUE(charging_dock_->disableCharging()); +// EXPECT_TRUE(charging_dock_->hasStoppedCharging()); +// } + +// TEST(PantherChargingDockTests, ObjectLifecycle) +// { +// rclcpp::init(0, nullptr); +// auto node = std::make_shared("panther_charging_dock_test"); +// node->declare_parameter("panther_version", rclcpp::ParameterValue(1.06)); + +// auto dock = std::make_unique(); +// EXPECT_THROW({ dock->configure(node, "my_dock", nullptr); }, std::runtime_error); + +// auto tf2_buffer = std::make_shared(node->get_clock()); +// tf2_buffer->setUsingDedicatedThread(true); + +// EXPECT_NO_THROW({ dock->configure(node, "my_dock", tf2_buffer); }); + +// dock->activate(); +// EXPECT_FALSE(dock->isCharging()); +// EXPECT_TRUE(dock->disableCharging()); +// EXPECT_TRUE(dock->hasStoppedCharging()); + +// dock->deactivate(); +// dock->cleanup(); +// dock.reset(); +// rclcpp::shutdown(); +// } diff --git a/panther_utils/include/panther_utils/common_utilities.hpp b/panther_utils/include/panther_utils/common_utilities.hpp index 17a8f3647..df34819a7 100644 --- a/panther_utils/include/panther_utils/common_utilities.hpp +++ b/panther_utils/include/panther_utils/common_utilities.hpp @@ -63,6 +63,20 @@ std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmo return file; } +/** + * @brief Checks if the Panther version is at least the specified version. + * + * This function checks if the Panther version is at least the specified version. + * + * @param panther_version_ The Panther version to be checked. + * @param version The version to be compared with. + * @return bool True if the Panther version is at least the specified version, false otherwise. + */ +bool IsPantherVersionAtLeast(const float panther_version_, const float version) +{ + return panther_version_ >= version - std::numeric_limits::epsilon(); +} + } // namespace panther_utils::common_utilities #endif // PANTHER_UTILS_COMMON_UTILITIES_HPP_ From c8cfb372a229012ffd338ce9e62aa83d171b9b67 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 19 Aug 2024 18:48:45 +0000 Subject: [PATCH 03/23] Added tests Signed-off-by: Jakub Delicat --- .../panther_docking/panther_charging_dock.hpp | 11 - panther_docking/src/panther_charging_dock.cpp | 64 +-- .../test/test_panther_charging_dock.cpp | 473 ++++++++++-------- 3 files changed, 290 insertions(+), 258 deletions(-) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 049ca6fd0..946364117 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -138,17 +138,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void setChargerState(bool state); - /** - * @brief Get the pose from a transform between two frames. - * - * This method retrieves the pose by transforming a pose from the child frame to the parent frame using a transform. - * - * @param frame_id The ID of the parent frame. - * @param child_frame_id The ID of the child frame. - * @return The pose in the parent frame. - */ - geometry_msgs::msg::PoseStamped getPoseFromTransform(const std::string& frame_id, const std::string& child_frame_id); - /** * @brief Transform a pose to a target frame. * diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index c3660888e..9f00fe827 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -79,7 +79,8 @@ void PantherChargingDock::configure(const rclcpp_lifecycle::LifecycleNode::WeakP 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_.setEuler(yaw, pitch, 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_); @@ -169,11 +170,20 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose(const geomet return staging_pose_; } + bool PantherChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped& pose) { - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(base_frame_name_); - pose = dock_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()); + return false; + } return true; } @@ -297,33 +307,6 @@ void PantherChargingDock::setChargerState(bool state) } } -geometry_msgs::msg::PoseStamped PantherChargingDock::getPoseFromTransform(const std::string& frame_id, - const std::string& child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - try - { - auto timeout = tf2::durationFromSec(external_detection_timeout_); - - transform = tf2_buffer_->lookupTransform(frame_id, child_frame_id, node_->get_clock()->now(), timeout); - } - catch (tf2::TransformException& ex) - { - throw std::runtime_error("An exception occurred while getting pose from transform: " + std::string(ex.what())); - } - - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = transform.header.frame_id; - pose.header.stamp = transform.header.stamp; - pose.pose.position.x = transform.transform.translation.x; - pose.pose.position.y = transform.transform.translation.y; - pose.pose.position.z = transform.transform.translation.z; - pose.pose.orientation.x = transform.transform.rotation.x; - pose.pose.orientation.y = transform.transform.rotation.y; - pose.pose.orientation.z = transform.transform.rotation.z; - return pose; -} - geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose(const geometry_msgs::msg::PoseStamped& pose, const std::string& target_frame) { @@ -399,27 +382,32 @@ PantherChargingDock::offsetDetectedDockPose(const geometry_msgs::msg::PoseStampe geometry_msgs::msg::PoseStamped PantherChargingDock::getDockPose(const std::string& frame) { - geometry_msgs::msg::PoseStamped offset_detected_dock_pose; + geometry_msgs::msg::PoseStamped filtered_offset_detected_dock_pose; try { - offset_detected_dock_pose = offsetDetectedDockPose(getPoseFromTransform(frame, frame)); - auto filtered_offset_detected_dock_pose = filter_->update(offset_detected_dock_pose); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = frame; + pose.header.stamp = node_->get_clock()->now(); + auto offset_detected_dock_pose = offsetDetectedDockPose(pose); + + filtered_offset_detected_dock_pose = filter_->update(offset_detected_dock_pose); + filtered_offset_detected_dock_pose = transformPose(filtered_offset_detected_dock_pose, base_frame_name_); + + 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())); } - return offset_detected_dock_pose; + return filtered_offset_detected_dock_pose; } void PantherChargingDock::updateDockPoseAndPublish() { try { - auto new_dock_pose = getDockPose(dock_frame_); - dock_pose_ = transformPose(new_dock_pose, base_frame_name_); - dock_pose_.pose.position.z = 0.0; + dock_pose_ = getDockPose(dock_frame_); dock_pose_pub_->publish(dock_pose_); } catch (const std::runtime_error& e) diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp index 130bc256a..9dc87a954 100644 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -17,16 +17,19 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" -#include "panther_docking/panther_charging_dock.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/static_transform_broadcaster.h" +#include "panther_docking/panther_charging_dock.hpp" +#include "panther_utils/test/ros_test_utils.hpp" + class PantherChargingDockWrapper : public panther_docking::PantherChargingDock { public: + using SharedPtr = std::shared_ptr; PantherChargingDockWrapper() : panther_docking::PantherChargingDock() { } @@ -36,11 +39,6 @@ class PantherChargingDockWrapper : public panther_docking::PantherChargingDock panther_docking::PantherChargingDock::setChargerState(state); } - geometry_msgs::msg::PoseStamped getPoseFromTransform(const std::string& frame_id, const std::string& child_frame_id) - { - return panther_docking::PantherChargingDock::getPoseFromTransform(frame_id, child_frame_id); - } - geometry_msgs::msg::PoseStamped transformPose(const geometry_msgs::msg::PoseStamped& pose, const std::string& target_frame) { @@ -82,20 +80,32 @@ class TestPantherChargingDock : public testing::Test void CreateChargerEnableService(bool success); void CreateChargerStatusPublisher(); void PublishChargerStatus(bool charging); - void PublishBaseLinkTransform(); - void PublishDockTransform(); - void EnableSpinningNode(); - - std::shared_ptr executor_; - std::shared_ptr node_; - std::shared_ptr tf2_buffer_; - std::shared_ptr charging_dock_; - std::shared_ptr tf2_static_broadcaster_; + void SetBaseLinkToOdomTransform(); + void SetDockToBaseLinkTransform(double x = 1.0, double y = 2.0, double z = 3.0, double yaw = 1.57); + void SetDetectionOffsets(); + void SetStagingOffsets(); + + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + tf2_ros::Buffer::SharedPtr tf2_buffer_; + PantherChargingDockWrapper::SharedPtr charging_dock_; rclcpp::Service::SharedPtr charger_enable_service_; rclcpp::Publisher::SharedPtr charger_state_pub_; + 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_; - std::shared_ptr spin_thread_; 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() @@ -117,11 +127,6 @@ TestPantherChargingDock::~TestPantherChargingDock() executor_->cancel(); } - if (spin_thread_) - { - spin_thread_->join(); - } - rclcpp::shutdown(); } @@ -130,9 +135,16 @@ void TestPantherChargingDock::ConfigureAndActivateDock(double panther_version) node_->declare_parameter("panther_version", rclcpp::ParameterValue(panther_version)); tf2_buffer_ = std::make_shared(node_->get_clock()); - // tf2_buffer_->setUsingDedicatedThread(true); - tf2_static_broadcaster_ = std::make_shared(node_); + 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_shared(); charging_dock_->configure(node_, "test_dock", tf2_buffer_); @@ -174,7 +186,7 @@ void TestPantherChargingDock::PublishChargerStatus(bool charging) charger_state_pub_->publish(msg); } -void TestPantherChargingDock::PublishBaseLinkTransform() +void TestPantherChargingDock::SetBaseLinkToOdomTransform() { geometry_msgs::msg::TransformStamped transform; transform.header.stamp = node_->now(); @@ -188,31 +200,46 @@ void TestPantherChargingDock::PublishBaseLinkTransform() transform.transform.rotation.z = 0.0; transform.transform.rotation.w = 1.0; - tf2_static_broadcaster_->sendTransform(transform); + tf2_buffer_->setTransform(transform, "unittest", true); } -void TestPantherChargingDock::PublishDockTransform() +void TestPantherChargingDock::SetDockToBaseLinkTransform(double x, double y, double z, double yaw) { geometry_msgs::msg::TransformStamped transform; transform.header.stamp = node_->now(); transform.header.frame_id = "base_link"; transform.child_frame_id = "test_dock"; - transform.transform.translation.x = 1.0; - transform.transform.translation.y = 2.0; - transform.transform.translation.z = 3.0; - transform.transform.rotation.x = 0.0; - transform.transform.rotation.y = 0.0; - transform.transform.rotation.z = 0.0; - transform.transform.rotation.w = 1.0; + transform.transform.translation.x = x; + transform.transform.translation.y = y; + transform.transform.translation.z = z; - tf2_static_broadcaster_->sendTransform(transform); + tf2::Quaternion just_orientation; + just_orientation.setRPY(0, 0, yaw); + transform.transform.rotation = tf2::toMsg(just_orientation); + + tf2_buffer_->setTransform(transform, "unittest", true); } -void TestPantherChargingDock::EnableSpinningNode() +void TestPantherChargingDock::SetDetectionOffsets() { - executor_ = std::make_shared(); - executor_->add_node(node_->get_node_base_interface()); - spin_thread_ = std::make_shared([this]() { this->executor_->spin(); }); + 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, SetChargerStateOlderFailure) @@ -241,30 +268,7 @@ TEST_F(TestPantherChargingDock, SetChargerStateSuccess) EXPECT_FALSE(charging_status_); } -TEST_F(TestPantherChargingDock, GetPoseFromTransformFailure) -{ - ConfigureAndActivateDock(1.21); - EXPECT_THROW({ charging_dock_->getPoseFromTransform("world", "base_link"); }, std::runtime_error); -} - -// tf2_buffer_ cannot read transformations -// TEST_F(TestPantherChargingDock, GetPoseFromTransformSuccess) -// { -// ConfigureAndActivateDock(1.21); -// CreateChargerEnableService(true); -// EnableSpinningNode(); -// PublishBaseLinkTransform(); - -// geometry_msgs::msg::PoseStamped pose; -// ASSERT_NO_THROW({ pose = charging_dock_->getPoseFromTransform("base_link", "world"); };); - -// EXPECT_NEAR(pose.pose.position.x, 0.3, 0.01); -// EXPECT_NEAR(pose.pose.position.y, 0.2, 0.01); -// EXPECT_NEAR(pose.pose.position.z, 0.1, 0.01); -// EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); -// } - -TEST_F(TestPantherChargingDock, TransformPoseFailure) +TEST_F(TestPantherChargingDock, TransformPose) { ConfigureAndActivateDock(1.21); geometry_msgs::msg::PoseStamped pose; @@ -273,176 +277,227 @@ TEST_F(TestPantherChargingDock, TransformPoseFailure) pose.header.frame_id = "odom"; EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); -} -// tf2_buffer_ cannot read transformations -// TEST_F(TestPantherChargingDock, TransformPoseSuccess) -// { -// ConfigureAndActivateDock(1.21); -// CreateChargerEnableService(true); -// EnableSpinningNode(); -// PublishBaseLinkTransform(); -// geometry_msgs::msg::PoseStamped pose; -// EXPECT_THROW({ charging_dock_->transformPose(pose, ""); }, std::runtime_error); -// EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); - -// pose.header.frame_id = "odom"; -// EXPECT_NO_THROW({ pose = charging_dock_->transformPose(pose, "base_link"); }); -// } + SetBaseLinkToOdomTransform(); + + pose.header.frame_id = "odom"; + pose.pose.position.x = 0.1; + + ASSERT_NO_THROW({ pose = charging_dock_->transformPose(pose, "odom"); };); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); + EXPECT_EQ(pose.header.frame_id, "odom"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); + + ASSERT_NO_THROW({ pose = charging_dock_->transformPose(pose, "base_link"); };); + EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); + EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); + EXPECT_EQ(pose.header.frame_id, "base_link"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); +} TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) { - const double x_offset = 0.5; - const double yaw_offset = 1.57; - node_->declare_parameter("test_dock.staging_x_offset", rclcpp::ParameterValue(x_offset)); - node_->declare_parameter("test_dock.staging_yaw_offset", rclcpp::ParameterValue(yaw_offset)); + SetStagingOffsets(); ConfigureAndActivateDock(1.21); - geometry_msgs::msg::PoseStamped pose; - auto new_pose = charging_dock_->offsetStagingPoseToDockPose(pose); - EXPECT_NEAR(new_pose.pose.position.x, x_offset, 0.01); - EXPECT_NEAR(new_pose.pose.position.y, 0.0, 0.01); - EXPECT_NEAR(tf2::getYaw(new_pose.pose.orientation), yaw_offset, 0.01); + 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) { - const double external_detection_translation_x_ = 0.3; - const double external_detection_translation_y_ = 0.1; - const double external_detection_translation_z_ = 0.0; - const double roll = 0.0; - const double pitch = 0.0; - const double yaw = 1.57; - - 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(roll)); - node_->declare_parameter("test_dock.external_detection_rotation_pitch", rclcpp::ParameterValue(pitch)); - node_->declare_parameter("test_dock.external_detection_rotation_yaw", rclcpp::ParameterValue(yaw)); - + SetDetectionOffsets(); ConfigureAndActivateDock(1.21); geometry_msgs::msg::PoseStamped pose; auto new_pose = charging_dock_->offsetDetectedDockPose(pose); - tf2::Quaternion external_detection_rotation_; - external_detection_rotation_.setEuler(yaw, pitch, roll); + tf2::Quaternion external_detection_rotation; + + external_detection_rotation.setRPY(external_detection_roll_, external_detection_pitch_, external_detection_yaw_); tf2::Quaternion offset_rotation; tf2::fromMsg(new_pose.pose.orientation, offset_rotation); EXPECT_NEAR(new_pose.pose.position.x, external_detection_translation_x_, 0.01); EXPECT_NEAR(new_pose.pose.position.y, external_detection_translation_y_, 0.01); - EXPECT_NEAR(new_pose.pose.position.z, 0.0, 0.01); - EXPECT_EQ(offset_rotation, external_detection_rotation_); + EXPECT_NEAR(new_pose.pose.position.z, external_detection_translation_z_, 0.01); + EXPECT_EQ(offset_rotation, external_detection_rotation); +} + +TEST_F(TestPantherChargingDock, GetDockPose) +{ + SetDetectionOffsets(); + + ConfigureAndActivateDock(1.21); + + EXPECT_THROW({ charging_dock_->getDockPose("wrong_dock_pose"); }, std::runtime_error); + + SetDockToBaseLinkTransform(); + geometry_msgs::msg::PoseStamped pose; + ASSERT_NO_THROW({ pose = charging_dock_->getDockPose("test_dock"); };); + + 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); + + EXPECT_NEAR(pose.pose.position.x, 1.0 - external_detection_translation_y_, 0.01); + EXPECT_NEAR(pose.pose.position.y, 2.0 + external_detection_translation_x_, 0.01); + EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); + EXPECT_EQ(pose.header.frame_id, "base_link"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); +} + +TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) +{ + SetDetectionOffsets(); + SetStagingOffsets(); + + ConfigureAndActivateDock(1.21); + SetDockToBaseLinkTransform(); + SetBaseLinkToOdomTransform(); + + ASSERT_NO_THROW({ + charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + charging_dock_->updateDockPoseAndPublish(); + charging_dock_->updateStagingPoseAndPublish("base_link"); + }); + + 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))); + + 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); + + EXPECT_NEAR(dock_pose_->pose.position.x, 1.0 - external_detection_translation_y_, 0.01); + EXPECT_NEAR(dock_pose_->pose.position.y, 2.0 + external_detection_translation_x_, 0.01); + EXPECT_NEAR(dock_pose_->pose.position.z, 0.0, 0.01); + EXPECT_EQ(dock_pose_->header.frame_id, "base_link"); + EXPECT_NEAR(tf2::getYaw(dock_pose_->pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); + + EXPECT_NEAR(staging_pose_->pose.position.x, 1.0 - external_detection_translation_y_ - staging_x_offset_, 0.01); + EXPECT_NEAR(staging_pose_->pose.position.y, 2.0 + external_detection_translation_x_, 0.01); + EXPECT_NEAR(staging_pose_->pose.position.z, 0.0, 0.01); + EXPECT_EQ(staging_pose_->header.frame_id, "base_link"); + + // It is 4.71 but tf2::getYaw moves by 2pi what is 6.28 + EXPECT_NEAR(tf2::getYaw(staging_pose_->pose.orientation), -1.57, 0.01); + + ASSERT_NO_THROW({ charging_dock_->updateStagingPoseAndPublish("odom"); }); + ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); + + EXPECT_NEAR(staging_pose_->pose.position.x, 1.0 - external_detection_translation_y_ - staging_x_offset_ + 0.3, 0.01); + EXPECT_NEAR(staging_pose_->pose.position.y, 2.0 + external_detection_translation_x_ + 0.2, 0.01); + EXPECT_NEAR(staging_pose_->pose.position.z, 0.1, 0.01); + EXPECT_EQ(staging_pose_->header.frame_id, "odom"); } -TEST_F(TestPantherChargingDock, GetDockPoseFailure) +TEST_F(TestPantherChargingDock, GetStagingPose) { + SetStagingOffsets(); ConfigureAndActivateDock(1.21); - EXPECT_THROW({ charging_dock_->getDockPose("test_dock"); }, std::runtime_error); + + geometry_msgs::msg::PoseStamped pose; + ASSERT_THROW({ charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }, + opennav_docking_core::FailedToDetectDock); + + SetDockToBaseLinkTransform(); + ASSERT_NO_THROW({ pose = charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }); + EXPECT_NEAR(pose.pose.position.x, 1.0, 0.01); + EXPECT_NEAR(pose.pose.position.y, 2.0 + staging_x_offset_, 0.01); + EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 1.57 + staging_yaw_offset_, 0.01); + EXPECT_EQ(pose.header.frame_id, "base_link"); +} + +TEST_F(TestPantherChargingDock, GetRefinedPose) +{ + SetDetectionOffsets(); + ConfigureAndActivateDock(1.21); + + geometry_msgs::msg::PoseStamped pose; + ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); + + SetDockToBaseLinkTransform(); + ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); + + charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + ASSERT_TRUE(charging_dock_->getRefinedPose(pose)); + + 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); + + EXPECT_NEAR(pose.pose.position.x, 1.0 - external_detection_translation_y_, 0.01); + EXPECT_NEAR(pose.pose.position.y, 2.0 + external_detection_translation_x_, 0.01); + EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); + EXPECT_EQ(pose.header.frame_id, "base_link"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); } -// tf2_buffer_ cannot read transformations -// TEST_F(TestPantherChargingDock, GetDockPoseSuccess) -// { -// const double external_detection_translation_x_ = 0.3; -// const double external_detection_translation_y_ = 0.1; -// const double external_detection_translation_z_ = 0.0; -// const double roll = 0.0; -// const double pitch = 0.0; -// const double yaw = 1.57; - -// 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(roll)); -// node_->declare_parameter("test_dock.external_detection_rotation_pitch", rclcpp::ParameterValue(pitch)); -// node_->declare_parameter("test_dock.external_detection_rotation_yaw", rclcpp::ParameterValue(yaw)); - -// ConfigureAndActivateDock(1.21); -// CreateChargerEnableService(true); -// EnableSpinningNode(); -// PublishBaseLinkTransform(); - -// auto new_pose = charging_dock_->getDockPose("test_dock"); - -// tf2::Quaternion external_detection_rotation_; -// external_detection_rotation_.setEuler(yaw, pitch, roll); - -// tf2::Quaternion offset_rotation; -// tf2::fromMsg(new_pose.pose.orientation, offset_rotation); -// EXPECT_NEAR(new_pose.pose.position.x, external_detection_translation_x_, 0.01); -// EXPECT_NEAR(new_pose.pose.position.y, external_detection_translation_y_, 0.01); -// EXPECT_NEAR(new_pose.pose.position.z, 0.0, 0.01); -// EXPECT_EQ(offset_rotation, external_detection_rotation_); -// } - -// TEST_F(TestPantherChargingDock, ConfigureAndActivateOlder) -// { -// ConfigureAndActivateDock(1.06); -// EXPECT_FALSE(charging_dock_->isCharging()); -// EXPECT_TRUE(charging_dock_->disableCharging()); -// EXPECT_TRUE(charging_dock_->hasStoppedCharging()); -// } - -// TEST_F(TestPantherChargingDock, ChargingStateNoServiceFailure) -// { -// ConfigureAndActivateDock(1.21); -// EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); -// EXPECT_FALSE(charging_dock_->disableCharging()); -// EXPECT_THROW({ charging_dock_->hasStoppedCharging(); }, opennav_docking_core::FailedToCharge); -// } - -// TEST_F(TestPantherChargingDock, ChargingStateFailRespondFailure) -// { -// ConfigureAndActivateDock(1.21); -// CreateChargerStatusPublisher(); -// CreateChargerEnableService(false); - -// EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); -// EXPECT_FALSE(charging_dock_->disableCharging()); -// EXPECT_TRUE(charging_dock_->hasStoppedCharging()); -// } - -// TEST_F(TestPantherChargingDock, ChargingStateSuccess) -// { -// ConfigureAndActivateDock(1.21); -// CreateChargerStatusPublisher(); -// CreateChargerEnableService(true); - -// EXPECT_TRUE(charging_dock_->isCharging()); -// EXPECT_TRUE(charging_dock_->disableCharging()); -// EXPECT_TRUE(charging_dock_->hasStoppedCharging()); -// } - -// TEST(PantherChargingDockTests, ObjectLifecycle) -// { -// rclcpp::init(0, nullptr); -// auto node = std::make_shared("panther_charging_dock_test"); -// node->declare_parameter("panther_version", rclcpp::ParameterValue(1.06)); - -// auto dock = std::make_unique(); -// EXPECT_THROW({ dock->configure(node, "my_dock", nullptr); }, std::runtime_error); - -// auto tf2_buffer = std::make_shared(node->get_clock()); -// tf2_buffer->setUsingDedicatedThread(true); - -// EXPECT_NO_THROW({ dock->configure(node, "my_dock", tf2_buffer); }); - -// dock->activate(); -// EXPECT_FALSE(dock->isCharging()); -// EXPECT_TRUE(dock->disableCharging()); -// EXPECT_TRUE(dock->hasStoppedCharging()); - -// dock->deactivate(); -// dock->cleanup(); -// dock.reset(); -// rclcpp::shutdown(); -// } +TEST_F(TestPantherChargingDock, IsDocked) +{ + SetDetectionOffsets(); + ConfigureAndActivateDock(1.21); + + geometry_msgs::msg::PoseStamped pose; + SetDockToBaseLinkTransform(); + SetBaseLinkToOdomTransform(); + charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + ASSERT_FALSE(charging_dock_->isDocked()); + + SetBaseLinkToOdomTransform(); + SetDockToBaseLinkTransform(-external_detection_translation_y_, external_detection_translation_x_, external_detection_translation_z_, -external_detection_yaw_); + charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + + EXPECT_TRUE(charging_dock_->isDocked()); +} + +TEST_F(TestPantherChargingDock, Charging106) +{ + ConfigureAndActivateDock(1.06); + EXPECT_FALSE(charging_dock_->isCharging()); + EXPECT_TRUE(charging_dock_->disableCharging()); + EXPECT_TRUE(charging_dock_->hasStoppedCharging()); +} + +TEST_F(TestPantherChargingDock, ChargingStateNoServiceFailure) +{ + ConfigureAndActivateDock(1.21); + EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); + EXPECT_FALSE(charging_dock_->disableCharging()); + EXPECT_THROW({ charging_dock_->hasStoppedCharging(); }, opennav_docking_core::FailedToCharge); +} + +TEST_F(TestPantherChargingDock, ChargingStateFailRespondFailure) +{ + ConfigureAndActivateDock(1.21); + CreateChargerStatusPublisher(); + CreateChargerEnableService(false); + + EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); + EXPECT_FALSE(charging_dock_->disableCharging()); + EXPECT_TRUE(charging_dock_->hasStoppedCharging()); +} + +TEST_F(TestPantherChargingDock, ChargingStateSuccess) +{ + ConfigureAndActivateDock(1.21); + CreateChargerStatusPublisher(); + CreateChargerEnableService(true); + + EXPECT_TRUE(charging_dock_->isCharging()); + EXPECT_TRUE(charging_dock_->disableCharging()); + EXPECT_TRUE(charging_dock_->hasStoppedCharging()); +} From 4dab7b13d854157a28683ef09a01c576eed6040e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Aug 2024 13:53:32 +0000 Subject: [PATCH 04/23] Added redme Signed-off-by: Jakub Delicat --- panther_docking/README.md | 53 ++++++++++++- .../launch/apriltag_linux_cam.launch.py | 75 ------------------- panther_docking/launch/docking.launch.py | 56 +++++++------- panther_docking/src/panther_charging_dock.cpp | 3 +- 4 files changed, 82 insertions(+), 105 deletions(-) delete mode 100644 panther_docking/launch/apriltag_linux_cam.launch.py diff --git a/panther_docking/README.md b/panther_docking/README.md index 88583c682..b60c0bfa0 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -1,3 +1,50 @@ -ros2 action send_goal /panther/dock_robot opennav_docking_msgs/action/DockRobot "{dock_id: main_dock, navigate_to_staging_pose: false}" -ros2 launch panther_bringup bringup.launch.py namespace:=panther components_config_path:=$(pwd)/gazebo_components.yaml -PANTHER_ROBOT_VERSION=1.21 ros2 launch panther_docking docking.launch.py namespace:=panther +# panther_docking + +Package contains a `PantherChargingDock` plugin for [opennav_docking](https://github.com/open-navigation/opennav_docking) project. Thanks to this package Panther can dock to a charging station. + +## Launch Files + +- `docking.launch.py.launch.py`: Launch a node that creates `docking_server` and run a `PantherChargingDock` plugin. + +## Configuration Files + +- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. + +## ROS Nodes + +- [`docking_server`](https://github.com/open-navigation/opennav_docking): An action server which implements charger docking node for AMRs. +- `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. + +### PantherChargingDock + +#### 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. + +#### Service Clients + +- `hardware/charger_enable` [*std_srvs/SetBoot*]: This service client enables charging in a robot. + +#### Parameters + +- `~panther_version` [*double*, default: **1.21**]: A version of Panther robot. +- `~.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 an 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 an Z axis between a detected frame and a dock pose. +- `~.filter_coef` [*double*, default: **0.1**]: # TODO: @delihus +- `~.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 an 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. diff --git a/panther_docking/launch/apriltag_linux_cam.launch.py b/panther_docking/launch/apriltag_linux_cam.launch.py deleted file mode 100644 index 561e54988..000000000 --- a/panther_docking/launch/apriltag_linux_cam.launch.py +++ /dev/null @@ -1,75 +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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import ( - EnvironmentVariable, - 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_sim = LaunchConfiguration("use_sim") - declare_use_sim_arg = DeclareLaunchArgument( - "use_sim", - default_value="False", - description="Whether simulation is used", - choices=["True", "False"], - ) - - namespace = LaunchConfiguration("namespace") - declare_namespace_arg = DeclareLaunchArgument( - "namespace", - default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), - description="Add namespace to all launched nodes.", - ) - - apriltag_config_path = LaunchConfiguration("apriltag_config_path") - apriltag_config_path_arg = DeclareLaunchArgument( - "apriltag_config_path", - default_value=PathJoinSubstitution( - [FindPackageShare("panther_docking"), "config", "apriltag.yaml"] - ), - description=("Path to apriltag configuration file."), - ) - - namespaced_apriltag_config_path = ReplaceString( - source_file=apriltag_config_path, - replacements={"": namespace, "//": "/"}, - ) - - return LaunchDescription( - [ - declare_use_sim_arg, - declare_namespace_arg, - apriltag_config_path_arg, - 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(), - ), - ] - ) diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 4f9fde75c..112966f89 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -56,36 +56,40 @@ def generate_launch_description(): replacements={"": namespace, "//": "/"}, ) + docking_server = Node( + package="opennav_docking", + executable="opennav_docking", + parameters=[ + {"panther_version": panther_version}, + namespaced_docking_server_config, + {"use_sim_time": True}, + ], + namespace=namespace, + emulate_tty=True, + ) + + docking_server_activate = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="nav2_docking_lifecycle_manager", + parameters=[ + { + "autostart": True, + "node_names": [ + "docking_server", + ], + "use_sim_time": use_sim, + }, + ], + namespace=namespace, + ) + return LaunchDescription( [ declare_use_sim_arg, declare_namespace_arg, declare_docking_server_path_arg, - Node( - package="opennav_docking", - executable="opennav_docking", - parameters=[ - {"panther_version": panther_version}, - namespaced_docking_server_config, - {"use_sim_time": True}, - ], - namespace=namespace, - emulate_tty=True, - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="nav2_docking_lifecycle_manager", - parameters=[ - { - "autostart": True, - "node_names": [ - "docking_server", - ], - "use_sim_time": use_sim, - }, - ], - namespace=namespace, - ), + docking_server, + docking_server_activate, ] ) diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 9f00fe827..cc44169e6 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -43,7 +43,7 @@ void PantherChargingDock::configure(const rclcpp_lifecycle::LifecycleNode::WeakP 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.0)); + 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", @@ -75,6 +75,7 @@ void PantherChargingDock::configure(const rclcpp_lifecycle::LifecycleNode::WeakP 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); From a5828844c8128872358b536c853af6a9f7b8cee7 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Aug 2024 16:10:36 +0200 Subject: [PATCH 05/23] run precommit Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 10 +- .../panther_docking/panther_charging_dock.hpp | 38 +-- panther_docking/src/panther_charging_dock.cpp | 284 ++++++++---------- .../test/test_panther_charging_dock.cpp | 157 +++++----- 4 files changed, 243 insertions(+), 246 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index d2d860e30..60f25eb1e 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -37,13 +37,15 @@ target_include_directories(${library_name} pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock test/test_panther_charging_dock.cpp) - target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock ${library_name} ) - ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock ${PACKAGE_DEPENDENCIES}) + ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock + test/test_panther_charging_dock.cpp) + target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock + ${library_name}) + ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock + ${PACKAGE_DEPENDENCIES}) endif() install(DIRECTORY include/ DESTINATION include/) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 946364117..25aff97e8 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -45,17 +45,16 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock public: using Ptr = std::shared_ptr; - PantherChargingDock() - { - } + PantherChargingDock() {} /** * @param parent pointer to user's node * @param name The name of this planner * @param tf A pointer to a TF buffer */ - virtual void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent, const std::string& name, - std::shared_ptr tf) override final; + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, + std::shared_ptr tf) override final; /** * @brief Method to cleanup resources used on shutdown. @@ -80,14 +79,14 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * @param frame Dock's frame of pose * @return PoseStamped of staging pose in the specified frame */ - virtual geometry_msgs::msg::PoseStamped getStagingPose(const geometry_msgs::msg::Pose& pose, - const std::string& frame) override final; + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) override final; /** * @brief Method to obtain the refined pose of the dock, usually based on sensors * @param pose The initial estimate of the dock pose. */ - virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped& pose) override final; + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose) override final; /** * @brief Have we made contact with dock? This can be implemented in a variety @@ -146,8 +145,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @return The transformed pose. */ - geometry_msgs::msg::PoseStamped transformPose(const geometry_msgs::msg::PoseStamped& pose, - const std::string& target_frame); + geometry_msgs::msg::PoseStamped transformPose( + const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame); /** * @brief Offset the staging pose. @@ -158,7 +157,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @return The offset staging pose. */ - geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose(const geometry_msgs::msg::PoseStamped& dock_pose); + geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose( + const geometry_msgs::msg::PoseStamped & dock_pose); /** * @brief Offset the detected dock pose. @@ -169,7 +169,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @return The offset detected dock pose. */ - geometry_msgs::msg::PoseStamped offsetDetectedDockPose(const geometry_msgs::msg::PoseStamped& detected_dock_pose); + geometry_msgs::msg::PoseStamped offsetDetectedDockPose( + const geometry_msgs::msg::PoseStamped & detected_dock_pose); /** * @brief Get the dock pose in a detection dock frame. @@ -179,7 +180,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * @param frame The detection frame to get the dock pose in. * @return The dock pose in the detection frame. */ - geometry_msgs::msg::PoseStamped getDockPose(const std::string& frame); + geometry_msgs::msg::PoseStamped getDockPose(const std::string & frame); /** * @brief Method to update the dock pose and publish it. @@ -196,15 +197,15 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @param frame The frame to publish the staging pose in. */ - void updateStagingPoseAndPublish(const std::string& frame); + void updateStagingPoseAndPublish(const std::string & frame); std::string name_; std::string base_frame_name_; std::string dock_frame_; float panther_version_; - rclcpp::Logger logger_{ rclcpp::get_logger("PantherChargingDock") }; - rclcpp::Clock steady_clock_{ RCL_STEADY_TIME }; + rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; + rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; rclcpp_lifecycle::LifecycleNode::SharedPtr node_; std::shared_ptr tf2_buffer_; rclcpp::Subscription::SharedPtr charging_status_sub_; @@ -213,8 +214,9 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp::Publisher::SharedPtr dock_pose_pub_; rclcpp::Client::SharedPtr charger_enable_client_; - realtime_tools::RealtimeBox> charging_status_box_{ nullptr }; - realtime_tools::RealtimeBox> io_state_box_{ nullptr }; + realtime_tools::RealtimeBox> + charging_status_box_{nullptr}; + realtime_tools::RealtimeBox> io_state_box_{nullptr}; geometry_msgs::msg::PoseStamped dock_pose_; geometry_msgs::msg::PoseStamped staging_pose_; diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index cc44169e6..325f87e6f 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -21,60 +21,68 @@ namespace panther_docking { -void PantherChargingDock::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent, const std::string& name, - std::shared_ptr tf) +void PantherChargingDock::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, + std::shared_ptr tf) { name_ = name; - if (!tf) - { + if (!tf) { throw std::runtime_error("PantherChargingDock requires a TF buffer"); } tf2_buffer_ = tf; node_ = parent.lock(); - if (!node_) - { + if (!node_) { throw std::runtime_error("Failed to lock node"); } - nav2_util::declare_parameter_if_not_declared(node_, "panther_version", rclcpp::ParameterValue(1.0)); - - 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)); - nav2_util::declare_parameter_if_not_declared(node_, name + ".external_detection_rotation_roll", - rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared(node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); - - nav2_util::declare_parameter_if_not_declared(node_, name + ".docking_distance_threshold", - rclcpp::ParameterValue(0.05)); - nav2_util::declare_parameter_if_not_declared(node_, name + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); - - 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 + ".enable_charger_service_call_timeout", - rclcpp::ParameterValue(0.2)); + nav2_util::declare_parameter_if_not_declared( + node_, "panther_version", rclcpp::ParameterValue(1.0)); + + 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)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared( + node_, name + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); + + 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 + ".enable_charger_service_call_timeout", rclcpp::ParameterValue(0.2)); node_->get_parameter("panther_version", panther_version_); node_->get_parameter(name + ".base_frame", base_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_); + 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); @@ -87,7 +95,8 @@ void PantherChargingDock::configure(const rclcpp_lifecycle::LifecycleNode::WeakP node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); - node_->get_parameter(name + ".enable_charger_service_call_timeout", enable_charger_service_call_timeout_); + node_->get_parameter( + name + ".enable_charger_service_call_timeout", enable_charger_service_call_timeout_); double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); @@ -99,8 +108,7 @@ void PantherChargingDock::cleanup() dock_pose_pub_.reset(); staging_pose_pub_.reset(); - if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { + if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { charging_status_sub_.reset(); io_state_sub_.reset(); charger_enable_client_.reset(); @@ -111,27 +119,30 @@ void PantherChargingDock::activate() { dock_pose_pub_ = node_->create_publisher("docking/dock_pose", 1); - staging_pose_pub_ = node_->create_publisher("docking/staging_pose", 1); + staging_pose_pub_ = node_->create_publisher( + "docking/staging_pose", 1); - if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { + if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { charging_status_sub_ = node_->create_subscription( - "battery/charging_status", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { - charging_status_box_.set(std::move(msg)); - }); + "battery/charging_status", rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + charging_status_box_.set(std::move(msg)); + }); io_state_sub_ = node_->create_subscription( - "hardware/io_state", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { io_state_box_.set(std::move(msg)); }); + "hardware/io_state", rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + io_state_box_.set(std::move(msg)); + }); - charger_enable_client_ = node_->create_client("hardware/charger_enable"); + charger_enable_client_ = + node_->create_client("hardware/charger_enable"); using namespace std::chrono_literals; - if (!charger_enable_client_->wait_for_service(1s)) - { - RCLCPP_WARN_STREAM(logger_, - "Service \"hardware/charger_enable\" not available. Make sure if Panther ROS 2 Stack is " - "running."); + if (!charger_enable_client_->wait_for_service(1s)) { + RCLCPP_WARN_STREAM( + logger_, + "Service \"hardware/charger_enable\" not available. Make sure if Panther ROS 2 Stack is " + "running."); } } } @@ -141,28 +152,23 @@ void PantherChargingDock::deactivate() dock_pose_pub_.reset(); staging_pose_pub_.reset(); - if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { + if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { charging_status_sub_.reset(); io_state_sub_.reset(); charger_enable_client_.reset(); } } -geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose(const geometry_msgs::msg::Pose& pose, - const std::string& frame) +geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) { // When the pose if default the robot is docking so the frame is the dock frame - if (pose == geometry_msgs::msg::Pose()) - { + if (pose == geometry_msgs::msg::Pose()) { dock_frame_ = frame; updateDockPoseAndPublish(); updateStagingPoseAndPublish(base_frame_name_); - } - else - { - if (dock_frame_.empty()) - { + } else { + if (dock_frame_.empty()) { throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); } updateDockPoseAndPublish(); @@ -172,16 +178,13 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose(const geomet return staging_pose_; } -bool PantherChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped& pose) +bool PantherChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) { - try - { + try { updateDockPoseAndPublish(); updateStagingPoseAndPublish(base_frame_name_); pose = dock_pose_; - } - catch (const opennav_docking_core::DockingException &e) - { + } catch (const opennav_docking_core::DockingException & e) { RCLCPP_ERROR_STREAM(logger_, "An occurred error while getting refined pose: " << e.what()); return false; } @@ -202,34 +205,27 @@ bool PantherChargingDock::isCharging() { std::shared_ptr charging_status_msg; - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { - try - { + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + try { return isDocked(); - } - catch (const opennav_docking_core::FailedToDetectDock& e) - { + } catch (const opennav_docking_core::FailedToDetectDock & e) { return false; } } - // TODO: This might be used whe is undocking but we not want to enable charging when the robot is undocking - try - { + // TODO: This might be used when a robot is undocking but we do not want to enable charging when + // the robot is undocking + try { setChargerState(true); - } - catch (const std::runtime_error& e) - { - throw opennav_docking_core::FailedToCharge("An exception occurred while enabling charging: " + - std::string(e.what())); + } catch (const std::runtime_error & e) { + throw opennav_docking_core::FailedToCharge( + "An exception occurred while enabling charging: " + std::string(e.what())); } // CAUTION: The conteroller frequency can be higher than the message frequency charging_status_box_.get(charging_status_msg); - if (!charging_status_msg) - { + if (!charging_status_msg) { throw opennav_docking_core::FailedToCharge("Did not receive charging_status_msg message"); } @@ -238,17 +234,13 @@ bool PantherChargingDock::isCharging() bool PantherChargingDock::disableCharging() { - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { return true; } - try - { + try { setChargerState(false); - } - catch (const std::runtime_error& e) - { + } catch (const std::runtime_error & e) { RCLCPP_ERROR_STREAM(logger_, "An exception occurred while disabling charging: " << e.what()); return false; } @@ -258,35 +250,28 @@ bool PantherChargingDock::disableCharging() bool PantherChargingDock::hasStoppedCharging() { - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { return !isCharging(); } std::shared_ptr charging_status_msg; charging_status_box_.get(charging_status_msg); - if (!charging_status_msg) - { + if (!charging_status_msg) { throw opennav_docking_core::FailedToCharge("Did not receive charging_status_msg message"); } - if (charging_status_msg->charging) - { + if (charging_status_msg->charging) { throw opennav_docking_core::FailedToCharge("Charging status is still true"); } return true; } -std::string PantherChargingDock::getName() -{ - return name_; -} +std::string PantherChargingDock::getName() { return name_; } void PantherChargingDock::setChargerState(bool state) { - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) - { + if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { throw std::runtime_error("This version of Panther does not support charger control"); } @@ -297,33 +282,34 @@ void PantherChargingDock::setChargerState(bool state) const auto timeout = std::chrono::duration(enable_charger_service_call_timeout_); // This doubles spinning the node because the node is spinning in docking_server - if (rclcpp::spin_until_future_complete(node_, result, timeout) != rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error("Failed to call charger enable service"); } - if (!result.get()->success) - { + if (!result.get()->success) { throw std::runtime_error("Failed to enable charger"); } } -geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose(const geometry_msgs::msg::PoseStamped& pose, - const std::string& target_frame) +geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose( + const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame) { geometry_msgs::msg::PoseStamped transformed_pose; - if (pose.header.frame_id.empty() || target_frame.empty()) - { - throw std::runtime_error("Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + - "\", target frame: \"" + target_frame + "\""); + if (pose.header.frame_id.empty() || target_frame.empty()) { + throw std::runtime_error( + "Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + + "\", target frame: \"" + target_frame + "\""); } - if (!tf2_buffer_->canTransform(pose.header.frame_id, target_frame, pose.header.stamp, - rclcpp::Duration::from_seconds(external_detection_timeout_))) - { - throw std::runtime_error("Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + - std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); + if (!tf2_buffer_->canTransform( + pose.header.frame_id, target_frame, pose.header.stamp, + rclcpp::Duration::from_seconds(external_detection_timeout_))) { + throw std::runtime_error( + "Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + + std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); } tf2_buffer_->transform(pose, transformed_pose, target_frame); @@ -331,8 +317,8 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose(const geometr return transformed_pose; } -geometry_msgs::msg::PoseStamped -PantherChargingDock::offsetStagingPoseToDockPose(const geometry_msgs::msg::PoseStamped& dock_pose) +geometry_msgs::msg::PoseStamped PantherChargingDock::offsetStagingPoseToDockPose( + const geometry_msgs::msg::PoseStamped & dock_pose) { tf2::Transform dock_pose_transform; tf2::fromMsg(dock_pose.pose, dock_pose_transform); @@ -357,8 +343,8 @@ PantherChargingDock::offsetStagingPoseToDockPose(const geometry_msgs::msg::PoseS return staging_pose; } -geometry_msgs::msg::PoseStamped -PantherChargingDock::offsetDetectedDockPose(const geometry_msgs::msg::PoseStamped& detected_dock_pose) +geometry_msgs::msg::PoseStamped PantherChargingDock::offsetDetectedDockPose( + const geometry_msgs::msg::PoseStamped & detected_dock_pose) { geometry_msgs::msg::PoseStamped just_orientation; just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); @@ -367,8 +353,9 @@ PantherChargingDock::offsetDetectedDockPose(const geometry_msgs::msg::PoseStampe tf2::doTransform(just_orientation, just_orientation, transform); - tf2::Quaternion orientation(just_orientation.pose.orientation.x, just_orientation.pose.orientation.y, - just_orientation.pose.orientation.z, just_orientation.pose.orientation.w); + tf2::Quaternion orientation( + just_orientation.pose.orientation.x, just_orientation.pose.orientation.y, + just_orientation.pose.orientation.z, just_orientation.pose.orientation.w); geometry_msgs::msg::PoseStamped offset_detected_dock_pose = detected_dock_pose; offset_detected_dock_pose.pose.orientation = tf2::toMsg(orientation); @@ -381,24 +368,23 @@ PantherChargingDock::offsetDetectedDockPose(const geometry_msgs::msg::PoseStampe return offset_detected_dock_pose; } -geometry_msgs::msg::PoseStamped PantherChargingDock::getDockPose(const std::string& frame) +geometry_msgs::msg::PoseStamped PantherChargingDock::getDockPose(const std::string & frame) { geometry_msgs::msg::PoseStamped filtered_offset_detected_dock_pose; - try - { + try { geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = frame; pose.header.stamp = node_->get_clock()->now(); auto offset_detected_dock_pose = offsetDetectedDockPose(pose); filtered_offset_detected_dock_pose = filter_->update(offset_detected_dock_pose); - filtered_offset_detected_dock_pose = transformPose(filtered_offset_detected_dock_pose, base_frame_name_); + filtered_offset_detected_dock_pose = transformPose( + filtered_offset_detected_dock_pose, base_frame_name_); 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())); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "An exception occurred while getting dock pose: " + std::string(e.what())); } return filtered_offset_detected_dock_pose; @@ -406,31 +392,25 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::getDockPose(const std::stri void PantherChargingDock::updateDockPoseAndPublish() { - try - { + 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())); + } 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) +void PantherChargingDock::updateStagingPoseAndPublish(const std::string & frame) { - try - { + try { auto new_staging_pose = offsetStagingPoseToDockPose(dock_pose_); staging_pose_ = transformPose(new_staging_pose, frame); 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())); + } catch (const std::runtime_error & e) { + throw opennav_docking_core::FailedToStage( + "An exception occurred while transforming staging pose: " + std::string(e.what())); } } diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp index 9dc87a954..82876124a 100644 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/joint_state.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/static_transform_broadcaster.h" @@ -30,31 +30,28 @@ class PantherChargingDockWrapper : public panther_docking::PantherChargingDock { public: using SharedPtr = std::shared_ptr; - PantherChargingDockWrapper() : panther_docking::PantherChargingDock() - { - } + PantherChargingDockWrapper() : panther_docking::PantherChargingDock() {} - void setChargerState(bool state) - { - panther_docking::PantherChargingDock::setChargerState(state); - } + void setChargerState(bool state) { panther_docking::PantherChargingDock::setChargerState(state); } - geometry_msgs::msg::PoseStamped transformPose(const geometry_msgs::msg::PoseStamped& pose, - const std::string& target_frame) + geometry_msgs::msg::PoseStamped transformPose( + const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame) { return panther_docking::PantherChargingDock::transformPose(pose, target_frame); } - geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose(const geometry_msgs::msg::PoseStamped& dock_pose) + 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) + 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) + geometry_msgs::msg::PoseStamped getDockPose(const std::string & frame) { return panther_docking::PantherChargingDock::getDockPose(frame); } @@ -63,7 +60,7 @@ class PantherChargingDockWrapper : public panther_docking::PantherChargingDock panther_docking::PantherChargingDock::updateDockPoseAndPublish(); } - void updateStagingPoseAndPublish(const std::string& frame) + void updateStagingPoseAndPublish(const std::string & frame) { panther_docking::PantherChargingDock::updateStagingPoseAndPublish(frame); } @@ -81,7 +78,8 @@ class TestPantherChargingDock : public testing::Test void CreateChargerStatusPublisher(); void PublishChargerStatus(bool charging); void SetBaseLinkToOdomTransform(); - void SetDockToBaseLinkTransform(double x = 1.0, double y = 2.0, double z = 3.0, double yaw = 1.57); + void SetDockToBaseLinkTransform( + double x = 1.0, double y = 2.0, double z = 3.0, double yaw = 1.57); void SetDetectionOffsets(); void SetStagingOffsets(); @@ -116,14 +114,12 @@ TestPantherChargingDock::TestPantherChargingDock() TestPantherChargingDock::~TestPantherChargingDock() { - if (charging_dock_) - { + if (charging_dock_) { charging_dock_->deactivate(); charging_dock_->cleanup(); } - if (executor_) - { + if (executor_) { executor_->cancel(); } @@ -137,10 +133,12 @@ void TestPantherChargingDock::ConfigureAndActivateDock(double panther_version) 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; }); + "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; }); + "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); @@ -154,29 +152,26 @@ void TestPantherChargingDock::ConfigureAndActivateDock(double panther_version) void TestPantherChargingDock::CreateChargerEnableService(bool success) { charger_enable_service_ = node_->create_service( - "hardware/charger_enable", [success, this](const std::shared_ptr request, - std::shared_ptr response) { - response->success = success; - if (success) - { - this->charging_status_ = request->data; - } - else - { - this->charging_status_ = false; - } - - if (this->charger_state_pub_) - { - PublishChargerStatus(this->charging_status_); - } - }); + "hardware/charger_enable", [success, this]( + const std::shared_ptr request, + std::shared_ptr response) { + response->success = success; + if (success) { + this->charging_status_ = request->data; + } else { + this->charging_status_ = false; + } + + if (this->charger_state_pub_) { + PublishChargerStatus(this->charging_status_); + } + }); } void TestPantherChargingDock::CreateChargerStatusPublisher() { - charger_state_pub_ = - node_->create_publisher("battery/charging_status", rclcpp::QoS(1)); + charger_state_pub_ = node_->create_publisher( + "battery/charging_status", rclcpp::QoS(1)); } void TestPantherChargingDock::PublishChargerStatus(bool charging) @@ -222,24 +217,29 @@ void TestPantherChargingDock::SetDockToBaseLinkTransform(double x, double y, dou 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_)); + 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_)); + node_->declare_parameter( + "test_dock.staging_yaw_offset", rclcpp::ParameterValue(staging_yaw_offset_)); } TEST_F(TestPantherChargingDock, SetChargerStateOlderFailure) @@ -320,7 +320,8 @@ TEST_F(TestPantherChargingDock, offsetDetectedDockPose) tf2::Quaternion external_detection_rotation; - external_detection_rotation.setRPY(external_detection_roll_, external_detection_pitch_, external_detection_yaw_); + external_detection_rotation.setRPY( + external_detection_roll_, external_detection_pitch_, external_detection_yaw_); tf2::Quaternion offset_rotation; tf2::fromMsg(new_pose.pose.orientation, offset_rotation); @@ -345,8 +346,8 @@ TEST_F(TestPantherChargingDock, GetDockPose) 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); + external_detection_rotation_.setRPY( + external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); EXPECT_NEAR(pose.pose.position.x, 1.0 - external_detection_translation_y_, 0.01); EXPECT_NEAR(pose.pose.position.y, 2.0 + external_detection_translation_x_, 0.01); @@ -370,22 +371,27 @@ TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) charging_dock_->updateStagingPoseAndPublish("base_link"); }); - 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_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))); 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); + external_detection_rotation_.setRPY( + external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); EXPECT_NEAR(dock_pose_->pose.position.x, 1.0 - external_detection_translation_y_, 0.01); EXPECT_NEAR(dock_pose_->pose.position.y, 2.0 + external_detection_translation_x_, 0.01); EXPECT_NEAR(dock_pose_->pose.position.z, 0.0, 0.01); EXPECT_EQ(dock_pose_->header.frame_id, "base_link"); - EXPECT_NEAR(tf2::getYaw(dock_pose_->pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); + EXPECT_NEAR( + tf2::getYaw(dock_pose_->pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); - EXPECT_NEAR(staging_pose_->pose.position.x, 1.0 - external_detection_translation_y_ - staging_x_offset_, 0.01); + EXPECT_NEAR( + staging_pose_->pose.position.x, 1.0 - external_detection_translation_y_ - staging_x_offset_, + 0.01); EXPECT_NEAR(staging_pose_->pose.position.y, 2.0 + external_detection_translation_x_, 0.01); EXPECT_NEAR(staging_pose_->pose.position.z, 0.0, 0.01); EXPECT_EQ(staging_pose_->header.frame_id, "base_link"); @@ -394,9 +400,12 @@ TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) EXPECT_NEAR(tf2::getYaw(staging_pose_->pose.orientation), -1.57, 0.01); ASSERT_NO_THROW({ charging_dock_->updateStagingPoseAndPublish("odom"); }); - ASSERT_TRUE(panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); + ASSERT_TRUE( + panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); - EXPECT_NEAR(staging_pose_->pose.position.x, 1.0 - external_detection_translation_y_ - staging_x_offset_ + 0.3, 0.01); + EXPECT_NEAR( + staging_pose_->pose.position.x, + 1.0 - external_detection_translation_y_ - staging_x_offset_ + 0.3, 0.01); EXPECT_NEAR(staging_pose_->pose.position.y, 2.0 + external_detection_translation_x_ + 0.2, 0.01); EXPECT_NEAR(staging_pose_->pose.position.z, 0.1, 0.01); EXPECT_EQ(staging_pose_->header.frame_id, "odom"); @@ -408,11 +417,13 @@ TEST_F(TestPantherChargingDock, GetStagingPose) ConfigureAndActivateDock(1.21); geometry_msgs::msg::PoseStamped pose; - ASSERT_THROW({ charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }, - opennav_docking_core::FailedToDetectDock); + ASSERT_THROW( + { charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }, + opennav_docking_core::FailedToDetectDock); SetDockToBaseLinkTransform(); - ASSERT_NO_THROW({ pose = charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }); + ASSERT_NO_THROW( + { pose = charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }); EXPECT_NEAR(pose.pose.position.x, 1.0, 0.01); EXPECT_NEAR(pose.pose.position.y, 2.0 + staging_x_offset_, 0.01); EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); @@ -436,8 +447,8 @@ TEST_F(TestPantherChargingDock, GetRefinedPose) 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); + external_detection_rotation_.setRPY( + external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); EXPECT_NEAR(pose.pose.position.x, 1.0 - external_detection_translation_y_, 0.01); EXPECT_NEAR(pose.pose.position.y, 2.0 + external_detection_translation_x_, 0.01); @@ -458,7 +469,9 @@ TEST_F(TestPantherChargingDock, IsDocked) ASSERT_FALSE(charging_dock_->isDocked()); SetBaseLinkToOdomTransform(); - SetDockToBaseLinkTransform(-external_detection_translation_y_, external_detection_translation_x_, external_detection_translation_z_, -external_detection_yaw_); + SetDockToBaseLinkTransform( + -external_detection_translation_y_, external_detection_translation_x_, + external_detection_translation_z_, -external_detection_yaw_); charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); EXPECT_TRUE(charging_dock_->isDocked()); From 058471954c538d387de9308602c880ed3ce0d223 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Aug 2024 16:15:37 +0200 Subject: [PATCH 06/23] Fix typo Signed-off-by: Jakub Delicat --- panther_docking/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index b60c0bfa0..e1527f659 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -4,7 +4,7 @@ Package contains a `PantherChargingDock` plugin for [opennav_docking](https://gi ## Launch Files -- `docking.launch.py.launch.py`: Launch a node that creates `docking_server` and run a `PantherChargingDock` plugin. +- `docking.launch.py`: Launch a node that creates `docking_server` and run a `PantherChargingDock` plugin. ## Configuration Files From ea582cf8ad3a070f8e648c41cdfa0090494046ad Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Aug 2024 14:52:10 +0000 Subject: [PATCH 07/23] After coderabbit review Signed-off-by: Jakub Delicat --- panther_docking/README.md | 4 ++-- .../include/panther_docking/panther_charging_dock.hpp | 2 +- panther_docking/src/panther_charging_dock.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index e1527f659..8221e6915 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -1,6 +1,6 @@ # panther_docking -Package contains a `PantherChargingDock` plugin for [opennav_docking](https://github.com/open-navigation/opennav_docking) project. Thanks to this package Panther can dock to a charging station. +The package contains a `PantherChargingDock` plugin for the [opennav_docking](https://github.com/open-navigation/opennav_docking) project. Thanks to this package, Panther can dock to a charging station. ## Launch Files @@ -44,7 +44,7 @@ Package contains a `PantherChargingDock` plugin for [opennav_docking](https://gi - `~.external_detection_rotation_yaw` [*double*, default: **0.0**]: A rotation over an Z axis between a detected frame and a dock pose. - `~.filter_coef` [*double*, default: **0.1**]: # TODO: @delihus - `~.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 an yaw angles 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. diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 25aff97e8..183fb1b6a 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -202,7 +202,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock std::string name_; std::string base_frame_name_; std::string dock_frame_; - float panther_version_; + double panther_version_; rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 325f87e6f..c074eb7e8 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -222,7 +222,7 @@ bool PantherChargingDock::isCharging() "An exception occurred while enabling charging: " + std::string(e.what())); } - // CAUTION: The conteroller frequency can be higher than the message frequency + // CAUTION: The controller frequency can be higher than the message frequency charging_status_box_.get(charging_status_msg); if (!charging_status_msg) { From cc093cedaf69e3c0c4fffd737184a9748d6b8a2f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 20 Aug 2024 17:47:20 +0000 Subject: [PATCH 08/23] fixed parameters | fixed exporting_package Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 19 ++++++++++++++----- panther_docking/README.md | 4 ++-- .../config/panther_docking_server.yaml | 12 ++++++------ panther_docking/package.xml | 1 + panther_docking/plugin.xml | 4 ++-- 5 files changed, 25 insertions(+), 15 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 60f25eb1e..38539ecb8 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -25,15 +25,16 @@ endforeach() include_directories(include) -set(library_name ${PROJECT_NAME}_battery_state_dock) +set(library_name ${PROJECT_NAME}_panther_charging_dock) add_library(${library_name} SHARED src/panther_charging_dock.cpp) # TODO @delihus how to link the library what is not a name of a package target_link_libraries(${library_name} /opt/ros/humble/lib/libpose_filter.so) -target_include_directories(${library_name} - PUBLIC ${CMAKE_INSTALL_PREFIX}/include) +target_include_directories( + ${library_name} PUBLIC $ + $) pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) @@ -50,8 +51,16 @@ endif() install(DIRECTORY include/ DESTINATION include/) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) +install( + TARGETS ${library_name} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) -ament_export_include_directories(include) -ament_export_libraries(panther_charging_dock ${library_name}) ament_target_dependencies(${library_name} ${PACKAGE_DEPENDENCIES}) +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_targets(export_${PROJECT_NAME}) + ament_package() diff --git a/panther_docking/README.md b/panther_docking/README.md index 8221e6915..e92ae8ce1 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -38,10 +38,10 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `~.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 an Z 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 an Z 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**]: # TODO: @delihus - `~.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. diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index cb62a177f..6cd5fad0e 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -13,8 +13,8 @@ dock_prestaging_tolerance: 0.5 - dock_plugins: ["panther_docking_dock"] - panther_docking_dock: + dock_plugins: ["panther_docking_panther_charging_dock"] + panther_docking_panther_charging_dock: plugin: panther_docking::PantherChargingDock base_frame: "/base_link" docking_distance_threshold: 0.15 @@ -28,16 +28,16 @@ external_detection_translation_x: 0.0 external_detection_translation_y: -0.175 # Distance between the detection and ground external_detection_translation_z: 0.8 # Distance between the detection and the front of the robot - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: 0.0 - external_detection_rotation_yaw: 1.57 + external_detection_rotation_roll: 0.0 + external_detection_rotation_pitch: 1.57 + external_detection_rotation_yaw: 0.0 filter_coef: 0.1 enable_charger_service_call_timeout: 1.0 docks: ["main_dock"] main_dock: - type: "panther_docking_dock" + type: panther_docking_panther_charging_dock frame: /main_dock 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 diff --git a/panther_docking/package.xml b/panther_docking/package.xml index ae3377b8f..18738c6fe 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -15,6 +15,7 @@ Jakub Delicat geometry_msgs + nav2_lifecycle_manager nav2_util opennav_docking panther_msgs diff --git a/panther_docking/plugin.xml b/panther_docking/plugin.xml index ef3b789db..a84a458d7 100644 --- a/panther_docking/plugin.xml +++ b/panther_docking/plugin.xml @@ -1,6 +1,6 @@ - - + A dock plugin interface for Panther robot From 5634f151d540baa5a6a52d6982553935a9cfd715 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 22 Aug 2024 10:05:37 +0000 Subject: [PATCH 09/23] Adjust cmake Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 44 +++++++------------ .../config/panther_docking_server.yaml | 6 +-- panther_docking/plugin.xml | 2 +- 3 files changed, 20 insertions(+), 32 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 38539ecb8..160c2ce60 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -7,17 +7,17 @@ endif() set(PACKAGE_DEPENDENCIES ament_cmake - rclcpp - panther_msgs geometry_msgs - sensor_msgs - std_srvs - realtime_tools opennav_docking_core opennav_docking + panther_msgs + panther_utils pluginlib - tf2_ros - panther_utils) + rclcpp + realtime_tools + sensor_msgs + std_srvs + tf2_ros) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) @@ -25,18 +25,17 @@ endforeach() include_directories(include) -set(library_name ${PROJECT_NAME}_panther_charging_dock) - -add_library(${library_name} SHARED src/panther_charging_dock.cpp) +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(${library_name} /opt/ros/humble/lib/libpose_filter.so) +target_link_libraries(panther_charging_dock + /opt/ros/humble/lib/libpose_filter.so) -target_include_directories( - ${library_name} PUBLIC $ - $) +install(TARGETS panther_charging_dock LIBRARY DESTINATION lib) -pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -44,23 +43,12 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock test/test_panther_charging_dock.cpp) target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock - ${library_name}) + panther_charging_dock) ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock ${PACKAGE_DEPENDENCIES}) endif() -install(DIRECTORY include/ DESTINATION include/) -install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) -install( - TARGETS ${library_name} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_target_dependencies(${library_name} ${PACKAGE_DEPENDENCIES}) ament_export_include_directories(include) -ament_export_libraries(${library_name}) -ament_export_targets(export_${PROJECT_NAME}) +ament_export_libraries(panther_charging_dock) ament_package() diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 6cd5fad0e..42ef82e1f 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -13,8 +13,8 @@ dock_prestaging_tolerance: 0.5 - dock_plugins: ["panther_docking_panther_charging_dock"] - panther_docking_panther_charging_dock: + dock_plugins: ["panther_charging_dock"] + panther_charging_dock: plugin: panther_docking::PantherChargingDock base_frame: "/base_link" docking_distance_threshold: 0.15 @@ -37,7 +37,7 @@ docks: ["main_dock"] main_dock: - type: panther_docking_panther_charging_dock + type: panther_charging_dock frame: /main_dock 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 diff --git a/panther_docking/plugin.xml b/panther_docking/plugin.xml index a84a458d7..91823ea7b 100644 --- a/panther_docking/plugin.xml +++ b/panther_docking/plugin.xml @@ -1,5 +1,5 @@ - + A dock plugin interface for Panther robot From 7fdd0907850c6bd773462c1b22061fbe09c41a4c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 22 Aug 2024 11:52:20 +0000 Subject: [PATCH 10/23] Added getParameters and declareParameters | review suggestions Signed-off-by: Jakub Delicat --- panther_docking/README.md | 1 - .../panther_docking/panther_charging_dock.hpp | 99 +++++---- panther_docking/launch/docking.launch.py | 18 +- panther_docking/src/panther_charging_dock.cpp | 192 +++++++++--------- .../panther_utils/common_utilities.hpp | 2 +- panther_utils/test/test_common_utilities.cpp | 12 ++ 6 files changed, 177 insertions(+), 147 deletions(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index e92ae8ce1..a150f3601 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -12,7 +12,6 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht ## ROS Nodes -- [`docking_server`](https://github.com/open-navigation/opennav_docking): An action server which implements charger docking node for AMRs. - `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. ### PantherChargingDock diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 183fb1b6a..0e79e2f90 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -12,64 +12,67 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_CHARGING_DOCK_HPP_ -#define PANTHER_CHARGING_DOCK_HPP_ +#ifndef PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ +#define PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ #include #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "opennav_docking/pose_filter.hpp" -#include "opennav_docking_core/charging_dock.hpp" -#include "opennav_docking_core/docking_exceptions.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + #include "panther_msgs/msg/charging_status.hpp" #include "panther_msgs/msg/io_state.hpp" -#include "rclcpp/rclcpp.hpp" -#include "realtime_tools/realtime_box.h" -#include "sensor_msgs/msg/battery_state.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "tf2/utils.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" namespace panther_docking { /** * @class PantherChargingDock - * @brief Abstract interface for a charging dock for the docking framework + * @brief A class to represent a Panther charging dock. */ class PantherChargingDock : public opennav_docking_core::ChargingDock { public: - using Ptr = std::shared_ptr; - - PantherChargingDock() {} + using SharedPtr = std::shared_ptr; + using PoseStampedMsg = geometry_msgs::msg::PoseStamped; /** - * @param parent pointer to user's node + * @brief Configure the dock with the necessary information. + * + * @param parent Pointer to parent node * @param name The name of this planner * @param tf A pointer to a TF buffer */ - virtual void configure( + void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf) override final; /** * @brief Method to cleanup resources used on shutdown. */ - virtual void cleanup() override final; + void cleanup() override final; /** * @brief Method to active Behavior and any threads involved in execution. */ - virtual void activate() override final; + void activate() override final; /** * @brief Method to deactivate Behavior and any threads involved in execution. */ - virtual void deactivate() override final; + void deactivate() override final; /** * @brief Method to obtain the dock's staging pose. This method should likely @@ -79,14 +82,14 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * @param frame Dock's frame of pose * @return PoseStamped of staging pose in the specified frame */ - virtual geometry_msgs::msg::PoseStamped getStagingPose( + PoseStampedMsg getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) override final; /** * @brief Method to obtain the refined pose of the dock, usually based on sensors * @param pose The initial estimate of the dock pose. */ - virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose) override final; + bool getRefinedPose(PoseStampedMsg & pose) override final; /** * @brief Have we made contact with dock? This can be implemented in a variety @@ -96,7 +99,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * NOTE: this function is expected to return QUICKLY. Blocking here will block * the docking controller loop. */ - virtual bool isDocked() override final; + bool isDocked() override final; /** * @brief Are we charging? If a charge dock requires any sort of negotiation @@ -106,7 +109,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * NOTE: this function is expected to return QUICKLY. Blocking here will block * the docking controller loop. */ - virtual bool isCharging() override final; + bool isCharging() override final; /** * @brief Undocking while current is still flowing can damage a charge dock @@ -117,19 +120,24 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * NOTE: this function is expected to return QUICKLY. Blocking here will block * the docking controller loop. */ - virtual bool disableCharging() override final; + bool disableCharging() override final; /** * @brief Similar to isCharging() but called when undocking. */ - virtual bool hasStoppedCharging() override final; + bool hasStoppedCharging() override final; +protected: /** - * @brief Get the name of the dock + * @brief Method to declare parameters. */ - std::string getName(); + void declareParameters(); + + /** + * @brief Method to get parameters. + */ + void getParameters(); -protected: /** * @brief Method calls enable/disable service of the charger * @@ -145,8 +153,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @return The transformed pose. */ - geometry_msgs::msg::PoseStamped transformPose( - const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame); + PoseStampedMsg transformPose(const PoseStampedMsg & pose, const std::string & target_frame); /** * @brief Offset the staging pose. @@ -157,8 +164,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @return The offset staging pose. */ - geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose( - const geometry_msgs::msg::PoseStamped & dock_pose); + PoseStampedMsg offsetStagingPoseToDockPose(const PoseStampedMsg & dock_pose); /** * @brief Offset the detected dock pose. @@ -169,8 +175,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * * @return The offset detected dock pose. */ - geometry_msgs::msg::PoseStamped offsetDetectedDockPose( - const geometry_msgs::msg::PoseStamped & detected_dock_pose); + PoseStampedMsg offsetDetectedDockPose(const PoseStampedMsg & detected_dock_pose); /** * @brief Get the dock pose in a detection dock frame. @@ -180,7 +185,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock * @param frame The detection frame to get the dock pose in. * @return The dock pose in the detection frame. */ - geometry_msgs::msg::PoseStamped getDockPose(const std::string & frame); + PoseStampedMsg getDockPose(const std::string & frame); /** * @brief Method to update the dock pose and publish it. @@ -206,20 +211,24 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; std::shared_ptr tf2_buffer_; + rclcpp::Subscription::SharedPtr charging_status_sub_; rclcpp::Subscription::SharedPtr io_state_sub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; + + rclcpp::Publisher::SharedPtr staging_pose_pub_; + rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Client::SharedPtr charger_enable_client_; realtime_tools::RealtimeBox> charging_status_box_{nullptr}; realtime_tools::RealtimeBox> io_state_box_{nullptr}; - geometry_msgs::msg::PoseStamped dock_pose_; - geometry_msgs::msg::PoseStamped staging_pose_; + PoseStampedMsg dock_pose_; + PoseStampedMsg staging_pose_; double external_detection_timeout_; tf2::Quaternion external_detection_rotation_; @@ -227,7 +236,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double external_detection_translation_y_; double external_detection_translation_z_; - std::shared_ptr filter_; + std::shared_ptr pose_filter_; double docking_distance_threshold_; double docking_yaw_threshold_; @@ -236,8 +245,10 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double staging_yaw_offset_; double enable_charger_service_call_timeout_; + + double pose_filter_coef_; }; } // namespace panther_docking -#endif // PANTHER_CHARGING_DOCK_HPP_ +#endif // PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 112966f89..8b16709e8 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -42,9 +42,9 @@ def generate_launch_description(): description="Add namespace to all launched nodes.", ) - docking_server_path = LaunchConfiguration("docking_server_path") - declare_docking_server_path_arg = DeclareLaunchArgument( - "docking_server_path", + 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"] ), @@ -52,11 +52,11 @@ def generate_launch_description(): ) namespaced_docking_server_config = ReplaceString( - source_file=docking_server_path, + source_file=docking_server_config_path, replacements={"": namespace, "//": "/"}, ) - docking_server = Node( + docking_server_node = Node( package="opennav_docking", executable="opennav_docking", parameters=[ @@ -68,7 +68,7 @@ def generate_launch_description(): emulate_tty=True, ) - docking_server_activate = Node( + docking_server_activate_node = Node( package="nav2_lifecycle_manager", executable="lifecycle_manager", name="nav2_docking_lifecycle_manager", @@ -88,8 +88,8 @@ def generate_launch_description(): [ declare_use_sim_arg, declare_namespace_arg, - declare_docking_server_path_arg, - docking_server, - docking_server_activate, + declare_docking_server_config_path_arg, + docking_server_node, + docking_server_activate_node, ] ) diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index c074eb7e8..8607a6da4 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "panther_docking/panther_charging_dock.hpp" + #include -#include "nav2_util/node_utils.hpp" -#include "panther_docking/panther_charging_dock.hpp" +#include + #include "panther_utils/common_utilities.hpp" namespace panther_docking @@ -38,69 +40,11 @@ void PantherChargingDock::configure( throw std::runtime_error("Failed to lock node"); } - nav2_util::declare_parameter_if_not_declared( - node_, "panther_version", rclcpp::ParameterValue(1.0)); - - 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)); - nav2_util::declare_parameter_if_not_declared( - node_, name + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); - - nav2_util::declare_parameter_if_not_declared( - node_, name + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); - nav2_util::declare_parameter_if_not_declared( - node_, name + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); - - 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 + ".enable_charger_service_call_timeout", rclcpp::ParameterValue(0.2)); - - node_->get_parameter("panther_version", panther_version_); + declareParameters(); + getParameters(); - node_->get_parameter(name + ".base_frame", base_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 + ".enable_charger_service_call_timeout", enable_charger_service_call_timeout_); - - double filter_coef; - node_->get_parameter(name + ".filter_coef", filter_coef); - filter_ = std::make_unique(filter_coef, external_detection_timeout_); + pose_filter_ = std::make_unique( + pose_filter_coef_, external_detection_timeout_); } void PantherChargingDock::cleanup() @@ -108,7 +52,7 @@ void PantherChargingDock::cleanup() dock_pose_pub_.reset(); staging_pose_pub_.reset(); - if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { charging_status_sub_.reset(); io_state_sub_.reset(); charger_enable_client_.reset(); @@ -116,13 +60,11 @@ void PantherChargingDock::cleanup() } void PantherChargingDock::activate() - { - dock_pose_pub_ = node_->create_publisher("docking/dock_pose", 1); - staging_pose_pub_ = node_->create_publisher( - "docking/staging_pose", 1); + dock_pose_pub_ = node_->create_publisher("docking/dock_pose", 1); + staging_pose_pub_ = node_->create_publisher("docking/staging_pose", 1); - if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { charging_status_sub_ = node_->create_subscription( "battery/charging_status", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { @@ -152,14 +94,82 @@ void PantherChargingDock::deactivate() dock_pose_pub_.reset(); staging_pose_pub_.reset(); - if (panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { charging_status_sub_.reset(); io_state_sub_.reset(); charger_enable_client_.reset(); } } -geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose( +void PantherChargingDock::declareParameters() +{ + nav2_util::declare_parameter_if_not_declared( + node_, "panther_version", rclcpp::ParameterValue(1.0)); + + 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)); + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); + + 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_ + ".enable_charger_service_call_timeout", rclcpp::ParameterValue(0.2)); +} + +void PantherChargingDock::getParameters() +{ + node_->get_parameter("panther_version", panther_version_); + + node_->get_parameter(name_ + ".base_frame", base_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_ + ".enable_charger_service_call_timeout", enable_charger_service_call_timeout_); + + node_->get_parameter(name_ + ".filter_coef", pose_filter_coef_); +} + +PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { // When the pose if default the robot is docking so the frame is the dock frame @@ -178,7 +188,7 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::getStagingPose( return staging_pose_; } -bool PantherChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) +bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) { try { updateDockPoseAndPublish(); @@ -205,7 +215,7 @@ bool PantherChargingDock::isCharging() { std::shared_ptr charging_status_msg; - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { try { return isDocked(); } catch (const opennav_docking_core::FailedToDetectDock & e) { @@ -234,7 +244,7 @@ bool PantherChargingDock::isCharging() bool PantherChargingDock::disableCharging() { - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { return true; } @@ -250,7 +260,7 @@ bool PantherChargingDock::disableCharging() bool PantherChargingDock::hasStoppedCharging() { - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { return !isCharging(); } @@ -267,11 +277,9 @@ bool PantherChargingDock::hasStoppedCharging() return true; } -std::string PantherChargingDock::getName() { return name_; } - void PantherChargingDock::setChargerState(bool state) { - if (!panther_utils::common_utilities::IsPantherVersionAtLeast(panther_version_, 1.2f)) { + if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { throw std::runtime_error("This version of Panther does not support charger control"); } @@ -293,10 +301,10 @@ void PantherChargingDock::setChargerState(bool state) } } -geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose( - const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame) +PantherChargingDock::PoseStampedMsg PantherChargingDock::transformPose( + const PoseStampedMsg & pose, const std::string & target_frame) { - geometry_msgs::msg::PoseStamped transformed_pose; + PoseStampedMsg transformed_pose; if (pose.header.frame_id.empty() || target_frame.empty()) { throw std::runtime_error( @@ -317,8 +325,8 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::transformPose( return transformed_pose; } -geometry_msgs::msg::PoseStamped PantherChargingDock::offsetStagingPoseToDockPose( - const geometry_msgs::msg::PoseStamped & dock_pose) +PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDockPose( + const PoseStampedMsg & dock_pose) { tf2::Transform dock_pose_transform; tf2::fromMsg(dock_pose.pose, dock_pose_transform); @@ -343,10 +351,10 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::offsetStagingPoseToDockPose return staging_pose; } -geometry_msgs::msg::PoseStamped PantherChargingDock::offsetDetectedDockPose( - const geometry_msgs::msg::PoseStamped & detected_dock_pose) +PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetDetectedDockPose( + const PoseStampedMsg & detected_dock_pose) { - geometry_msgs::msg::PoseStamped just_orientation; + PoseStampedMsg just_orientation; just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); geometry_msgs::msg::TransformStamped transform; transform.transform.rotation = detected_dock_pose.pose.orientation; @@ -357,7 +365,7 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::offsetDetectedDockPose( just_orientation.pose.orientation.x, just_orientation.pose.orientation.y, just_orientation.pose.orientation.z, just_orientation.pose.orientation.w); - geometry_msgs::msg::PoseStamped offset_detected_dock_pose = detected_dock_pose; + PoseStampedMsg offset_detected_dock_pose = detected_dock_pose; offset_detected_dock_pose.pose.orientation = tf2::toMsg(orientation); offset_detected_dock_pose.header = detected_dock_pose.header; offset_detected_dock_pose.pose.position = detected_dock_pose.pose.position; @@ -368,16 +376,16 @@ geometry_msgs::msg::PoseStamped PantherChargingDock::offsetDetectedDockPose( return offset_detected_dock_pose; } -geometry_msgs::msg::PoseStamped PantherChargingDock::getDockPose(const std::string & frame) +PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std::string & frame) { - geometry_msgs::msg::PoseStamped filtered_offset_detected_dock_pose; + PoseStampedMsg filtered_offset_detected_dock_pose; try { - geometry_msgs::msg::PoseStamped pose; + PoseStampedMsg pose; pose.header.frame_id = frame; pose.header.stamp = node_->get_clock()->now(); auto offset_detected_dock_pose = offsetDetectedDockPose(pose); - filtered_offset_detected_dock_pose = filter_->update(offset_detected_dock_pose); + filtered_offset_detected_dock_pose = pose_filter_->update(offset_detected_dock_pose); filtered_offset_detected_dock_pose = transformPose( filtered_offset_detected_dock_pose, base_frame_name_); diff --git a/panther_utils/include/panther_utils/common_utilities.hpp b/panther_utils/include/panther_utils/common_utilities.hpp index df34819a7..862068e9f 100644 --- a/panther_utils/include/panther_utils/common_utilities.hpp +++ b/panther_utils/include/panther_utils/common_utilities.hpp @@ -72,7 +72,7 @@ std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmo * @param version The version to be compared with. * @return bool True if the Panther version is at least the specified version, false otherwise. */ -bool IsPantherVersionAtLeast(const float panther_version_, const float version) +bool MeetsVersionRequirement(const float panther_version_, const float version) { return panther_version_ >= version - std::numeric_limits::epsilon(); } diff --git a/panther_utils/test/test_common_utilities.cpp b/panther_utils/test/test_common_utilities.cpp index 6eeaaa3ac..2cbc01094 100644 --- a/panther_utils/test/test_common_utilities.cpp +++ b/panther_utils/test/test_common_utilities.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -90,6 +91,17 @@ TEST(OpenFileTest, HandleOpenFileThrow) { panther_utils::common_utilities::OpenFile(path, std::ios_base::in); }, std::runtime_error); } +TEST(MeetsVersionRequirement, CorrectlyComparesVersions) +{ + float panther_version = 1.06; + EXPECT_TRUE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 0.0)); + EXPECT_TRUE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.0)); + EXPECT_TRUE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.06)); + EXPECT_FALSE(panther_utils::common_utilities::MeetsVersionRequirement( + panther_version, std::numeric_limits::quiet_NaN())); + EXPECT_FALSE(panther_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.2)); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 415a28e7c567dabafb5919d512a9403c754f40a0 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Thu, 22 Aug 2024 14:33:18 +0200 Subject: [PATCH 11/23] Add pre-commit workflow (#395) --- .github/workflows/pre-commit.yaml | 11 +++++++++++ .pre-commit-config.yaml | 8 ++++---- ROS_API.md | 12 ++++++------ panther_localization/launch/nmea_navsat.launch.py | 1 - 4 files changed, 21 insertions(+), 11 deletions(-) create mode 100644 .github/workflows/pre-commit.yaml diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 000000000..08f7311e1 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,11 @@ +--- +name: Pre-Commit + +on: + push: + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 421c74955..c426d2101 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -34,7 +34,7 @@ repos: - id: cmake-format - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.6 + rev: v18.1.8 hooks: - id: clang-format @@ -63,13 +63,13 @@ repos: args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/PyCQA/flake8 - rev: 7.1.0 + rev: 7.1.1 hooks: - id: flake8 args: @@ -94,7 +94,7 @@ repos: exclude: ^.*\/CHANGELOG\.rst/.*$ - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.8.0 + rev: v0.10.0 hooks: - id: prettier-package-xml - id: sort-package-xml diff --git a/ROS_API.md b/ROS_API.md index 8b663c8c5..9497bd700 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,12 +1,12 @@ # ROS API -> [!IMPORTANT] -> **Beta Release** -> -> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur. -> +> [!IMPORTANT] +> **Beta Release** +> +> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur. +> > We would greatly appreciate your feedback regarding the Panther ROS 2 driver. You can reach us in the following ways: -> +> > - By email at: [support@husarion.com](mailto:support@husarion.com) > - Via our community forum: [Husarion Community](https://community.husarion.com) > - By submitting an issue request on: [GitHub](https://github.com/husarion/panther_ros/issues) diff --git a/panther_localization/launch/nmea_navsat.launch.py b/panther_localization/launch/nmea_navsat.launch.py index f5995b2d2..585438f26 100644 --- a/panther_localization/launch/nmea_navsat.launch.py +++ b/panther_localization/launch/nmea_navsat.launch.py @@ -23,7 +23,6 @@ ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from nav2_common.launch import ReplaceString def generate_launch_description(): From 0b8c46e5f06761579ae8c385ceb7af882f431cb4 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 22 Aug 2024 13:52:28 +0000 Subject: [PATCH 12/23] added offsetPose | moved transformPose to ros_utils Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 1 + .../panther_docking/panther_charging_dock.hpp | 15 +-- panther_docking/src/panther_charging_dock.cpp | 77 ++++--------- .../test/test_panther_charging_dock.cpp | 35 ------ panther_utils/CMakeLists.txt | 5 +- .../include/panther_utils/ros_utils.hpp | 40 ++++++- panther_utils/package.xml | 2 + panther_utils/test/test_ros_utils.cpp | 101 ++++++++++++++++++ 8 files changed, 177 insertions(+), 99 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 160c2ce60..060eb8e3b 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -17,6 +17,7 @@ set(PACKAGE_DEPENDENCIES realtime_tools sensor_msgs std_srvs + tf2_geometry_msgs tf2_ros) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 0e79e2f90..52640b4e7 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -146,14 +146,15 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock void setChargerState(bool state); /** - * @brief Transform a pose to a target frame. + * @brief Offset a pose by a given transform. * - * @param pose The pose to transform. - * @param target_frame The target frame to transform the pose to. - * - * @return The transformed pose. + * This method offsets a pose by a given transform. + * @param pose The pose to offset. + * @param offset The offset to apply to the pose. + * @return The offset pose. */ - PoseStampedMsg transformPose(const PoseStampedMsg & pose, const std::string & target_frame); + PoseStampedMsg offsetPose( + const geometry_msgs::msg::PoseStamped & pose, const tf2::Transform & offset); /** * @brief Offset the staging pose. @@ -213,7 +214,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - std::shared_ptr tf2_buffer_; + tf2_ros::Buffer::SharedPtr tf2_buffer_; rclcpp::Subscription::SharedPtr charging_status_sub_; rclcpp::Subscription::SharedPtr io_state_sub_; diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 8607a6da4..8d69267a7 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -19,6 +19,7 @@ #include #include "panther_utils/common_utilities.hpp" +#include "panther_utils/ros_utils.hpp" namespace panther_docking { @@ -301,26 +302,16 @@ void PantherChargingDock::setChargerState(bool state) } } -PantherChargingDock::PoseStampedMsg PantherChargingDock::transformPose( - const PoseStampedMsg & pose, const std::string & target_frame) +PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetPose( + const geometry_msgs::msg::PoseStamped & pose, const tf2::Transform & offset) { - PoseStampedMsg transformed_pose; + tf2::Transform pose_transform; + tf2::fromMsg(pose.pose, pose_transform); - if (pose.header.frame_id.empty() || target_frame.empty()) { - throw std::runtime_error( - "Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + - "\", target frame: \"" + target_frame + "\""); - } - - if (!tf2_buffer_->canTransform( - pose.header.frame_id, target_frame, pose.header.stamp, - rclcpp::Duration::from_seconds(external_detection_timeout_))) { - throw std::runtime_error( - "Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + - std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); - } - - tf2_buffer_->transform(pose, transformed_pose, target_frame); + tf2::Transform offset_pose_transform = pose_transform * offset; + PantherChargingDock::PoseStampedMsg transformed_pose; + transformed_pose.header = pose.header; + tf2::toMsg(offset_pose_transform, transformed_pose.pose); return transformed_pose; } @@ -328,9 +319,6 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::transformPose( PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDockPose( const PoseStampedMsg & dock_pose) { - tf2::Transform dock_pose_transform; - tf2::fromMsg(dock_pose.pose, dock_pose_transform); - tf2::Transform staging_offset_transform; staging_offset_transform.setOrigin(tf2::Vector3(staging_x_offset_, 0.0, 0.0)); @@ -338,42 +326,19 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDock staging_yaw_rotation.setRPY(0, 0, staging_yaw_offset_); staging_offset_transform.setRotation(staging_yaw_rotation); - tf2::Transform staging_pose_transform = dock_pose_transform * staging_offset_transform; - - auto staging_pose = dock_pose; - staging_pose.header = dock_pose.header; - staging_pose.pose.position.x = staging_pose_transform.getOrigin().getX(); - staging_pose.pose.position.y = staging_pose_transform.getOrigin().getY(); - staging_pose.pose.position.z = staging_pose_transform.getOrigin().getZ(); - - staging_pose.pose.orientation = tf2::toMsg(staging_pose_transform.getRotation()); - - return staging_pose; + return offsetPose(dock_pose, staging_offset_transform); } PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetDetectedDockPose( const PoseStampedMsg & detected_dock_pose) { - PoseStampedMsg just_orientation; - just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); - geometry_msgs::msg::TransformStamped transform; - transform.transform.rotation = detected_dock_pose.pose.orientation; - - tf2::doTransform(just_orientation, just_orientation, transform); - - tf2::Quaternion orientation( - just_orientation.pose.orientation.x, just_orientation.pose.orientation.y, - just_orientation.pose.orientation.z, just_orientation.pose.orientation.w); - - PoseStampedMsg offset_detected_dock_pose = detected_dock_pose; - offset_detected_dock_pose.pose.orientation = tf2::toMsg(orientation); - offset_detected_dock_pose.header = detected_dock_pose.header; - offset_detected_dock_pose.pose.position = detected_dock_pose.pose.position; - offset_detected_dock_pose.pose.position.x += external_detection_translation_x_; - offset_detected_dock_pose.pose.position.y += external_detection_translation_y_; - offset_detected_dock_pose.pose.position.z += external_detection_translation_z_; - - return offset_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 offsetPose(detected_dock_pose, offset); } PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std::string & frame) @@ -386,8 +351,9 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std:: auto offset_detected_dock_pose = offsetDetectedDockPose(pose); filtered_offset_detected_dock_pose = pose_filter_->update(offset_detected_dock_pose); - filtered_offset_detected_dock_pose = transformPose( - filtered_offset_detected_dock_pose, base_frame_name_); + filtered_offset_detected_dock_pose = panther_utils::ros::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) { @@ -413,7 +379,8 @@ void PantherChargingDock::updateStagingPoseAndPublish(const std::string & frame) { try { auto new_staging_pose = offsetStagingPoseToDockPose(dock_pose_); - staging_pose_ = transformPose(new_staging_pose, frame); + staging_pose_ = panther_utils::ros::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) { diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp index 82876124a..8d5f46e94 100644 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -34,11 +34,6 @@ class PantherChargingDockWrapper : public panther_docking::PantherChargingDock void setChargerState(bool state) { panther_docking::PantherChargingDock::setChargerState(state); } - geometry_msgs::msg::PoseStamped transformPose( - const geometry_msgs::msg::PoseStamped & pose, const std::string & target_frame) - { - return panther_docking::PantherChargingDock::transformPose(pose, target_frame); - } geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose( const geometry_msgs::msg::PoseStamped & dock_pose) { @@ -268,36 +263,6 @@ TEST_F(TestPantherChargingDock, SetChargerStateSuccess) EXPECT_FALSE(charging_status_); } -TEST_F(TestPantherChargingDock, TransformPose) -{ - ConfigureAndActivateDock(1.21); - geometry_msgs::msg::PoseStamped pose; - EXPECT_THROW({ charging_dock_->transformPose(pose, ""); }, std::runtime_error); - EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); - - pose.header.frame_id = "odom"; - EXPECT_THROW({ charging_dock_->transformPose(pose, "base_link"); }, std::runtime_error); - - SetBaseLinkToOdomTransform(); - - pose.header.frame_id = "odom"; - pose.pose.position.x = 0.1; - - ASSERT_NO_THROW({ pose = charging_dock_->transformPose(pose, "odom"); };); - EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); - EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); - EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); - EXPECT_EQ(pose.header.frame_id, "odom"); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); - - ASSERT_NO_THROW({ pose = charging_dock_->transformPose(pose, "base_link"); };); - EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); - EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); - EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); - EXPECT_EQ(pose.header.frame_id, "base_link"); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); -} - TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) { SetStagingOffsets(); diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index 9fbca5440..99b0c417d 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -12,6 +12,8 @@ set(PACKAGE_DEPENDENCIES realtime_tools rclcpp std_msgs + tf2_geometry_msgs + tf2_ros yaml-cpp) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) @@ -55,7 +57,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_ros_utils PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_ros_utils std_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_ros_utils std_msgs tf2_ros + tf2_geometry_msgs) ament_add_gtest(${PROJECT_NAME}_test_ros_test_utils test/test_ros_test_utils.cpp) diff --git a/panther_utils/include/panther_utils/ros_utils.hpp b/panther_utils/include/panther_utils/ros_utils.hpp index d8157bc11..ee99ddee3 100644 --- a/panther_utils/include/panther_utils/ros_utils.hpp +++ b/panther_utils/include/panther_utils/ros_utils.hpp @@ -17,7 +17,10 @@ #include -#include "std_msgs/msg/header.hpp" +#include + +#include +#include namespace panther_utils::ros { @@ -90,6 +93,16 @@ std_msgs::msg::Header MergeHeaders( return merged_header; } +/** + * @brief Adds a namespace to a frame ID. + * + * This function adds a namespace to a frame ID. The namespace is added as a prefix to the frame ID. + * + * @param frame_id The frame ID to which the namespace should be added. + * @param node_namespace The namespace to be added to the frame ID. + * + * @return The frame ID with the namespace added as a prefix. + */ std::string AddNamespaceToFrameID(const std::string & frame_id, const std::string & node_namespace) { std::string tf_prefix = node_namespace; @@ -105,6 +118,31 @@ std::string AddNamespaceToFrameID(const std::string & frame_id, const std::strin return tf_prefix + frame_id; } +geometry_msgs::msg::PoseStamped TransformPose( + const tf2_ros::Buffer::SharedPtr & tf2_buffer, const geometry_msgs::msg::PoseStamped & pose, + const std::string & target_frame, double timeout_s = 0.0) +{ + geometry_msgs::msg::PoseStamped transformed_pose; + + if (pose.header.frame_id.empty() || target_frame.empty()) { + throw std::runtime_error( + "Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + + "\", target frame: \"" + target_frame + "\""); + } + + if (!tf2_buffer->canTransform( + pose.header.frame_id, target_frame, pose.header.stamp, + rclcpp::Duration::from_seconds(timeout_s))) { + throw std::runtime_error( + "Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + + std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); + } + + tf2_buffer->transform(pose, transformed_pose, target_frame); + + return transformed_pose; +} + } // namespace panther_utils::ros #endif // PANTHER_UTILS__ROS_UTILS_HPP_ diff --git a/panther_utils/package.xml b/panther_utils/package.xml index 442d1a5d3..6cb314784 100644 --- a/panther_utils/package.xml +++ b/panther_utils/package.xml @@ -19,6 +19,8 @@ rclcpp realtime_tools std_msgs + tf2_geometry_msgs + tf2_ros yaml-cpp ament_cmake_gtest diff --git a/panther_utils/test/test_ros_utils.cpp b/panther_utils/test/test_ros_utils.cpp index 5832388a4..5a5e9b9b5 100644 --- a/panther_utils/test/test_ros_utils.cpp +++ b/panther_utils/test/test_ros_utils.cpp @@ -15,6 +15,11 @@ #include #include +#include +#include +#include +#include + #include #include "panther_utils/ros_utils.hpp" @@ -173,6 +178,102 @@ TEST(TestAddNamespaceToFrameID, WithNamespace) EXPECT_EQ(result, "namespace/frame"); } +class TestTransformPose : public ::testing::Test +{ +public: + TestTransformPose() + { + rclcpp::init(0, nullptr); + + clock_ = std::make_shared(RCL_SYSTEM_TIME); + tf2_buffer_ = std::make_shared(clock_); + + // Silence error about dedicated thread's being necessary + tf2_buffer_->setUsingDedicatedThread(true); + } + + ~TestTransformPose() { rclcpp::shutdown(); } + + void SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp) + { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = stamp; + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = 0.3; + transform.transform.translation.y = 0.2; + transform.transform.translation.z = 0.1; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf2_buffer_->setTransform(transform, "unittest", false); + } + +protected: + rclcpp::Clock::SharedPtr clock_; + tf2_ros::Buffer::SharedPtr tf2_buffer_; +}; + +TEST_F(TestTransformPose, NoTf) +{ + geometry_msgs::msg::PoseStamped pose; + EXPECT_THROW({ panther_utils::ros::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); + EXPECT_THROW( + { panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); + pose.header.frame_id = "odom"; + EXPECT_THROW( + { panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); +} + +TEST_F(TestTransformPose, FrameToFrameTransform) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "odom"; + pose.pose.position.x = 0.1; + pose.header.stamp = clock_->now(); + + ASSERT_NO_THROW({ pose = panther_utils::ros::TransformPose(tf2_buffer_, pose, "odom", 10.0); };); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); + EXPECT_EQ(pose.header.frame_id, "odom"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); +} + +TEST_F(TestTransformPose, FrameToOtherFrameTransform) +{ + geometry_msgs::msg::PoseStamped pose; + const auto stamp = clock_->now(); + pose.header.frame_id = "odom"; + pose.pose.position.x = 0.1; + pose.header.stamp = stamp; + SetBaseLinkToOdomTransform(stamp); + + ASSERT_NO_THROW( + { pose = panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link", 10.0); };); + EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); + EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); + EXPECT_EQ(pose.header.frame_id, "base_link"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); +} + +TEST_F(TestTransformPose, TestTimeout) +{ + const auto stamp = clock_->now(); + SetBaseLinkToOdomTransform(stamp); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "odom"; + pose.header.stamp = stamp; + pose.header.stamp.sec += 5; + + EXPECT_THROW( + { panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link", 1.0); }, + std::runtime_error); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From bfb48114c28f7bae09c91d2bf9241d68ed403846 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 22 Aug 2024 14:26:11 +0000 Subject: [PATCH 13/23] removed hardware deps Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 1 - .../panther_docking/panther_charging_dock.hpp | 24 --- panther_docking/package.xml | 1 - panther_docking/src/panther_charging_dock.cpp | 146 ++---------------- .../test/test_panther_charging_dock.cpp | 143 +++-------------- 5 files changed, 32 insertions(+), 283 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 060eb8e3b..d7a741559 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -10,7 +10,6 @@ set(PACKAGE_DEPENDENCIES geometry_msgs opennav_docking_core opennav_docking - panther_msgs panther_utils pluginlib rclcpp diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 52640b4e7..1e3740bf6 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -32,9 +31,6 @@ #include #include -#include "panther_msgs/msg/charging_status.hpp" -#include "panther_msgs/msg/io_state.hpp" - namespace panther_docking { @@ -138,13 +134,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(); - /** - * @brief Method calls enable/disable service of the charger - * - * @param state The state to set the charger to - */ - void setChargerState(bool state); - /** * @brief Offset a pose by a given transform. * @@ -205,10 +194,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void updateStagingPoseAndPublish(const std::string & frame); - std::string name_; std::string base_frame_name_; std::string dock_frame_; - double panther_version_; rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; @@ -216,18 +203,9 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp_lifecycle::LifecycleNode::SharedPtr node_; tf2_ros::Buffer::SharedPtr tf2_buffer_; - rclcpp::Subscription::SharedPtr charging_status_sub_; - rclcpp::Subscription::SharedPtr io_state_sub_; - rclcpp::Publisher::SharedPtr staging_pose_pub_; rclcpp::Publisher::SharedPtr dock_pose_pub_; - rclcpp::Client::SharedPtr charger_enable_client_; - - realtime_tools::RealtimeBox> - charging_status_box_{nullptr}; - realtime_tools::RealtimeBox> io_state_box_{nullptr}; - PoseStampedMsg dock_pose_; PoseStampedMsg staging_pose_; @@ -245,8 +223,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double staging_x_offset_; double staging_yaw_offset_; - double enable_charger_service_call_timeout_; - double pose_filter_coef_; }; diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 18738c6fe..10223d4e4 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -18,7 +18,6 @@ nav2_lifecycle_manager nav2_util opennav_docking - panther_msgs panther_utils pluginlib rclcpp diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 8d69267a7..f4968752d 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -52,54 +52,18 @@ void PantherChargingDock::cleanup() { dock_pose_pub_.reset(); staging_pose_pub_.reset(); - - if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - charging_status_sub_.reset(); - io_state_sub_.reset(); - charger_enable_client_.reset(); - } } void PantherChargingDock::activate() { dock_pose_pub_ = node_->create_publisher("docking/dock_pose", 1); staging_pose_pub_ = node_->create_publisher("docking/staging_pose", 1); - - if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - charging_status_sub_ = node_->create_subscription( - "battery/charging_status", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { - charging_status_box_.set(std::move(msg)); - }); - io_state_sub_ = node_->create_subscription( - "hardware/io_state", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { - io_state_box_.set(std::move(msg)); - }); - - charger_enable_client_ = - node_->create_client("hardware/charger_enable"); - - using namespace std::chrono_literals; - if (!charger_enable_client_->wait_for_service(1s)) { - RCLCPP_WARN_STREAM( - logger_, - "Service \"hardware/charger_enable\" not available. Make sure if Panther ROS 2 Stack is " - "running."); - } - } } void PantherChargingDock::deactivate() { dock_pose_pub_.reset(); staging_pose_pub_.reset(); - - if (panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - charging_status_sub_.reset(); - io_state_sub_.reset(); - charger_enable_client_.reset(); - } } void PantherChargingDock::declareParameters() @@ -123,8 +87,6 @@ void PantherChargingDock::declareParameters() node_, name_ + ".external_detection_rotation_pitch", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( node_, name_ + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); nav2_util::declare_parameter_if_not_declared( node_, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); @@ -137,13 +99,11 @@ void PantherChargingDock::declareParameters() node_, name_ + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".enable_charger_service_call_timeout", rclcpp::ParameterValue(0.2)); + node_, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); } void PantherChargingDock::getParameters() { - node_->get_parameter("panther_version", panther_version_); - node_->get_parameter(name_ + ".base_frame", base_frame_name_); node_->get_parameter(name_ + ".external_detection_timeout", external_detection_timeout_); node_->get_parameter( @@ -164,28 +124,25 @@ void PantherChargingDock::getParameters() node_->get_parameter(name_ + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name_ + ".staging_yaw_offset", staging_yaw_offset_); - node_->get_parameter( - name_ + ".enable_charger_service_call_timeout", enable_charger_service_call_timeout_); - node_->get_parameter(name_ + ".filter_coef", pose_filter_coef_); } PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { + std::string stage_frame = frame; // When the pose if default the robot is docking so the frame is the dock frame if (pose == geometry_msgs::msg::Pose()) { dock_frame_ = frame; - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(base_frame_name_); - } else { - if (dock_frame_.empty()) { - throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); - } - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(frame); + stage_frame = base_frame_name_; + } + + if (dock_frame_.empty()) { + throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); } + updateDockPoseAndPublish(); + updateStagingPoseAndPublish(stage_frame); return staging_pose_; } @@ -214,93 +171,16 @@ bool PantherChargingDock::isDocked() bool PantherChargingDock::isCharging() { - std::shared_ptr charging_status_msg; - - if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - try { - return isDocked(); - } catch (const opennav_docking_core::FailedToDetectDock & e) { - return false; - } - } - - // TODO: This might be used when a robot is undocking but we do not want to enable charging when - // the robot is undocking try { - setChargerState(true); - } catch (const std::runtime_error & e) { - throw opennav_docking_core::FailedToCharge( - "An exception occurred while enabling charging: " + std::string(e.what())); - } - - // CAUTION: The controller frequency can be higher than the message frequency - charging_status_box_.get(charging_status_msg); - - if (!charging_status_msg) { - throw opennav_docking_core::FailedToCharge("Did not receive charging_status_msg message"); - } - - return charging_status_msg->charging; -} - -bool PantherChargingDock::disableCharging() -{ - if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - return true; - } - - try { - setChargerState(false); - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "An exception occurred while disabling charging: " << e.what()); + return isDocked(); + } catch (const opennav_docking_core::FailedToDetectDock & e) { return false; } - - return true; -} - -bool PantherChargingDock::hasStoppedCharging() -{ - if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - return !isCharging(); - } - - std::shared_ptr charging_status_msg; - charging_status_box_.get(charging_status_msg); - if (!charging_status_msg) { - throw opennav_docking_core::FailedToCharge("Did not receive charging_status_msg message"); - } - - if (charging_status_msg->charging) { - throw opennav_docking_core::FailedToCharge("Charging status is still true"); - } - - return true; } -void PantherChargingDock::setChargerState(bool state) -{ - if (!panther_utils::common_utilities::MeetsVersionRequirement(panther_version_, 1.2f)) { - throw std::runtime_error("This version of Panther does not support charger control"); - } +bool PantherChargingDock::disableCharging() { return true; } - auto request = std::make_shared(); - request->data = state; - - auto result = charger_enable_client_->async_send_request(request); - - const auto timeout = std::chrono::duration(enable_charger_service_call_timeout_); - // This doubles spinning the node because the node is spinning in docking_server - if ( - rclcpp::spin_until_future_complete(node_, result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) { - throw std::runtime_error("Failed to call charger enable service"); - } - - if (!result.get()->success) { - throw std::runtime_error("Failed to enable charger"); - } -} +bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetPose( const geometry_msgs::msg::PoseStamped & pose, const tf2::Transform & offset) diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp index 8d5f46e94..f8bf8a83a 100644 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/battery_state.hpp" -#include "sensor_msgs/msg/joint_state.hpp" -#include "tf2/utils.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/static_transform_broadcaster.h" +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include "panther_docking/panther_charging_dock.hpp" #include "panther_utils/test/ros_test_utils.hpp" @@ -32,8 +32,6 @@ class PantherChargingDockWrapper : public panther_docking::PantherChargingDock using SharedPtr = std::shared_ptr; PantherChargingDockWrapper() : panther_docking::PantherChargingDock() {} - void setChargerState(bool state) { panther_docking::PantherChargingDock::setChargerState(state); } - geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose( const geometry_msgs::msg::PoseStamped & dock_pose) { @@ -68,10 +66,7 @@ class TestPantherChargingDock : public testing::Test ~TestPantherChargingDock(); protected: - void ConfigureAndActivateDock(double panther_version); - void CreateChargerEnableService(bool success); - void CreateChargerStatusPublisher(); - void PublishChargerStatus(bool charging); + void ConfigureAndActivateDock(); void SetBaseLinkToOdomTransform(); void SetDockToBaseLinkTransform( double x = 1.0, double y = 2.0, double z = 3.0, double yaw = 1.57); @@ -82,8 +77,6 @@ class TestPantherChargingDock : public testing::Test rclcpp_lifecycle::LifecycleNode::SharedPtr node_; tf2_ros::Buffer::SharedPtr tf2_buffer_; PantherChargingDockWrapper::SharedPtr charging_dock_; - rclcpp::Service::SharedPtr charger_enable_service_; - rclcpp::Publisher::SharedPtr charger_state_pub_; rclcpp::Subscription::SharedPtr dock_pose_sub_; rclcpp::Subscription::SharedPtr staging_pose_sub_; geometry_msgs::msg::PoseStamped::SharedPtr dock_pose_; @@ -121,10 +114,8 @@ TestPantherChargingDock::~TestPantherChargingDock() rclcpp::shutdown(); } -void TestPantherChargingDock::ConfigureAndActivateDock(double panther_version) +void TestPantherChargingDock::ConfigureAndActivateDock() { - node_->declare_parameter("panther_version", rclcpp::ParameterValue(panther_version)); - tf2_buffer_ = std::make_shared(node_->get_clock()); dock_pose_sub_ = node_->create_subscription( @@ -144,38 +135,6 @@ void TestPantherChargingDock::ConfigureAndActivateDock(double panther_version) charging_dock_->activate(); } -void TestPantherChargingDock::CreateChargerEnableService(bool success) -{ - charger_enable_service_ = node_->create_service( - "hardware/charger_enable", [success, this]( - const std::shared_ptr request, - std::shared_ptr response) { - response->success = success; - if (success) { - this->charging_status_ = request->data; - } else { - this->charging_status_ = false; - } - - if (this->charger_state_pub_) { - PublishChargerStatus(this->charging_status_); - } - }); -} - -void TestPantherChargingDock::CreateChargerStatusPublisher() -{ - charger_state_pub_ = node_->create_publisher( - "battery/charging_status", rclcpp::QoS(1)); -} - -void TestPantherChargingDock::PublishChargerStatus(bool charging) -{ - panther_msgs::msg::ChargingStatus msg; - msg.charging = charging; - charger_state_pub_->publish(msg); -} - void TestPantherChargingDock::SetBaseLinkToOdomTransform() { geometry_msgs::msg::TransformStamped transform; @@ -237,36 +196,10 @@ void TestPantherChargingDock::SetStagingOffsets() "test_dock.staging_yaw_offset", rclcpp::ParameterValue(staging_yaw_offset_)); } -TEST_F(TestPantherChargingDock, SetChargerStateOlderFailure) -{ - ConfigureAndActivateDock(1.06); - EXPECT_THROW({ charging_dock_->setChargerState(true); }, std::runtime_error); - EXPECT_THROW({ charging_dock_->setChargerState(false); }, std::runtime_error); -} - -TEST_F(TestPantherChargingDock, SetChargerStateFailure) -{ - ConfigureAndActivateDock(1.21); - CreateChargerEnableService(false); - EXPECT_THROW({ charging_dock_->setChargerState(true); }, std::runtime_error); -} - -TEST_F(TestPantherChargingDock, SetChargerStateSuccess) -{ - ConfigureAndActivateDock(1.21); - CreateChargerEnableService(true); - CreateChargerStatusPublisher(); - - EXPECT_NO_THROW({ charging_dock_->setChargerState(true); }); - EXPECT_TRUE(charging_status_); - EXPECT_NO_THROW({ charging_dock_->setChargerState(false); }); - EXPECT_FALSE(charging_status_); -} - TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) { SetStagingOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); geometry_msgs::msg::PoseStamped pose; pose = charging_dock_->offsetStagingPoseToDockPose(pose); @@ -278,7 +211,7 @@ TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) TEST_F(TestPantherChargingDock, offsetDetectedDockPose) { SetDetectionOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); geometry_msgs::msg::PoseStamped pose; auto new_pose = charging_dock_->offsetDetectedDockPose(pose); @@ -300,7 +233,7 @@ TEST_F(TestPantherChargingDock, GetDockPose) { SetDetectionOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); EXPECT_THROW({ charging_dock_->getDockPose("wrong_dock_pose"); }, std::runtime_error); @@ -326,7 +259,7 @@ TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) SetDetectionOffsets(); SetStagingOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); SetDockToBaseLinkTransform(); SetBaseLinkToOdomTransform(); @@ -379,7 +312,7 @@ TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) TEST_F(TestPantherChargingDock, GetStagingPose) { SetStagingOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); geometry_msgs::msg::PoseStamped pose; ASSERT_THROW( @@ -399,7 +332,7 @@ TEST_F(TestPantherChargingDock, GetStagingPose) TEST_F(TestPantherChargingDock, GetRefinedPose) { SetDetectionOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); geometry_msgs::msg::PoseStamped pose; ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); @@ -425,7 +358,7 @@ TEST_F(TestPantherChargingDock, GetRefinedPose) TEST_F(TestPantherChargingDock, IsDocked) { SetDetectionOffsets(); - ConfigureAndActivateDock(1.21); + ConfigureAndActivateDock(); geometry_msgs::msg::PoseStamped pose; SetDockToBaseLinkTransform(); @@ -441,41 +374,3 @@ TEST_F(TestPantherChargingDock, IsDocked) EXPECT_TRUE(charging_dock_->isDocked()); } - -TEST_F(TestPantherChargingDock, Charging106) -{ - ConfigureAndActivateDock(1.06); - EXPECT_FALSE(charging_dock_->isCharging()); - EXPECT_TRUE(charging_dock_->disableCharging()); - EXPECT_TRUE(charging_dock_->hasStoppedCharging()); -} - -TEST_F(TestPantherChargingDock, ChargingStateNoServiceFailure) -{ - ConfigureAndActivateDock(1.21); - EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); - EXPECT_FALSE(charging_dock_->disableCharging()); - EXPECT_THROW({ charging_dock_->hasStoppedCharging(); }, opennav_docking_core::FailedToCharge); -} - -TEST_F(TestPantherChargingDock, ChargingStateFailRespondFailure) -{ - ConfigureAndActivateDock(1.21); - CreateChargerStatusPublisher(); - CreateChargerEnableService(false); - - EXPECT_THROW({ charging_dock_->isCharging(); }, opennav_docking_core::FailedToCharge); - EXPECT_FALSE(charging_dock_->disableCharging()); - EXPECT_TRUE(charging_dock_->hasStoppedCharging()); -} - -TEST_F(TestPantherChargingDock, ChargingStateSuccess) -{ - ConfigureAndActivateDock(1.21); - CreateChargerStatusPublisher(); - CreateChargerEnableService(true); - - EXPECT_TRUE(charging_dock_->isCharging()); - EXPECT_TRUE(charging_dock_->disableCharging()); - EXPECT_TRUE(charging_dock_->hasStoppedCharging()); -} From a19a85f1da45aa7adebaf2dc56b47c455de38e04 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 22 Aug 2024 15:32:39 +0000 Subject: [PATCH 14/23] Moved TransformPose to tf2_utils Signed-off-by: Jakub Delicat --- panther_docking/src/panther_charging_dock.cpp | 6 +- panther_utils/CMakeLists.txt | 11 +- .../include/panther_utils/ros_utils.hpp | 34 +---- .../include/panther_utils/tf2_utils.hpp | 63 +++++++++ panther_utils/test/test_ros_utils.cpp | 101 -------------- panther_utils/test/test_tf2_utils.cpp | 124 ++++++++++++++++++ 6 files changed, 202 insertions(+), 137 deletions(-) create mode 100644 panther_utils/include/panther_utils/tf2_utils.hpp create mode 100644 panther_utils/test/test_tf2_utils.cpp diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index f4968752d..c2446921b 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -19,7 +19,7 @@ #include #include "panther_utils/common_utilities.hpp" -#include "panther_utils/ros_utils.hpp" +#include "panther_utils/tf2_utils.hpp" namespace panther_docking { @@ -231,7 +231,7 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std:: 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::ros::TransformPose( + filtered_offset_detected_dock_pose = panther_utils::tf2::TransformPose( tf2_buffer_, filtered_offset_detected_dock_pose, base_frame_name_, external_detection_timeout_); @@ -259,7 +259,7 @@ void PantherChargingDock::updateStagingPoseAndPublish(const std::string & frame) { try { auto new_staging_pose = offsetStagingPoseToDockPose(dock_pose_); - staging_pose_ = panther_utils::ros::TransformPose( + staging_pose_ = panther_utils::tf2::TransformPose( tf2_buffer_, new_staging_pose, frame, external_detection_timeout_); dock_pose_.pose.position.z = 0.0; staging_pose_pub_->publish(staging_pose_); diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index 99b0c417d..d482401d5 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -57,8 +57,7 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_ros_utils PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_ros_utils std_msgs tf2_ros - tf2_geometry_msgs) + ament_target_dependencies(${PROJECT_NAME}_test_ros_utils std_msgs) ament_add_gtest(${PROJECT_NAME}_test_ros_test_utils test/test_ros_test_utils.cpp) @@ -68,6 +67,14 @@ if(BUILD_TESTING) $) ament_target_dependencies(${PROJECT_NAME}_test_ros_test_utils rclcpp std_msgs) + ament_add_gtest(${PROJECT_NAME}_test_tf2_utils test/test_tf2_utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_tf2_utils + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_tf2_utils std_msgs tf2_ros + tf2_geometry_msgs) + ament_add_gtest(${PROJECT_NAME}_test_yaml_utils test/test_yaml_utils.cpp) target_include_directories( ${PROJECT_NAME}_test_yaml_utils diff --git a/panther_utils/include/panther_utils/ros_utils.hpp b/panther_utils/include/panther_utils/ros_utils.hpp index ee99ddee3..7121af2f2 100644 --- a/panther_utils/include/panther_utils/ros_utils.hpp +++ b/panther_utils/include/panther_utils/ros_utils.hpp @@ -12,14 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS__ROS_UTILS_HPP_ -#define PANTHER_UTILS__ROS_UTILS_HPP_ +#ifndef PANTHER_UTILS_ROS_UTILS_HPP_ +#define PANTHER_UTILS_ROS_UTILS_HPP_ #include -#include - -#include #include namespace panther_utils::ros @@ -118,31 +115,6 @@ std::string AddNamespaceToFrameID(const std::string & frame_id, const std::strin return tf_prefix + frame_id; } -geometry_msgs::msg::PoseStamped TransformPose( - const tf2_ros::Buffer::SharedPtr & tf2_buffer, const geometry_msgs::msg::PoseStamped & pose, - const std::string & target_frame, double timeout_s = 0.0) -{ - geometry_msgs::msg::PoseStamped transformed_pose; - - if (pose.header.frame_id.empty() || target_frame.empty()) { - throw std::runtime_error( - "Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + - "\", target frame: \"" + target_frame + "\""); - } - - if (!tf2_buffer->canTransform( - pose.header.frame_id, target_frame, pose.header.stamp, - rclcpp::Duration::from_seconds(timeout_s))) { - throw std::runtime_error( - "Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + - std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); - } - - tf2_buffer->transform(pose, transformed_pose, target_frame); - - return transformed_pose; -} - } // namespace panther_utils::ros -#endif // PANTHER_UTILS__ROS_UTILS_HPP_ +#endif // PANTHER_UTILS_ROS_UTILS_HPP_ diff --git a/panther_utils/include/panther_utils/tf2_utils.hpp b/panther_utils/include/panther_utils/tf2_utils.hpp new file mode 100644 index 000000000..14968c41a --- /dev/null +++ b/panther_utils/include/panther_utils/tf2_utils.hpp @@ -0,0 +1,63 @@ +// 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_UTILS_TF2_UTILS_HPP +#define PANTHER_UTILS_TF2_UTILS_HPP + +#include + +#include + +namespace panther_utils::tf2 +{ + +/** + * @brief Transforms a pose to a target frame. + * This function transforms a pose to a target frame using the provided tf2 buffer. + * + * @param tf2_buffer The tf2 buffer to be used for the transformation. + * @param pose The pose to be transformed. + * @param target_frame The target frame to which the pose should be transformed. + * @param timeout_s The timeout in seconds for the transformation. + * + * @return The transformed pose. + */ +geometry_msgs::msg::PoseStamped TransformPose( + const tf2_ros::Buffer::SharedPtr & tf2_buffer, const geometry_msgs::msg::PoseStamped & pose, + const std::string & target_frame, double timeout_s = 0.0) +{ + geometry_msgs::msg::PoseStamped transformed_pose; + + if (pose.header.frame_id.empty() || target_frame.empty()) { + throw std::runtime_error( + "Pose or target frame is empty, pose frame: \"" + pose.header.frame_id + + "\", target frame: \"" + target_frame + "\""); + } + + if (!tf2_buffer->canTransform( + pose.header.frame_id, target_frame, pose.header.stamp, + rclcpp::Duration::from_seconds(timeout_s))) { + throw std::runtime_error( + "Cannot transform " + pose.header.frame_id + " to " + target_frame + " at time " + + std::to_string(pose.header.stamp.sec) + "." + std::to_string(pose.header.stamp.nanosec)); + } + + tf2_buffer->transform(pose, transformed_pose, target_frame); + + return transformed_pose; +} + +} // namespace panther_utils::tf2 + +#endif // PANTHER_UTILS_TF2_UTILS_HPP diff --git a/panther_utils/test/test_ros_utils.cpp b/panther_utils/test/test_ros_utils.cpp index 5a5e9b9b5..5832388a4 100644 --- a/panther_utils/test/test_ros_utils.cpp +++ b/panther_utils/test/test_ros_utils.cpp @@ -15,11 +15,6 @@ #include #include -#include -#include -#include -#include - #include #include "panther_utils/ros_utils.hpp" @@ -178,102 +173,6 @@ TEST(TestAddNamespaceToFrameID, WithNamespace) EXPECT_EQ(result, "namespace/frame"); } -class TestTransformPose : public ::testing::Test -{ -public: - TestTransformPose() - { - rclcpp::init(0, nullptr); - - clock_ = std::make_shared(RCL_SYSTEM_TIME); - tf2_buffer_ = std::make_shared(clock_); - - // Silence error about dedicated thread's being necessary - tf2_buffer_->setUsingDedicatedThread(true); - } - - ~TestTransformPose() { rclcpp::shutdown(); } - - void SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp) - { - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = stamp; - transform.header.frame_id = "odom"; - transform.child_frame_id = "base_link"; - transform.transform.translation.x = 0.3; - transform.transform.translation.y = 0.2; - transform.transform.translation.z = 0.1; - transform.transform.rotation.x = 0.0; - transform.transform.rotation.y = 0.0; - transform.transform.rotation.z = 0.0; - transform.transform.rotation.w = 1.0; - - tf2_buffer_->setTransform(transform, "unittest", false); - } - -protected: - rclcpp::Clock::SharedPtr clock_; - tf2_ros::Buffer::SharedPtr tf2_buffer_; -}; - -TEST_F(TestTransformPose, NoTf) -{ - geometry_msgs::msg::PoseStamped pose; - EXPECT_THROW({ panther_utils::ros::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); - EXPECT_THROW( - { panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); - pose.header.frame_id = "odom"; - EXPECT_THROW( - { panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); -} - -TEST_F(TestTransformPose, FrameToFrameTransform) -{ - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "odom"; - pose.pose.position.x = 0.1; - pose.header.stamp = clock_->now(); - - ASSERT_NO_THROW({ pose = panther_utils::ros::TransformPose(tf2_buffer_, pose, "odom", 10.0); };); - EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); - EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); - EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); - EXPECT_EQ(pose.header.frame_id, "odom"); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); -} - -TEST_F(TestTransformPose, FrameToOtherFrameTransform) -{ - geometry_msgs::msg::PoseStamped pose; - const auto stamp = clock_->now(); - pose.header.frame_id = "odom"; - pose.pose.position.x = 0.1; - pose.header.stamp = stamp; - SetBaseLinkToOdomTransform(stamp); - - ASSERT_NO_THROW( - { pose = panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link", 10.0); };); - EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); - EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); - EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); - EXPECT_EQ(pose.header.frame_id, "base_link"); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); -} - -TEST_F(TestTransformPose, TestTimeout) -{ - const auto stamp = clock_->now(); - SetBaseLinkToOdomTransform(stamp); - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "odom"; - pose.header.stamp = stamp; - pose.header.stamp.sec += 5; - - EXPECT_THROW( - { panther_utils::ros::TransformPose(tf2_buffer_, pose, "base_link", 1.0); }, - std::runtime_error); -} - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_utils/test/test_tf2_utils.cpp b/panther_utils/test/test_tf2_utils.cpp new file mode 100644 index 000000000..981bf7fd2 --- /dev/null +++ b/panther_utils/test/test_tf2_utils.cpp @@ -0,0 +1,124 @@ +// 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 "panther_utils/tf2_utils.hpp" + +class TestTransformPose : public ::testing::Test +{ +public: + TestTransformPose(); + ~TestTransformPose(); + + void SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp); + +protected: + rclcpp::Clock::SharedPtr clock_; + tf2_ros::Buffer::SharedPtr tf2_buffer_; +}; + +void TestTransformPose::SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = stamp; + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = 0.3; + transform.transform.translation.y = 0.2; + transform.transform.translation.z = 0.1; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf2_buffer_->setTransform(transform, "unittest", false); +} + +TestTransformPose::TestTransformPose() +{ + rclcpp::init(0, nullptr); + + clock_ = std::make_shared(RCL_SYSTEM_TIME); + tf2_buffer_ = std::make_shared(clock_); + + // Silence error about dedicated thread's being necessary + tf2_buffer_->setUsingDedicatedThread(true); +} + +TestTransformPose::~TestTransformPose() { rclcpp::shutdown(); } + +TEST_F(TestTransformPose, NoTf) +{ + geometry_msgs::msg::PoseStamped pose; + EXPECT_THROW({ panther_utils::tf2::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); + EXPECT_THROW( + { panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); + pose.header.frame_id = "odom"; + EXPECT_THROW( + { panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); +} + +TEST_F(TestTransformPose, FrameToFrameTransform) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "odom"; + pose.pose.position.x = 0.1; + pose.header.stamp = clock_->now(); + + ASSERT_NO_THROW({ pose = panther_utils::tf2::TransformPose(tf2_buffer_, pose, "odom", 10.0); };); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); + EXPECT_EQ(pose.header.frame_id, "odom"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); +} + +TEST_F(TestTransformPose, FrameToOtherFrameTransform) +{ + geometry_msgs::msg::PoseStamped pose; + const auto stamp = clock_->now(); + pose.header.frame_id = "odom"; + pose.pose.position.x = 0.1; + pose.header.stamp = stamp; + SetBaseLinkToOdomTransform(stamp); + + ASSERT_NO_THROW( + { pose = panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link", 10.0); };); + EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); + EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); + EXPECT_EQ(pose.header.frame_id, "base_link"); + EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); +} + +TEST_F(TestTransformPose, TestTimeout) +{ + const auto stamp = clock_->now(); + SetBaseLinkToOdomTransform(stamp); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "odom"; + pose.header.stamp = stamp; + pose.header.stamp.sec += 5; + + EXPECT_THROW( + { panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link", 1.0); }, + std::runtime_error); +} From 60f460fd4491aa6a393de933de5f2744ae15ce5b Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 23 Aug 2024 09:50:35 +0000 Subject: [PATCH 15/23] added filter description Signed-off-by: Jakub Delicat --- panther_docking/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_docking/README.md b/panther_docking/README.md index a150f3601..9eae1ab33 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -41,7 +41,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `~.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**]: # TODO: @delihus +- `~.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. From 2213878c61c8c6c7475f5017cb31ec23a8b5a5b8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 23 Aug 2024 09:53:47 +0000 Subject: [PATCH 16/23] removed realtime_tools Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 1 - panther_docking/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index d7a741559..617c22ea5 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -13,7 +13,6 @@ set(PACKAGE_DEPENDENCIES panther_utils pluginlib rclcpp - realtime_tools sensor_msgs std_srvs tf2_geometry_msgs diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 10223d4e4..bf0cd3c2d 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -21,7 +21,6 @@ panther_utils pluginlib rclcpp - realtime_tools sensor_msgs std_srvs tf2_ros From 3a87e3d73f91c9d2c5fd403a26afda1af35dfc6e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 23 Aug 2024 13:34:22 +0000 Subject: [PATCH 17/23] Modef OffsetFunction utils Signed-off-by: Jakub Delicat --- .../panther_docking/panther_charging_dock.hpp | 11 --- panther_docking/src/panther_charging_dock.cpp | 22 +----- .../panther_utils/common_utilities.hpp | 13 ++-- .../include/panther_utils/tf2_utils.hpp | 29 ++++++- panther_utils/test/test_tf2_utils.cpp | 77 +++++++++++++++++-- 5 files changed, 107 insertions(+), 45 deletions(-) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 1e3740bf6..5ceffa4be 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -134,17 +134,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(); - /** - * @brief Offset a pose by a given transform. - * - * This method offsets a pose by a given transform. - * @param pose The pose to offset. - * @param offset The offset to apply to the pose. - * @return The offset pose. - */ - PoseStampedMsg offsetPose( - const geometry_msgs::msg::PoseStamped & pose, const tf2::Transform & offset); - /** * @brief Offset the staging pose. * diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index c2446921b..9638db4ed 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -182,20 +182,6 @@ bool PantherChargingDock::disableCharging() { return true; } bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } -PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetPose( - const geometry_msgs::msg::PoseStamped & pose, const tf2::Transform & offset) -{ - tf2::Transform pose_transform; - tf2::fromMsg(pose.pose, pose_transform); - - tf2::Transform offset_pose_transform = pose_transform * offset; - PantherChargingDock::PoseStampedMsg transformed_pose; - transformed_pose.header = pose.header; - tf2::toMsg(offset_pose_transform, transformed_pose.pose); - - return transformed_pose; -} - PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDockPose( const PoseStampedMsg & dock_pose) { @@ -206,7 +192,7 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDock staging_yaw_rotation.setRPY(0, 0, staging_yaw_offset_); staging_offset_transform.setRotation(staging_yaw_rotation); - return offsetPose(dock_pose, staging_offset_transform); + return panther_utils::tf2_utils::OffsetPose(dock_pose, staging_offset_transform); } PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetDetectedDockPose( @@ -218,7 +204,7 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetDetectedDockPose( external_detection_translation_z_)); offset.setRotation(external_detection_rotation_); - return offsetPose(detected_dock_pose, offset); + return panther_utils::tf2_utils::OffsetPose(detected_dock_pose, offset); } PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std::string & frame) @@ -231,7 +217,7 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std:: 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::TransformPose( + filtered_offset_detected_dock_pose = panther_utils::tf2_utils::TransformPose( tf2_buffer_, filtered_offset_detected_dock_pose, base_frame_name_, external_detection_timeout_); @@ -259,7 +245,7 @@ void PantherChargingDock::updateStagingPoseAndPublish(const std::string & frame) { try { auto new_staging_pose = offsetStagingPoseToDockPose(dock_pose_); - staging_pose_ = panther_utils::tf2::TransformPose( + 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_); diff --git a/panther_utils/include/panther_utils/common_utilities.hpp b/panther_utils/include/panther_utils/common_utilities.hpp index 862068e9f..475f602c4 100644 --- a/panther_utils/include/panther_utils/common_utilities.hpp +++ b/panther_utils/include/panther_utils/common_utilities.hpp @@ -64,17 +64,14 @@ std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmo } /** - * @brief Checks if the Panther version is at least the specified version. + * @brief Checks if the Panther version meets the required version. * - * This function checks if the Panther version is at least the specified version. - * - * @param panther_version_ The Panther version to be checked. - * @param version The version to be compared with. - * @return bool True if the Panther version is at least the specified version, false otherwise. + * @param panther_version The Panther version. + * @param version The required version. */ -bool MeetsVersionRequirement(const float panther_version_, const float version) +bool MeetsVersionRequirement(const float panther_version, const float version) { - return panther_version_ >= version - std::numeric_limits::epsilon(); + return panther_version >= version - std::numeric_limits::epsilon(); } } // namespace panther_utils::common_utilities diff --git a/panther_utils/include/panther_utils/tf2_utils.hpp b/panther_utils/include/panther_utils/tf2_utils.hpp index 14968c41a..de7eee32c 100644 --- a/panther_utils/include/panther_utils/tf2_utils.hpp +++ b/panther_utils/include/panther_utils/tf2_utils.hpp @@ -15,11 +15,13 @@ #ifndef PANTHER_UTILS_TF2_UTILS_HPP #define PANTHER_UTILS_TF2_UTILS_HPP +#include #include +#include #include -namespace panther_utils::tf2 +namespace panther_utils::tf2_utils { /** @@ -58,6 +60,29 @@ geometry_msgs::msg::PoseStamped TransformPose( return transformed_pose; } -} // namespace panther_utils::tf2 +/** + * @brief Offsets a pose by a given transform. + * This function offsets a pose by a given transform in the same frame. + * + * @param pose The pose to be offset. + * @param offset The offset transform. + * + * @return The offset pose. + */ +geometry_msgs::msg::PoseStamped OffsetPose( + const geometry_msgs::msg::PoseStamped & pose, const tf2::Transform & offset) +{ + tf2::Transform pose_transform; + tf2::fromMsg(pose.pose, pose_transform); + + tf2::Transform offset_pose_transform = pose_transform * offset; + geometry_msgs::msg::PoseStamped transformed_pose; + transformed_pose.header = pose.header; + tf2::toMsg(offset_pose_transform, transformed_pose.pose); + + return transformed_pose; +} + +} // namespace panther_utils::tf2_utils #endif // PANTHER_UTILS_TF2_UTILS_HPP diff --git a/panther_utils/test/test_tf2_utils.cpp b/panther_utils/test/test_tf2_utils.cpp index 981bf7fd2..28ff0a6fe 100644 --- a/panther_utils/test/test_tf2_utils.cpp +++ b/panther_utils/test/test_tf2_utils.cpp @@ -68,12 +68,15 @@ TestTransformPose::~TestTransformPose() { rclcpp::shutdown(); } TEST_F(TestTransformPose, NoTf) { geometry_msgs::msg::PoseStamped pose; - EXPECT_THROW({ panther_utils::tf2::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); EXPECT_THROW( - { panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); + EXPECT_THROW( + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link"); }, + std::runtime_error); pose.header.frame_id = "odom"; EXPECT_THROW( - { panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link"); }, std::runtime_error); + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link"); }, + std::runtime_error); } TEST_F(TestTransformPose, FrameToFrameTransform) @@ -83,7 +86,8 @@ TEST_F(TestTransformPose, FrameToFrameTransform) pose.pose.position.x = 0.1; pose.header.stamp = clock_->now(); - ASSERT_NO_THROW({ pose = panther_utils::tf2::TransformPose(tf2_buffer_, pose, "odom", 10.0); };); + ASSERT_NO_THROW( + { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "odom", 10.0); };); EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); @@ -101,7 +105,7 @@ TEST_F(TestTransformPose, FrameToOtherFrameTransform) SetBaseLinkToOdomTransform(stamp); ASSERT_NO_THROW( - { pose = panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link", 10.0); };); + { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link", 10.0); };); EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); @@ -119,6 +123,67 @@ TEST_F(TestTransformPose, TestTimeout) pose.header.stamp.sec += 5; EXPECT_THROW( - { panther_utils::tf2::TransformPose(tf2_buffer_, pose, "base_link", 1.0); }, + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link", 1.0); }, std::runtime_error); } + +TEST(OffsetPose, CheckDefaultPose) +{ + const auto translation = tf2::Vector3(0.1, 0.2, 0.3); + tf2::Quaternion rotation; + rotation.setRPY(0.0, 1.57, 3.14); + + tf2::Transform transform; + transform.setOrigin(translation); + transform.setRotation(rotation); + + geometry_msgs::msg::PoseStamped pose; + auto offset_pose = panther_utils::tf2_utils::OffsetPose(pose, transform); + + EXPECT_NEAR(offset_pose.pose.position.x, translation.getX(), 0.01); + EXPECT_NEAR(offset_pose.pose.position.y, translation.getY(), 0.01); + EXPECT_NEAR(offset_pose.pose.position.z, translation.getZ(), 0.01); + + tf2::Quaternion offset_rotation; + tf2::fromMsg(offset_pose.pose.orientation, offset_rotation); + tf2::Matrix3x3 matrix(offset_rotation); + double roll, pitch, yaw; + matrix.getRPY(roll, pitch, yaw); + EXPECT_NEAR(roll, 0.0, 0.01); + EXPECT_NEAR(pitch, 1.57, 0.01); + EXPECT_NEAR(yaw, 3.14, 0.01); +} + +TEST(OffsetPose, CheckDefinedPose) +{ + const auto translation = tf2::Vector3(0.7, 0.2, 0.3); + tf2::Quaternion rotation; + rotation.setRPY(0.0, M_PI / 2.0, M_PI); + + tf2::Transform transform; + transform.setOrigin(translation); + transform.setRotation(rotation); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0.1; + pose.pose.position.y = 0.9; + pose.pose.position.z = 0.2; + tf2::Quaternion pose_rotation; + pose_rotation.setRPY(M_PI, M_PI / 2.0, 0.0); + pose.pose.orientation = tf2::toMsg(pose_rotation); + + auto offset_pose = panther_utils::tf2_utils::OffsetPose(pose, transform); + + EXPECT_NEAR(offset_pose.pose.position.x, pose.pose.position.x - translation.getZ(), 0.01); + EXPECT_NEAR(offset_pose.pose.position.y, pose.pose.position.y - translation.getY(), 0.01); + EXPECT_NEAR(offset_pose.pose.position.z, pose.pose.position.z - translation.getX(), 0.01); + + tf2::Quaternion offset_rotation; + tf2::fromMsg(offset_pose.pose.orientation, offset_rotation); + tf2::Matrix3x3 matrix(offset_rotation); + double roll, pitch, yaw; + matrix.getRPY(roll, pitch, yaw); + EXPECT_NEAR(roll, 0.0, 0.01); + EXPECT_NEAR(pitch, 0.0, 0.01); + EXPECT_NEAR(yaw, 0.0, 0.01); +} From c5bb087436fafae1d6eb5a59ae50ac59cdbaeff0 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Sat, 24 Aug 2024 21:30:03 +0000 Subject: [PATCH 18/23] added and applied ArePosesNear() Signed-off-by: Jakub Delicat --- .../panther_docking/panther_charging_dock.hpp | 1 + panther_docking/src/panther_charging_dock.cpp | 7 +- .../test/test_panther_charging_dock.cpp | 294 ++++++++++-------- .../include/panther_utils/tf2_utils.hpp | 54 ++++ panther_utils/test/test_tf2_utils.cpp | 170 +++++++--- 5 files changed, 354 insertions(+), 172 deletions(-) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 5ceffa4be..65c51c3fb 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -42,6 +42,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock { public: using SharedPtr = std::shared_ptr; + using UniquePtr = std::unique_ptr; using PoseStampedMsg = geometry_msgs::msg::PoseStamped; /** diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 9638db4ed..e495e9a33 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -163,10 +163,11 @@ bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) bool PantherChargingDock::isDocked() { updateDockPoseAndPublish(); - const double d = std::hypot(dock_pose_.pose.position.x, dock_pose_.pose.position.y); - const double yaw_diff = std::abs(tf2::getYaw(dock_pose_.pose.orientation)); - return d < docking_distance_threshold_ && yaw_diff < docking_yaw_threshold_; + 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_); } bool PantherChargingDock::isCharging() diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp index f8bf8a83a..422c384c8 100644 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include #include @@ -20,16 +22,25 @@ #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( @@ -67,21 +78,28 @@ class TestPantherChargingDock : public testing::Test protected: void ConfigureAndActivateDock(); - void SetBaseLinkToOdomTransform(); - void SetDockToBaseLinkTransform( - double x = 1.0, double y = 2.0, double z = 3.0, double yaw = 1.57); + 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::SharedPtr charging_dock_; + + 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; @@ -98,6 +116,42 @@ TestPantherChargingDock::TestPantherChargingDock() { rclcpp::init(0, nullptr); 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() @@ -116,57 +170,21 @@ TestPantherChargingDock::~TestPantherChargingDock() void TestPantherChargingDock::ConfigureAndActivateDock() { - 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_shared(); - - charging_dock_->configure(node_, "test_dock", tf2_buffer_); + charging_dock_->configure(node_, kDockFrame, tf2_buffer_); charging_dock_->activate(); } -void TestPantherChargingDock::SetBaseLinkToOdomTransform() +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; - transform.header.stamp = node_->now(); - transform.header.frame_id = "odom"; - transform.child_frame_id = "base_link"; - transform.transform.translation.x = 0.3; - transform.transform.translation.y = 0.2; - transform.transform.translation.z = 0.1; - transform.transform.rotation.x = 0.0; - transform.transform.rotation.y = 0.0; - transform.transform.rotation.z = 0.0; - transform.transform.rotation.w = 1.0; - - tf2_buffer_->setTransform(transform, "unittest", true); -} + 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; -void TestPantherChargingDock::SetDockToBaseLinkTransform(double x, double y, double z, double yaw) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = node_->now(); - transform.header.frame_id = "base_link"; - transform.child_frame_id = "test_dock"; - transform.transform.translation.x = x; - transform.transform.translation.y = y; - transform.transform.translation.z = z; - - tf2::Quaternion just_orientation; - just_orientation.setRPY(0, 0, yaw); - transform.transform.rotation = tf2::toMsg(just_orientation); - - tf2_buffer_->setTransform(transform, "unittest", true); + tf2_buffer_->setTransform(transform_stamped, "unittest", true); } void TestPantherChargingDock::SetDetectionOffsets() @@ -196,7 +214,7 @@ void TestPantherChargingDock::SetStagingOffsets() "test_dock.staging_yaw_offset", rclcpp::ParameterValue(staging_yaw_offset_)); } -TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) +TEST_F(TestPantherChargingDock, OffsetStagingPoseToDockPose) { SetStagingOffsets(); ConfigureAndActivateDock(); @@ -208,25 +226,24 @@ TEST_F(TestPantherChargingDock, offsetStagingPoseToDockPose) EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), staging_yaw_offset_, 0.01); } -TEST_F(TestPantherChargingDock, offsetDetectedDockPose) +TEST_F(TestPantherChargingDock, OffsetDetectedDockPose) { SetDetectionOffsets(); ConfigureAndActivateDock(); geometry_msgs::msg::PoseStamped pose; - - auto new_pose = charging_dock_->offsetDetectedDockPose(pose); - - tf2::Quaternion external_detection_rotation; - - external_detection_rotation.setRPY( + 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_); - - tf2::Quaternion offset_rotation; - tf2::fromMsg(new_pose.pose.orientation, offset_rotation); - EXPECT_NEAR(new_pose.pose.position.x, external_detection_translation_x_, 0.01); - EXPECT_NEAR(new_pose.pose.position.y, external_detection_translation_y_, 0.01); - EXPECT_NEAR(new_pose.pose.position.z, external_detection_translation_z_, 0.01); - EXPECT_EQ(offset_rotation, external_detection_rotation); + 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) @@ -237,21 +254,24 @@ TEST_F(TestPantherChargingDock, GetDockPose) EXPECT_THROW({ charging_dock_->getDockPose("wrong_dock_pose"); }, std::runtime_error); - SetDockToBaseLinkTransform(); - geometry_msgs::msg::PoseStamped pose; - ASSERT_NO_THROW({ pose = charging_dock_->getDockPose("test_dock"); };); - - tf2::Quaternion external_detection_rotation_; + 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 - external_detection_rotation_.setRPY( + expected_rotation.setRPY( external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); - EXPECT_NEAR(pose.pose.position.x, 1.0 - external_detection_translation_y_, 0.01); - EXPECT_NEAR(pose.pose.position.y, 2.0 + external_detection_translation_x_, 0.01); - EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); - EXPECT_EQ(pose.header.frame_id, "base_link"); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); + 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) @@ -260,13 +280,13 @@ TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) SetStagingOffsets(); ConfigureAndActivateDock(); - SetDockToBaseLinkTransform(); - SetBaseLinkToOdomTransform(); + 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(), "test_dock"); + charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); charging_dock_->updateDockPoseAndPublish(); - charging_dock_->updateStagingPoseAndPublish("base_link"); + charging_dock_->updateStagingPoseAndPublish(kBaseFrame); }); ASSERT_TRUE( @@ -274,39 +294,41 @@ TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) ASSERT_TRUE( panther_utils::test_utils::WaitForMsg(node_, dock_pose_, std::chrono::milliseconds(100))); - tf2::Quaternion external_detection_rotation_; - + 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 - external_detection_rotation_.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); - - EXPECT_NEAR(dock_pose_->pose.position.x, 1.0 - external_detection_translation_y_, 0.01); - EXPECT_NEAR(dock_pose_->pose.position.y, 2.0 + external_detection_translation_x_, 0.01); - EXPECT_NEAR(dock_pose_->pose.position.z, 0.0, 0.01); - EXPECT_EQ(dock_pose_->header.frame_id, "base_link"); - EXPECT_NEAR( - tf2::getYaw(dock_pose_->pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); - - EXPECT_NEAR( - staging_pose_->pose.position.x, 1.0 - external_detection_translation_y_ - staging_x_offset_, - 0.01); - EXPECT_NEAR(staging_pose_->pose.position.y, 2.0 + external_detection_translation_x_, 0.01); - EXPECT_NEAR(staging_pose_->pose.position.z, 0.0, 0.01); - EXPECT_EQ(staging_pose_->header.frame_id, "base_link"); + 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)); - // It is 4.71 but tf2::getYaw moves by 2pi what is 6.28 - EXPECT_NEAR(tf2::getYaw(staging_pose_->pose.orientation), -1.57, 0.01); - - ASSERT_NO_THROW({ charging_dock_->updateStagingPoseAndPublish("odom"); }); + ASSERT_NO_THROW({ charging_dock_->updateStagingPoseAndPublish(kOdomFrame); }); ASSERT_TRUE( panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); - EXPECT_NEAR( - staging_pose_->pose.position.x, - 1.0 - external_detection_translation_y_ - staging_x_offset_ + 0.3, 0.01); - EXPECT_NEAR(staging_pose_->pose.position.y, 2.0 + external_detection_translation_x_ + 0.2, 0.01); - EXPECT_NEAR(staging_pose_->pose.position.z, 0.1, 0.01); - EXPECT_EQ(staging_pose_->header.frame_id, "odom"); + 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) @@ -316,17 +338,24 @@ TEST_F(TestPantherChargingDock, GetStagingPose) geometry_msgs::msg::PoseStamped pose; ASSERT_THROW( - { charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }, + { charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); }, opennav_docking_core::FailedToDetectDock); - SetDockToBaseLinkTransform(); + SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); ASSERT_NO_THROW( - { pose = charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); }); - EXPECT_NEAR(pose.pose.position.x, 1.0, 0.01); - EXPECT_NEAR(pose.pose.position.y, 2.0 + staging_x_offset_, 0.01); - EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 1.57 + staging_yaw_offset_, 0.01); - EXPECT_EQ(pose.header.frame_id, "base_link"); + { 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) @@ -337,22 +366,25 @@ TEST_F(TestPantherChargingDock, GetRefinedPose) geometry_msgs::msg::PoseStamped pose; ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); - SetDockToBaseLinkTransform(); + SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); + ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + 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); - - EXPECT_NEAR(pose.pose.position.x, 1.0 - external_detection_translation_y_, 0.01); - EXPECT_NEAR(pose.pose.position.y, 2.0 + external_detection_translation_x_, 0.01); - EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); - EXPECT_EQ(pose.header.frame_id, "base_link"); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), tf2::getYaw(external_detection_rotation_), 0.01); + 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) @@ -360,17 +392,13 @@ TEST_F(TestPantherChargingDock, IsDocked) SetDetectionOffsets(); ConfigureAndActivateDock(); - geometry_msgs::msg::PoseStamped pose; - SetDockToBaseLinkTransform(); - SetBaseLinkToOdomTransform(); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + 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()); - SetBaseLinkToOdomTransform(); - SetDockToBaseLinkTransform( - -external_detection_translation_y_, external_detection_translation_x_, - external_detection_translation_z_, -external_detection_yaw_); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), "test_dock"); + 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()); } diff --git a/panther_utils/include/panther_utils/tf2_utils.hpp b/panther_utils/include/panther_utils/tf2_utils.hpp index de7eee32c..4bd144574 100644 --- a/panther_utils/include/panther_utils/tf2_utils.hpp +++ b/panther_utils/include/panther_utils/tf2_utils.hpp @@ -17,9 +17,11 @@ #include #include +#include #include #include +#include namespace panther_utils::tf2_utils { @@ -83,6 +85,58 @@ geometry_msgs::msg::PoseStamped OffsetPose( return transformed_pose; } +/** + * @brief Gets the roll, pitch, and yaw from a quaternion. + * This function gets the roll, pitch, and yaw from a quaternion. + * Roll is rotation around the X axis. Pitch is rotation around the Y axis. Yaw is rotation around + * the Z axis. + * + * @param orientation The orientation quaternion. + * @return geometry_msgs::msg::Vector3 The roll, pitch, and yaw. + */ +geometry_msgs::msg::Vector3 GetRPY(const geometry_msgs::msg::Quaternion & orientation) +{ + geometry_msgs::msg::Vector3 v; + tf2::Quaternion q; + tf2::fromMsg(orientation, q); + tf2::Matrix3x3(q).getRPY(v.x, v.y, v.z); + return v; +} + +/** + * @brief Checks if two poses are near each other on the XY plane. + * This function checks if two poses are near each other on the XY plane within a given distance and + * angle tolerance. + * + * @param pose_1 The first pose. + * @param pose_2 The second pose. + * @param distance_tolerance The distance tolerance. + * @param angle_tolerance The angle tolerance. + * + * @return True if the poses are near each other on the XY plane, false otherwise. + */ +bool ArePosesNear( + const geometry_msgs::msg::PoseStamped & pose_1, const geometry_msgs::msg::PoseStamped & pose_2, + double distance_tolerance, double angle_tolerance) +{ + if (pose_1.header.frame_id.empty() || pose_2.header.frame_id.empty()) { + throw std::runtime_error("Provided frame IDs are empty, can't compare poses."); + } + + if (pose_1.header.frame_id != pose_2.header.frame_id) { + throw std::runtime_error("Provided frame IDs are different, can't compare poses."); + } + + auto pose_1_rpy = GetRPY(pose_1.pose.orientation); + auto pose_2_rpy = GetRPY(pose_2.pose.orientation); + + const double d = std::hypot( + pose_1.pose.position.x - pose_2.pose.position.x, + pose_1.pose.position.y - pose_2.pose.position.y); + return d < distance_tolerance && std::abs(pose_1_rpy.x - pose_2_rpy.x) < angle_tolerance && + std::abs(pose_1_rpy.y - pose_2_rpy.y) < angle_tolerance && + std::abs(pose_1_rpy.z - pose_2_rpy.z) < angle_tolerance; +} } // namespace panther_utils::tf2_utils #endif // PANTHER_UTILS_TF2_UTILS_HPP diff --git a/panther_utils/test/test_tf2_utils.cpp b/panther_utils/test/test_tf2_utils.cpp index 28ff0a6fe..e2bd1f526 100644 --- a/panther_utils/test/test_tf2_utils.cpp +++ b/panther_utils/test/test_tf2_utils.cpp @@ -12,104 +12,114 @@ // 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 "panther_utils/tf2_utils.hpp" +static constexpr char kBaseFrame[] = "base_link"; +static constexpr char kOdomFrame[] = "odom"; + class TestTransformPose : public ::testing::Test { public: TestTransformPose(); - ~TestTransformPose(); void SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp); protected: rclcpp::Clock::SharedPtr clock_; tf2_ros::Buffer::SharedPtr tf2_buffer_; -}; -void TestTransformPose::SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = stamp; - transform.header.frame_id = "odom"; - transform.child_frame_id = "base_link"; - transform.transform.translation.x = 0.3; - transform.transform.translation.y = 0.2; - transform.transform.translation.z = 0.1; - transform.transform.rotation.x = 0.0; - transform.transform.rotation.y = 0.0; - transform.transform.rotation.z = 0.0; - transform.transform.rotation.w = 1.0; - - tf2_buffer_->setTransform(transform, "unittest", false); -} + geometry_msgs::msg::TransformStamped transform_; +}; TestTransformPose::TestTransformPose() { - rclcpp::init(0, nullptr); - clock_ = std::make_shared(RCL_SYSTEM_TIME); tf2_buffer_ = std::make_shared(clock_); // Silence error about dedicated thread's being necessary tf2_buffer_->setUsingDedicatedThread(true); + + transform_.header.frame_id = kOdomFrame; + transform_.child_frame_id = kBaseFrame; + transform_.transform.translation.x = 0.3; + transform_.transform.translation.y = 0.2; + transform_.transform.translation.z = 0.1; + transform_.transform.rotation.x = 0.0; + transform_.transform.rotation.y = 0.0; + transform_.transform.rotation.z = 0.0; + transform_.transform.rotation.w = 1.0; } -TestTransformPose::~TestTransformPose() { rclcpp::shutdown(); } +void TestTransformPose::SetBaseLinkToOdomTransform(const builtin_interfaces::msg::Time & stamp) +{ + transform_.header.stamp = stamp; + tf2_buffer_->setTransform(transform_, "unittest", false); +} -TEST_F(TestTransformPose, NoTf) +TEST_F(TestTransformPose, EmptyFrame) { geometry_msgs::msg::PoseStamped pose; EXPECT_THROW( { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, ""); }, std::runtime_error); +} + +TEST_F(TestTransformPose, TransformMissing) +{ + geometry_msgs::msg::PoseStamped pose; + EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link"); }, + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame); }, std::runtime_error); - pose.header.frame_id = "odom"; + pose.header.frame_id = kOdomFrame; EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link"); }, + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame); }, std::runtime_error); } -TEST_F(TestTransformPose, FrameToFrameTransform) +TEST_F(TestTransformPose, TransformWithinSameFrame) { geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "odom"; + pose.header.frame_id = kOdomFrame; pose.pose.position.x = 0.1; pose.header.stamp = clock_->now(); ASSERT_NO_THROW( - { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "odom", 10.0); };); + { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kOdomFrame, 10.0); };); EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); EXPECT_NEAR(pose.pose.position.z, 0.0, 0.01); - EXPECT_EQ(pose.header.frame_id, "odom"); + EXPECT_EQ(pose.header.frame_id, kOdomFrame); EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); } -TEST_F(TestTransformPose, FrameToOtherFrameTransform) +TEST_F(TestTransformPose, TransformToDifferentFrame) { geometry_msgs::msg::PoseStamped pose; const auto stamp = clock_->now(); - pose.header.frame_id = "odom"; + pose.header.frame_id = kOdomFrame; pose.pose.position.x = 0.1; pose.header.stamp = stamp; SetBaseLinkToOdomTransform(stamp); ASSERT_NO_THROW( - { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link", 10.0); };); + { pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame, 10.0); };); EXPECT_NEAR(pose.pose.position.x, -0.2, 0.01); EXPECT_NEAR(pose.pose.position.y, -0.2, 0.01); EXPECT_NEAR(pose.pose.position.z, -0.1, 0.01); - EXPECT_EQ(pose.header.frame_id, "base_link"); + EXPECT_EQ(pose.header.frame_id, kBaseFrame); EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), 0.0, 0.01); } @@ -118,12 +128,12 @@ TEST_F(TestTransformPose, TestTimeout) const auto stamp = clock_->now(); SetBaseLinkToOdomTransform(stamp); geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = "odom"; + pose.header.frame_id = kOdomFrame; pose.header.stamp = stamp; pose.header.stamp.sec += 5; EXPECT_THROW( - { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, "base_link", 1.0); }, + { panther_utils::tf2_utils::TransformPose(tf2_buffer_, pose, kBaseFrame, 1.0); }, std::runtime_error); } @@ -187,3 +197,91 @@ TEST(OffsetPose, CheckDefinedPose) EXPECT_NEAR(pitch, 0.0, 0.01); EXPECT_NEAR(yaw, 0.0, 0.01); } + +TEST(TestArePosesNear, EmptyFrame) +{ + geometry_msgs::msg::PoseStamped pose_1; + geometry_msgs::msg::PoseStamped pose_2; + + EXPECT_THROW( + { panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.1); }, std::runtime_error); +} + +TEST(TestArePosesNear, DifferentFrames) +{ + geometry_msgs::msg::PoseStamped pose_1; + pose_1.header.frame_id = kBaseFrame; + geometry_msgs::msg::PoseStamped pose_2; + pose_2.header.frame_id = kOdomFrame; + + EXPECT_THROW( + { panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.1); }, std::runtime_error); +} + +TEST(TestArePosesNear, TooFarDistance) +{ + geometry_msgs::msg::PoseStamped pose_1; + pose_1.header.frame_id = kBaseFrame; + pose_1.pose.position.x = 0.1; + pose_1.pose.position.y = 0.1; + pose_1.pose.position.z = 0.0; + + geometry_msgs::msg::PoseStamped pose_2; + pose_2.header.frame_id = kBaseFrame; + pose_2.pose.position.x = 0.2; + pose_2.pose.position.y = 0.2; + pose_2.pose.position.z = 0.0; + + EXPECT_FALSE(panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.05, 0.05)); +} + +TEST(TestArePosesNear, TooFarAngle) +{ + geometry_msgs::msg::PoseStamped pose_1; + pose_1.header.frame_id = kBaseFrame; + tf2::Quaternion pose_1_rotation; + pose_1_rotation.setRPY(0.0, 0.0, 0.1); + pose_1.pose.orientation = tf2::toMsg(pose_1_rotation); + + geometry_msgs::msg::PoseStamped pose_2; + pose_2.header.frame_id = kBaseFrame; + tf2::Quaternion pose_2_rotation; + pose_2_rotation.setRPY(0.0, 0.0, 0.2); + pose_2.pose.orientation = tf2::toMsg(pose_2_rotation); + + EXPECT_FALSE(panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.1, 0.05)); +} + +TEST(TestArePosesNear, NearAngleAndDistance) +{ + geometry_msgs::msg::PoseStamped pose_1; + pose_1.header.frame_id = kBaseFrame; + pose_1.pose.position.x = 0.1; + pose_1.pose.position.y = 0.1; + pose_1.pose.position.z = 0.0; + tf2::Quaternion pose_1_rotation; + pose_1_rotation.setRPY(0.0, 0.0, 0.1); + pose_1.pose.orientation = tf2::toMsg(pose_1_rotation); + + geometry_msgs::msg::PoseStamped pose_2; + pose_2.header.frame_id = kBaseFrame; + pose_2.pose.position.x = 0.2; + pose_2.pose.position.y = 0.2; + pose_2.pose.position.z = 0.0; + tf2::Quaternion pose_2_rotation; + pose_2_rotation.setRPY(0.0, 0.0, 0.1); + pose_2.pose.orientation = tf2::toMsg(pose_2_rotation); + + EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose_1, pose_2, 0.2, 0.2)); +} + +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; +} From 12a084d2cd63239505be3f3b5fd0cd263942f997 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Sat, 24 Aug 2024 21:32:44 +0000 Subject: [PATCH 19/23] fix tf utils header guard Signed-off-by: Jakub Delicat --- panther_utils/include/panther_utils/tf2_utils.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_utils/include/panther_utils/tf2_utils.hpp b/panther_utils/include/panther_utils/tf2_utils.hpp index 4bd144574..54e5547ea 100644 --- a/panther_utils/include/panther_utils/tf2_utils.hpp +++ b/panther_utils/include/panther_utils/tf2_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_UTILS_TF2_UTILS_HPP -#define PANTHER_UTILS_TF2_UTILS_HPP +#ifndef PANTHER_UTILS_TF2_UTILS_HPP_ +#define PANTHER_UTILS_TF2_UTILS_HPP_ #include #include @@ -139,4 +139,4 @@ bool ArePosesNear( } } // namespace panther_utils::tf2_utils -#endif // PANTHER_UTILS_TF2_UTILS_HPP +#endif // PANTHER_UTILS_TF2_UTILS_HPP_ From f0b03962517dfad30a5c35e9bccae593084a062c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Aug 2024 08:19:18 +0000 Subject: [PATCH 20/23] Added comment Signed-off-by: Jakub Delicat --- panther_docking/src/panther_charging_dock.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index e495e9a33..7eeaa96ec 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -131,7 +131,11 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { std::string stage_frame = frame; - // When the pose if default the robot is docking so the frame is the dock 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. if (pose == geometry_msgs::msg::Pose()) { dock_frame_ = frame; stage_frame = base_frame_name_; From c00010d05a906bba8829ee947c7105b2cc866f4d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 26 Aug 2024 13:23:31 +0000 Subject: [PATCH 21/23] Added locking node Signed-off-by: Jakub Delicat --- .../panther_docking/panther_charging_dock.hpp | 10 ++- panther_docking/src/panther_charging_dock.cpp | 77 ++++++++++--------- .../test/test_panther_charging_dock.cpp | 20 ++++- .../panther_utils/common_utilities.hpp | 2 +- 4 files changed, 66 insertions(+), 43 deletions(-) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 65c51c3fb..fdef74de8 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -127,13 +127,17 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock protected: /** * @brief Method to declare parameters. + * + * @param node The node to declare parameters in. */ - void declareParameters(); + void declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); /** * @brief Method to get parameters. + * + * @param node The node to declare parameters in. */ - void getParameters(); + void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); /** * @brief Offset the staging pose. @@ -190,7 +194,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; tf2_ros::Buffer::SharedPtr tf2_buffer_; rclcpp::Publisher::SharedPtr staging_pose_pub_; diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 7eeaa96ec..0e13221dd 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -36,13 +36,14 @@ void PantherChargingDock::configure( tf2_buffer_ = tf; - node_ = parent.lock(); - if (!node_) { + node_ = parent; + auto node = node_.lock(); + if (!node) { throw std::runtime_error("Failed to lock node"); } - declareParameters(); - getParameters(); + declareParameters(node); + getParameters(node); pose_filter_ = std::make_unique( pose_filter_coef_, external_detection_timeout_); @@ -56,8 +57,9 @@ void PantherChargingDock::cleanup() void PantherChargingDock::activate() { - dock_pose_pub_ = node_->create_publisher("docking/dock_pose", 1); - staging_pose_pub_ = node_->create_publisher("docking/staging_pose", 1); + auto node = node_.lock(); + dock_pose_pub_ = node->create_publisher("docking/dock_pose", 1); + staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); } void PantherChargingDock::deactivate() @@ -66,65 +68,65 @@ void PantherChargingDock::deactivate() staging_pose_pub_.reset(); } -void PantherChargingDock::declareParameters() +void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { nav2_util::declare_parameter_if_not_declared( - node_, "panther_version", rclcpp::ParameterValue(1.0)); + node, "panther_version", rclcpp::ParameterValue(1.0)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".base_frame", rclcpp::ParameterValue("base_link")); + node, name_ + ".base_frame", rclcpp::ParameterValue("base_link")); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".external_detection_timeout", rclcpp::ParameterValue(0.2)); + 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)); + 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)); + 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)); + 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)); + 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, name_ + ".external_detection_rotation_pitch", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); + node, name_ + ".external_detection_rotation_roll", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); + node, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); + node, name_ + ".docking_yaw_threshold", rclcpp::ParameterValue(0.3)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); + 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)); + node, name_ + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); + node, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); } -void PantherChargingDock::getParameters() +void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { - node_->get_parameter(name_ + ".base_frame", base_frame_name_); - node_->get_parameter(name_ + ".external_detection_timeout", external_detection_timeout_); - node_->get_parameter( + node->get_parameter(name_ + ".base_frame", base_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( + node->get_parameter( name_ + ".external_detection_translation_y", external_detection_translation_y_); - node_->get_parameter( + 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); + 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_ + ".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_); + node->get_parameter(name_ + ".filter_coef", pose_filter_coef_); } PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( @@ -218,7 +220,10 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std:: try { PoseStampedMsg pose; pose.header.frame_id = frame; - pose.header.stamp = node_->get_clock()->now(); + { + 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); diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp index 422c384c8..71656ee38 100644 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ b/panther_docking/test/test_panther_charging_dock.cpp @@ -114,7 +114,6 @@ class TestPantherChargingDock : public testing::Test TestPantherChargingDock::TestPantherChargingDock() { - rclcpp::init(0, nullptr); node_ = std::make_shared("panther_charging_dock_test"); tf2_buffer_ = std::make_shared(node_->get_clock()); @@ -164,8 +163,6 @@ TestPantherChargingDock::~TestPantherChargingDock() if (executor_) { executor_->cancel(); } - - rclcpp::shutdown(); } void TestPantherChargingDock::ConfigureAndActivateDock() @@ -402,3 +399,20 @@ TEST_F(TestPantherChargingDock, IsDocked) 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_utils/include/panther_utils/common_utilities.hpp b/panther_utils/include/panther_utils/common_utilities.hpp index 475f602c4..21de8b9a7 100644 --- a/panther_utils/include/panther_utils/common_utilities.hpp +++ b/panther_utils/include/panther_utils/common_utilities.hpp @@ -64,7 +64,7 @@ std::fstream OpenFile(const std::string & file_path, const std::ios_base::openmo } /** - * @brief Checks if the Panther version meets the required version. + * @brief Checks if a version meets the required version. * * @param panther_version The Panther version. * @param version The required version. From e77e0d5fad9080d38ce9e5320ef98de879520bb6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 28 Aug 2024 10:45:37 +0200 Subject: [PATCH 22/23] Update docking to develop (#399) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Implement testing POC * Namespace refactor * Add EStop to Gazebo * unify CMakeLists.txt files * Add dependencies * Add remapping * Rename files in panther_diagnostics package * Update after changes in panther_diagnostics * Rename config and launch file in manager package * Correct include guards in manager package * Restructure files tree in manager tests * Ros2 estop sim gui (#384) * New format of documentation (#369) * Change 3 package for demo * Improve ROS_API * fix links * Update * Update * Table improvements * Format * Save work * Save work * update * fix * fix * fix * fix * fix * Add API warning * Improve links * lights simplify * Create CONFIGURATION.md files * Typos * pre-commit * Apply suggestions from code review Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Save work * Final unification * Delete trash * typos * Update README.md * Update ROS_API.md * Update ROS_API.md * Update README.md Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change initial warning to beta warning * improve warn rendering * rendering * Update Diagram * Add Dawid suggestions * Dot * Change diagram ext and typos * Do not describe external nodes * Add Dawid suggestons * Add last Dawid suggestions * Format * Pawel suggestions * Diagram improvements * Update * Diagram Visual --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Change scheme theme (#380) * unify CMakeLists.txt files (#381) * First working version * Ros2 increase bt service timeout (#382) * Parametrize and increase service timeout in managers * Format panther API drawio file * Add Estop GUI and docs --------- Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_gazebo/panther_hardware_plugins.xml Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> * Add david suggestion and change gui layout * Typos in Readme + estop publish on service call * Reorganize files in panther_lights * UPdate include guards in panther_lights * Reorganize files in panther_battery * Move estop to plugins folder * add nmea gps * Rename battery driver files * Rename shutdown hosts config * Inherit from IgnitionSystem * Change to Estop -> EStop * Reorganize panther_hardware_interfaces files * Dawid suggestions part 1 * Rename PantherSystem -> GzPantherSystem * Update references to files * Rename battery exec * Fix links in documentations (#387) * Refer to header files * Update panther_gazebo/include/panther_gazebo/gz_panther_system.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_gazebo/src/gz_panther_system.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Update panther_gazebo/src/gz_panther_system.cpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Dawid suggestions * delete typo * Minor modifications * Move BT plugins to src directory * Reorganize test utilities in hardware_interfaces * Merge remote-tracking branch 'origin/ros2-devel' into ros2-testing-poc * Add missing module configuration * Update panther_gazebo/include/panther_gazebo/gz_panther_system.hpp Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> * Dawid suggestions * update docs * Extend filesystem responsibility * Update ROS_API.md Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> * update names * Add new common utility functions * System monitor improvements * Round temperature precision * Implement filesystem unit tests * Formatting * Add integration tests condition * Update ROS_API.md * Review changes * Add pre-commit workflow (#395) --------- Co-authored-by: pawelirh Co-authored-by: rafal-gorecki Co-authored-by: Dawid Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- .github/workflows/pre-commit.yaml | 11 + .pre-commit-config.yaml | 8 +- README.md | 3 +- ROS_API.md | 34 +- panther/panther_simulation.repos | 2 +- panther_battery/CMakeLists.txt | 107 +- panther_battery/README.md | 2 +- .../{ => battery}/adc_battery.hpp | 8 +- .../panther_battery/{ => battery}/battery.hpp | 6 +- .../{ => battery}/roboteq_battery.hpp | 8 +- ...ttery_node.hpp => battery_driver_node.hpp} | 14 +- .../battery_publisher.hpp | 6 +- .../dual_battery_publisher.hpp | 10 +- .../single_battery_publisher.hpp | 10 +- panther_battery/launch/battery.launch.py | 2 +- .../src/{ => battery}/adc_battery.cpp | 2 +- .../src/{ => battery}/roboteq_battery.cpp | 2 +- ...ttery_node.cpp => battery_driver_node.cpp} | 27 +- .../battery_publisher.cpp | 2 +- .../dual_battery_publisher.cpp | 6 +- .../single_battery_publisher.cpp | 6 +- panther_battery/src/main.cpp | 6 +- .../test/{ => battery}/test_adc_battery.cpp | 4 +- .../test/{ => battery}/test_battery.cpp | 2 +- .../{ => battery}/test_roboteq_battery.cpp | 5 +- .../test_battery_publisher.cpp | 2 +- .../test_dual_battery_publisher.cpp | 6 +- .../test_single_battery_publisher.cpp | 6 +- ... => test_battery_driver_node_adc_dual.cpp} | 28 +- ...> test_battery_driver_node_adc_single.cpp} | 32 +- ...p => test_battery_driver_node_roboteq.cpp} | 16 +- .../test_battery_driver_node.hpp} | 23 +- panther_bringup/launch/bringup.launch.py | 8 +- panther_description/CMakeLists.txt | 2 +- .../panther_description.sh.in | 0 panther_description/urdf/gazebo.urdf.xacro | 17 +- .../urdf/panther_macro.urdf.xacro | 10 +- panther_diagnostics/CMakeLists.txt | 48 +- panther_diagnostics/README.md | 19 +- .../config/system_monitor.yaml | 26 + .../config/system_status_parameters.yaml | 44 - .../panther_diagnostics/filesystem.hpp | 115 ++ ...tatus_node.hpp => system_monitor_node.hpp} | 59 +- .../include/panther_diagnostics/types.hpp | 40 + ...tus.launch.py => system_monitor.launch.py} | 6 +- panther_diagnostics/package.xml | 3 + panther_diagnostics/src/main.cpp | 16 +- .../src/system_monitor_node.cpp | 199 +++ .../src/system_status_node.cpp | 169 --- .../integration/system_monitor_node.test.py | 70 + .../test/test_system_status_node.cpp | 142 -- .../test/unit/test_filesystem.cpp | 98 ++ .../test/unit/test_system_monitor_node.cpp | 188 +++ panther_gazebo/CMakeLists.txt | 75 +- panther_gazebo/CONFIGURATION.md | 2 +- panther_gazebo/README.md | 34 + panther_gazebo/config/gz_bridge.yaml | 6 +- panther_gazebo/config/led_strips.yaml | 4 +- .../config/teleop_with_estop.config | 1266 +++++++++++++++++ panther_gazebo/hooks/panther_gazebo.sh.in | 1 + .../panther_gazebo/gz_panther_system.hpp | 74 + .../include/panther_gazebo/plugins/e_stop.hpp | 64 + .../launch/simulate_robot.launch.py | 2 +- panther_gazebo/launch/simulation.launch.py | 33 +- panther_gazebo/package.xml | 9 +- panther_gazebo/panther_simulation_plugins.xml | 10 + panther_gazebo/src/gz_panther_system.cpp | 182 +++ panther_gazebo/src/plugins/EStop.qml | 88 ++ panther_gazebo/src/plugins/EStop.qrc | 5 + panther_gazebo/src/plugins/e_stop.cpp | 95 ++ panther_hardware_interfaces/CMakeLists.txt | 126 +- panther_hardware_interfaces/README.md | 2 +- .../panther_imu_sensor.hpp} | 6 +- .../gpio}/gpio_controller.hpp | 8 +- .../{ => panther_system/gpio}/gpio_driver.hpp | 6 +- .../motors_controller}/canopen_controller.hpp | 8 +- .../motors_controller}/motors_controller.hpp | 12 +- .../roboteq_data_converters.hpp | 8 +- .../motors_controller}/roboteq_driver.hpp | 6 +- .../roboteq_error_filter.hpp | 6 +- .../{ => panther_system}/panther_system.hpp | 16 +- .../panther_system_e_stop.hpp | 12 +- .../panther_system_ros_interface.hpp | 10 +- .../panther_imu_sensor.cpp} | 2 +- .../gpio}/gpio_controller.cpp | 2 +- .../{ => panther_system/gpio}/gpio_driver.cpp | 2 +- .../motors_controller}/canopen_controller.cpp | 2 +- .../motors_controller}/motors_controller.cpp | 8 +- .../roboteq_data_converters.cpp | 2 +- .../motors_controller}/roboteq_driver.cpp | 2 +- .../roboteq_error_filter.cpp | 2 +- .../{ => panther_system}/panther_system.cpp | 2 +- .../panther_system_e_stop.cpp | 2 +- .../panther_system_ros_interface.cpp | 4 +- .../include/panther_system_test_utils.hpp | 94 -- .../test_panther_imu_sensor.cpp} | 2 +- .../gpio}/test_gpio_controller.cpp | 2 +- .../gpio}/test_gpio_driver.cpp | 2 +- .../test_canopen_controller.cpp | 6 +- .../test_motors_controller.cpp | 10 +- .../test_roboteq_data_converters.cpp | 5 +- .../test_roboteq_driver.cpp | 8 +- .../test_roboteq_error_filter.cpp | 2 +- .../test_panther_system.cpp | 4 +- .../test_panther_system_ros_interface.cpp | 4 +- .../test/src/panther_system_test_utils.cpp | 119 -- .../test/src/roboteqs_mock.cpp | 200 --- .../test/utils/panther_system_test_utils.hpp | 164 +++ .../test/{include => utils}/roboteqs_mock.hpp | 175 ++- .../{include => utils}/test_constants.hpp | 10 +- panther_lights/CMakeLists.txt | 75 +- panther_lights/README.md | 4 +- .../panther_lights/animation/animation.hpp | 6 +- .../animation/charging_animation.hpp | 6 +- .../animation/image_animation.hpp | 6 +- .../include/panther_lights/apa102.hpp | 6 + .../led_animations_queue.hpp | 8 +- .../{ => led_components}/led_panel.hpp | 6 +- .../{ => led_components}/led_segment.hpp | 6 +- .../segment_converter.hpp | 10 +- .../panther_lights/lights_controller_node.hpp | 10 +- .../panther_lights/lights_driver_node.hpp | 4 +- panther_lights/launch/lights.launch.py | 4 +- .../led_animations_queue.cpp | 2 +- .../src/{ => led_components}/led_panel.cpp | 2 +- .../src/{ => led_components}/led_segment.cpp | 2 +- .../segment_converter.cpp | 6 +- panther_lights/src/lights_controller_node.cpp | 40 +- panther_lights/src/lights_driver_node.cpp | 33 +- .../test/{ => animation}/test_animation.cpp | 0 .../test_charging_animation.cpp | 0 .../{ => animation}/test_image_animation.cpp | 0 .../test_led_animation.cpp | 4 +- .../test_led_animations_queue.cpp | 4 +- .../{ => led_components}/test_led_panel.cpp | 2 +- .../{ => led_components}/test_led_segment.cpp | 2 +- .../test_segment_converter.cpp | 6 +- .../test/test_lights_controller_node.cpp | 58 +- .../test/test_lights_driver_node.cpp | 56 +- panther_localization/README.md | 2 + .../config/nmea_navsat_params.yaml | 7 + .../launch/localization.launch.py | 23 +- .../launch/nmea_navsat.launch.py | 79 + panther_localization/package.xml | 3 + panther_manager/CMakeLists.txt | 84 +- panther_manager/CONFIGURATION.md | 4 +- panther_manager/README.md | 10 +- ...anager_config.yaml => lights_manager.yaml} | 0 ...anager_config.yaml => safety_manager.yaml} | 0 ..._hosts.yaml => shutdown_hosts_config.yaml} | 0 .../action/call_set_bool_service_node.hpp | 6 +- .../call_set_led_animation_service_node.hpp | 6 +- .../action/call_trigger_service_node.hpp | 6 +- .../action/shutdown_hosts_from_file_node.hpp | 6 +- .../action/shutdown_single_host_node.hpp | 6 +- .../plugins/action/signal_shutdown_node.hpp | 6 +- .../decorator/tick_after_timeout_node.hpp | 6 +- .../panther_manager/plugins/shutdown_host.hpp | 6 +- .../plugins/shutdown_hosts_node.hpp | 6 +- ...manager_bt.launch.py => manager.launch.py} | 6 +- .../action/call_set_bool_service_node.cpp | 0 .../call_set_led_animation_service_node.cpp | 0 .../action/call_trigger_service_node.cpp | 0 .../action/shutdown_hosts_from_file_node.cpp | 0 .../action/shutdown_single_host_node.cpp | 0 .../plugins/action/signal_shutdown_node.cpp | 0 .../decorator/tick_after_timeout_node.cpp | 0 .../test_call_set_bool_service_node.cpp | 2 +- ...st_call_set_led_animation_service_node.cpp | 2 +- .../test_call_trigger_service_node.cpp | 2 +- .../test_shutdown_hosts_from_file_node.cpp | 2 +- .../test_shutdown_single_host_node.cpp | 2 +- .../test_signal_shutdown_node.cpp | 2 +- .../test_tick_after_timeout_node.cpp | 2 +- .../test/plugins/src/plugin_test_utils.cpp | 69 - .../test/test_lights_behavior_tree.cpp | 2 +- .../test/test_safety_behavior_tree.cpp | 2 +- .../behavior_tree_test_utils.hpp | 6 +- .../include => utils}/plugin_test_utils.hpp | 48 +- .../panther_utils/common_utilities.hpp | 41 + panther_utils/test/test_common_utilities.cpp | 76 +- 181 files changed, 4016 insertions(+), 1580 deletions(-) create mode 100644 .github/workflows/pre-commit.yaml rename panther_battery/include/panther_battery/{ => battery}/adc_battery.hpp (94%) rename panther_battery/include/panther_battery/{ => battery}/battery.hpp (97%) rename panther_battery/include/panther_battery/{ => battery}/roboteq_battery.hpp (91%) rename panther_battery/include/panther_battery/{battery_node.hpp => battery_driver_node.hpp} (83%) rename panther_battery/include/panther_battery/{ => battery_publisher}/battery_publisher.hpp (91%) rename panther_battery/include/panther_battery/{ => battery_publisher}/dual_battery_publisher.hpp (87%) rename panther_battery/include/panther_battery/{ => battery_publisher}/single_battery_publisher.hpp (82%) rename panther_battery/src/{ => battery}/adc_battery.cpp (99%) rename panther_battery/src/{ => battery}/roboteq_battery.cpp (98%) rename panther_battery/src/{battery_node.cpp => battery_driver_node.cpp} (87%) rename panther_battery/src/{ => battery_publisher}/battery_publisher.cpp (98%) rename panther_battery/src/{ => battery_publisher}/dual_battery_publisher.cpp (98%) rename panther_battery/src/{ => battery_publisher}/single_battery_publisher.cpp (94%) rename panther_battery/test/{ => battery}/test_adc_battery.cpp (98%) rename panther_battery/test/{ => battery}/test_battery.cpp (99%) rename panther_battery/test/{ => battery}/test_roboteq_battery.cpp (98%) rename panther_battery/test/{ => battery_publisher}/test_battery_publisher.cpp (98%) rename panther_battery/test/{ => battery_publisher}/test_dual_battery_publisher.cpp (98%) rename panther_battery/test/{ => battery_publisher}/test_single_battery_publisher.cpp (94%) rename panther_battery/test/{test_battery_node_dual_bat.cpp => test_battery_driver_node_adc_dual.cpp} (84%) rename panther_battery/test/{test_battery_node.cpp => test_battery_driver_node_adc_single.cpp} (81%) rename panther_battery/test/{test_battery_node_roboteq.cpp => test_battery_driver_node_roboteq.cpp} (88%) rename panther_battery/test/{include/test_battery_node.hpp => utils/test_battery_driver_node.hpp} (87%) rename panther_description/{env-hooks => hooks}/panther_description.sh.in (100%) create mode 100644 panther_diagnostics/config/system_monitor.yaml delete mode 100644 panther_diagnostics/config/system_status_parameters.yaml create mode 100644 panther_diagnostics/include/panther_diagnostics/filesystem.hpp rename panther_diagnostics/include/panther_diagnostics/{system_status_node.hpp => system_monitor_node.hpp} (51%) create mode 100644 panther_diagnostics/include/panther_diagnostics/types.hpp rename panther_diagnostics/launch/{system_status.launch.py => system_monitor.launch.py} (93%) create mode 100644 panther_diagnostics/src/system_monitor_node.cpp delete mode 100644 panther_diagnostics/src/system_status_node.cpp create mode 100644 panther_diagnostics/test/integration/system_monitor_node.test.py delete mode 100644 panther_diagnostics/test/test_system_status_node.cpp create mode 100644 panther_diagnostics/test/unit/test_filesystem.cpp create mode 100644 panther_diagnostics/test/unit/test_system_monitor_node.cpp create mode 100644 panther_gazebo/config/teleop_with_estop.config create mode 100644 panther_gazebo/hooks/panther_gazebo.sh.in create mode 100644 panther_gazebo/include/panther_gazebo/gz_panther_system.hpp create mode 100644 panther_gazebo/include/panther_gazebo/plugins/e_stop.hpp create mode 100644 panther_gazebo/panther_simulation_plugins.xml create mode 100644 panther_gazebo/src/gz_panther_system.cpp create mode 100644 panther_gazebo/src/plugins/EStop.qml create mode 100644 panther_gazebo/src/plugins/EStop.qrc create mode 100644 panther_gazebo/src/plugins/e_stop.cpp rename panther_hardware_interfaces/include/panther_hardware_interfaces/{panther_imu.hpp => panther_imu_sensor/panther_imu_sensor.hpp} (96%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/gpio}/gpio_controller.hpp (97%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/gpio}/gpio_driver.hpp (97%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/motors_controller}/canopen_controller.hpp (90%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/motors_controller}/motors_controller.hpp (88%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/motors_controller}/roboteq_data_converters.hpp (94%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/motors_controller}/roboteq_driver.hpp (94%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system/motors_controller}/roboteq_error_filter.hpp (91%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system}/panther_system.hpp (90%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system}/panther_system_e_stop.hpp (93%) rename panther_hardware_interfaces/include/panther_hardware_interfaces/{ => panther_system}/panther_system_ros_interface.hpp (96%) rename panther_hardware_interfaces/src/{panther_imu.cpp => panther_imu_sensor/panther_imu_sensor.cpp} (99%) rename panther_hardware_interfaces/src/{ => panther_system/gpio}/gpio_controller.cpp (99%) rename panther_hardware_interfaces/src/{ => panther_system/gpio}/gpio_driver.cpp (99%) rename panther_hardware_interfaces/src/{ => panther_system/motors_controller}/canopen_controller.cpp (98%) rename panther_hardware_interfaces/src/{ => panther_system/motors_controller}/motors_controller.cpp (95%) rename panther_hardware_interfaces/src/{ => panther_system/motors_controller}/roboteq_data_converters.cpp (98%) rename panther_hardware_interfaces/src/{ => panther_system/motors_controller}/roboteq_driver.cpp (99%) rename panther_hardware_interfaces/src/{ => panther_system/motors_controller}/roboteq_error_filter.cpp (96%) rename panther_hardware_interfaces/src/{ => panther_system}/panther_system.cpp (99%) rename panther_hardware_interfaces/src/{ => panther_system}/panther_system_e_stop.cpp (98%) rename panther_hardware_interfaces/src/{ => panther_system}/panther_system_ros_interface.cpp (98%) delete mode 100644 panther_hardware_interfaces/test/include/panther_system_test_utils.hpp rename panther_hardware_interfaces/test/{test_panther_imu.cpp => panther_imu_sensor/test_panther_imu_sensor.cpp} (99%) rename panther_hardware_interfaces/test/{ => panther_system/gpio}/test_gpio_controller.cpp (98%) rename panther_hardware_interfaces/test/{ => panther_system/gpio}/test_gpio_driver.cpp (98%) rename panther_hardware_interfaces/test/{ => panther_system/motors_controller}/test_canopen_controller.cpp (95%) rename panther_hardware_interfaces/test/{ => panther_system/motors_controller}/test_motors_controller.cpp (98%) rename panther_hardware_interfaces/test/{ => panther_system/motors_controller}/test_roboteq_data_converters.cpp (98%) rename panther_hardware_interfaces/test/{ => panther_system/motors_controller}/test_roboteq_driver.cpp (98%) rename panther_hardware_interfaces/test/{ => panther_system/motors_controller}/test_roboteq_error_filter.cpp (99%) rename panther_hardware_interfaces/test/{ => panther_system}/test_panther_system.cpp (99%) rename panther_hardware_interfaces/test/{ => panther_system}/test_panther_system_ros_interface.cpp (98%) delete mode 100644 panther_hardware_interfaces/test/src/panther_system_test_utils.cpp delete mode 100644 panther_hardware_interfaces/test/src/roboteqs_mock.cpp create mode 100644 panther_hardware_interfaces/test/utils/panther_system_test_utils.hpp rename panther_hardware_interfaces/test/{include => utils}/roboteqs_mock.hpp (54%) rename panther_hardware_interfaces/test/{include => utils}/test_constants.hpp (88%) rename panther_lights/include/panther_lights/{ => led_components}/led_animations_queue.hpp (95%) rename panther_lights/include/panther_lights/{ => led_components}/led_panel.hpp (89%) rename panther_lights/include/panther_lights/{ => led_components}/led_segment.hpp (96%) rename panther_lights/include/panther_lights/{ => led_components}/segment_converter.hpp (77%) rename panther_lights/src/{ => led_components}/led_animations_queue.cpp (98%) rename panther_lights/src/{ => led_components}/led_panel.cpp (97%) rename panther_lights/src/{ => led_components}/led_segment.cpp (98%) rename panther_lights/src/{ => led_components}/segment_converter.cpp (90%) rename panther_lights/test/{ => animation}/test_animation.cpp (100%) rename panther_lights/test/{ => animation}/test_charging_animation.cpp (100%) rename panther_lights/test/{ => animation}/test_image_animation.cpp (100%) rename panther_lights/test/{ => led_components}/test_led_animation.cpp (97%) rename panther_lights/test/{ => led_components}/test_led_animations_queue.cpp (98%) rename panther_lights/test/{ => led_components}/test_led_panel.cpp (98%) rename panther_lights/test/{ => led_components}/test_led_segment.cpp (99%) rename panther_lights/test/{ => led_components}/test_segment_converter.cpp (97%) create mode 100644 panther_localization/config/nmea_navsat_params.yaml create mode 100644 panther_localization/launch/nmea_navsat.launch.py rename panther_manager/config/{lights_manager_config.yaml => lights_manager.yaml} (100%) rename panther_manager/config/{safety_manager_config.yaml => safety_manager.yaml} (100%) rename panther_manager/config/{shutdown_hosts.yaml => shutdown_hosts_config.yaml} (100%) rename panther_manager/launch/{manager_bt.launch.py => manager.launch.py} (97%) rename panther_manager/{ => src}/plugins/action/call_set_bool_service_node.cpp (100%) rename panther_manager/{ => src}/plugins/action/call_set_led_animation_service_node.cpp (100%) rename panther_manager/{ => src}/plugins/action/call_trigger_service_node.cpp (100%) rename panther_manager/{ => src}/plugins/action/shutdown_hosts_from_file_node.cpp (100%) rename panther_manager/{ => src}/plugins/action/shutdown_single_host_node.cpp (100%) rename panther_manager/{ => src}/plugins/action/signal_shutdown_node.cpp (100%) rename panther_manager/{ => src}/plugins/decorator/tick_after_timeout_node.cpp (100%) rename panther_manager/test/plugins/{ => action}/test_call_set_bool_service_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_call_set_led_animation_service_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_call_trigger_service_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_shutdown_hosts_from_file_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_shutdown_single_host_node.cpp (99%) rename panther_manager/test/plugins/{ => action}/test_signal_shutdown_node.cpp (98%) rename panther_manager/test/plugins/{ => decorator}/test_tick_after_timeout_node.cpp (98%) delete mode 100644 panther_manager/test/plugins/src/plugin_test_utils.cpp rename panther_manager/test/{include => utils}/behavior_tree_test_utils.hpp (89%) rename panther_manager/test/{plugins/include => utils}/plugin_test_utils.hpp (79%) diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 000000000..08f7311e1 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,11 @@ +--- +name: Pre-Commit + +on: + push: + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 421c74955..c426d2101 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -34,7 +34,7 @@ repos: - id: cmake-format - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.6 + rev: v18.1.8 hooks: - id: clang-format @@ -63,13 +63,13 @@ repos: args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/PyCQA/flake8 - rev: 7.1.0 + rev: 7.1.1 hooks: - id: flake8 args: @@ -94,7 +94,7 @@ repos: exclude: ^.*\/CHANGELOG\.rst/.*$ - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.8.0 + rev: v0.10.0 hooks: - id: prettier-package-xml - id: sort-package-xml diff --git a/README.md b/README.md index 1e2ce7440..633e44f3e 100644 --- a/README.md +++ b/README.md @@ -94,6 +94,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🖥️ | `gz_headless_mode` | Run the simulation in headless mode. Useful when a GUI is not needed or to reduce the number of calculations.
***bool:*** `False` | | 🖥️ | `gz_log_level` | Adjust the level of console output.
***int:*** `1` (choices: `0`, `1`, `2`, `3`, `4`) | | 🖥️ | `gz_world` | Absolute path to SDF world file.
***string:*** [`husarion_world.sdf`](https://github.com/husarion/husarion_gz_worlds/blob/main/worlds/husarion_world.sdf) | +| 🤖 | `launch_nmea_gps` | Whether to launch the NMEA NavSat driver node. Advisable when the robot is equipped with the [ANT02](https://husarion.com/manuals/panther/panther-options/#ant02---wi-fi--lte--gps).
***bool:*** `False` | | 🤖🖥️ | `led_config_file` | Path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations.
***string:*** [`led_config.yaml`](./panther_lights/config/led_config.yaml) | | 🤖🖥️ | `lights_bt_project_path` | Path to BehaviorTree project file, responsible for lights management.
***string:*** [`PantherLightsBT.btproj`](./panther_manager/behavior_trees/PantherLightsBT.btproj) | | 🤖🖥️ | `localization_config_path` | Specify the path to the localization configuration file.
***string:*** [`relative_localization.yaml`](./panther_localization/config/relative_localization.yaml) | @@ -102,7 +103,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `publish_robot_state` | Whether to publish the default Panther robot description.
***bool:*** `True` | | 🖥️ | `robots` | The list of the robots spawned in the simulation e.g. `robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'`
***string:*** `''` | | 🤖🖥️ | `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.yaml`](./panther_manager/config/shutdown_hosts.yaml) | +| 🤖🖥️ | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown.
***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml) | | 🤖🖥️ | `use_ekf` | Enable or disable EKF.
***bool:*** `True` | | 🤖🖥️ | `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/ROS_API.md b/ROS_API.md index c7c91471e..9497bd700 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -1,15 +1,15 @@ # ROS API -> [!WARNING] +> [!IMPORTANT] > **Beta Release** > -> Please be advised that the software you are about to use is the Beta version of the ROS 2 Driver for Panther. It is functional and the architecture will not change significantly. It was tested by the Husarion team, however, some stability issues and bugs might still occur. Additionally, the ROS 2 API may face some minor changes in the following releases. +> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur. > -> We would be grateful for your feedback related to Panther ROS 2 driver. You can reach us the following ways: +> We would greatly appreciate your feedback regarding the Panther ROS 2 driver. You can reach us in the following ways: > -> - by email at [support@husarion.com](mailto:support@husarion.com) -> - via our community forum: [Husarion Community](https://community.husarion.com) -> - using issue request on [GitHub](https://github.com/husarion/panther_ros/issues) +> - By email at: [support@husarion.com](mailto:support@husarion.com) +> - Via our community forum: [Husarion Community](https://community.husarion.com) +> - By submitting an issue request on: [GitHub](https://github.com/husarion/panther_ros/issues) ## ROS 2 System Design @@ -42,23 +42,24 @@ Below is information about the physical robot API. For the simulation, topics an | | Node name | Description | | --- | ------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| 🤖 | `battery_driver` | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot.
[*panther_batter/battery_node*](./panther_batter) | +| 🤖 | `battery_driver` | Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier versions of the robot.
[*panther_batter/battery_driver_node*](./panther_battery/include/panther_battery/battery_driver_node.hpp) | | 🤖🖥️ | `controller_manager` | The Controller Manager performs two main functions. First, it manages controllers and their required interfaces, handling tasks like loading, activating, deactivating, and unloading. Second, it interacts with hardware components, ensuring access to their interfaces. This node manages the: `imu_broadcaster`, `joint_state_broadcaster`, `drive_controller`.
*[controller_manager/controller_manager](https://github.com/ros-controls/ros2_control/blob/master/controller_manager)* | | 🤖🖥️ | `drive_controller` | Manages mobile robots with a differential or mecanum drive depending on the configuration. It converts velocity commands for the robot body into wheel commands for the base. It also calculates odometry from hardware feedback and shares it.
*[diff_drive_controller/diff_drive_controller](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller) or [mecanum_drive_controller/mecanum_drive_controller](https://github.com/husarion/husarion_controllers/tree/main/mecanum_drive_controller)* | | 🤖🖥️ | `ekf_filter` | The Extended Kalman Filter node is designed to fuse odometry data from various sources, including wheel encoders, IMU, and GPS.
*[robot_localization/ekf_filter](https://github.com/cra-ros-pkg/robot_localization)* | -| 🤖 | `hardware_controller` | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities.
*[panther_hardware_interfaces/PantherSystem](./panther_hardware_interfaces)* | +| 🤖 | `hardware_controller` | Plugin responsible for communicating with engine controllers via the CAN bus and providing E-Stop functionalities.
*[panther_hardware_interfaces/PantherSystem](./panther_hardware_interfaces/src/panther_system/)* | +| 🤖 | `gps` | Node responsible for parsing NMEA strings and publishing standard ROS NavSat message types.
*[nmea_navsat_driver/nmea_navsat_driver](https://github.com/ros-drivers/nmea_navsat_driver/tree/ros2/src/libnmea_navsat_driver)* | | 🖥️ | `gz_ros2_control` | Responsible for integrating the ros2_control controller architecture with the Gazebo simulator.
[gz_ros2_control/gz_ros2_control](https://github.com/ros-controls/gz_ros2_control/tree/master/gz_ros2_control/src) | | 🤖🖥️ | `imu_broadcaster` | Publishes readings of IMU sensors.
*[imu_sensor_broadcaster/imu_sensor_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/imu_sensor_broadcaster)* | | 🤖🖥️ | `joint_state_broadcaster` | Reads all state interfaces and reports them on specific topics.
*[joint_state_broadcaster/joint_state_broadcaster](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)* | | 🤖🖥️ | `lights_container` | Node for managing ROS components. This node manages: `lights_controller`, `lights_driver`.
[*rclcpp_components/component_container*](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components) | -| 🤖🖥️ | `lights_controller` | This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights.
[*panther_lights/ControllerNode*](./panther_lights) | -| 🤖 | `lights_driver` | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
[*panther_lights/DriverNode*](./panther_lights) | -| 🤖🖥️ | `lights_manager` | Node responsible for managing Bumper Lights animation scheduling.
[panther_manager/lights_manager](./panther_lights/) | +| 🤖🖥️ | `lights_controller` | This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights.
[*panther_lights/LightsControllerNode*](./panther_lights/include/panther_lights/lights_controller_node.hpp) | +| 🤖 | `lights_driver` | This node is responsible for displaying frames on the Husarion Panther robot's Bumper Lights.
[*panther_lights/LightsDriverNode*](./panther_lights/include/panther_lights/lights_driver_node.hpp) | +| 🤖🖥️ | `lights_manager` | Node responsible for managing Bumper Lights animation scheduling.
[panther_manager/lights_manager](./panther_manager/include/panther_manager/lights_manager_node.hpp) | | 🤖🖥️⚙️ | `navsat_transform` | It converts raw GPS data into odometry data and publishes corrected GPS positions based on sensor data at a higher frequency.
*[robot_localization/navsat_transform](https://github.com/cra-ros-pkg/robot_localization)* | | 🖥️ | `panther_base_gz_bridge` | Convert and transmit data between ROS and Gazebo
*[ros_gz_bridge/parameter_bridge](https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge)* | | 🤖🖥️ | `robot_state_publisher` | Broadcasts a robot's state to tf2 using a provided URDF model and joint states. It updates the model and broadcasts poses for fixed and movable joints to tf2 topics.
*[robot_state_publisher/robot_state_publisher](https://github.com/ros/robot_state_publisher)* | -| 🤖 | `safety_manager` | Node responsible for managing safety features, and software shutdown of components.
*[panther_manager/safety_manager_node](./panther_manager)* | -| 🤖 | `system_monitor` | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature.
*[panther_diagnostic/system_status_node](./panther_diagnostics/)* | +| 🤖 | `safety_manager` | Node responsible for managing safety features, and software shutdown of components.
*[panther_manager/safety_manager_node](./panther_manager/include/panther_manager/safety_manager_node.hpp)* | +| 🤖 | `system_monitor` | Publishes system state of the Built-in Computer such as CPU usage, RAM memory usage, disk usage and CPU temperature.
*[panther_diagnostic/system_monitor_node](./panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp)* | ### Topics @@ -69,9 +70,11 @@ Below is information about the physical robot API. For the simulation, topics an | 🤖🖥️ | `cmd_vel` | Command velocity value.
[*geometry_msgs/Twist*](https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html) | | 🤖🖥️ | `diagnostics` | Diagnostic data.
[*diagnostic_msgs/DiagnosticArray*](https://docs.ros2.org/latest/api/diagnostic_msgs/msg/DiagnosticArray.html) | | 🤖🖥️ | `dynamic_joint_states` | Information about the state of various movable joints in a robotic system.
[*control_msgs/DynamicJointState*](https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/DynamicJointState.msg) | -| 🤖🖥️⚙️ | `gps/fix` | Raw GPS data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | | 🤖🖥️⚙️ | `gps/filtered` | Filtered GPS position after fusing odometry data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | -| 🤖 | `hardware/e_stop` | Current E-stop state.
[*std_msgs/Bool*](https://docs.ros.org/en/latest/api/std_msgs/html/msg/Bool.html) | +| 🤖🖥️⚙️ | `gps/fix` | Raw GPS data.
[*sensor_msgs/NavSatFix*](https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html) | +| 🤖 | `gps/time_reference` | The timestamp from the GPS device.
[*sensor_msgs/TimeReference*](https://docs.ros2.org/latest/api/sensor_msgs/msg/TimeReference.html) | +| 🤖 | `gps/vel` | Velocity output from the GPS device.
[*geometry_msgs/TwistStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/TwistStamped.html) | +| 🤖 | `hardware/e_stop` | Current E-stop state.
[*std_msgs/Bool*](https://docs.ros2.org/latest/api/std_msgs/msg/Bool.html). | | 🤖 | `hardware/io_state` | Current IO state.
[*panther_msgs/IOState*](https://github.com/husarion/panther_msgs) | | 🤖🖥️ | `hardware/motor_controllers_state` | Current motor controllers' state and error flags. Subscribed if using Roboteq motor controllers data.
[*panther_msgs/DriverState*](https://github.com/husarion/panther_msgs) | | 🤖🖥️ | `imu/data` | Filtered IMU data.
[*sensor_msgs/Imu*](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html) | @@ -92,6 +95,7 @@ Below is information about the physical robot API. For the simulation, topics an | --- | ------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | 🤖 | `_battery/battery_1_status_raw` | First battery raw state.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | | 🤖 | `_battery/battery_2_status_raw` | Second battery raw state. Published if second battery detected.
[*sensor_msgs/BatteryState*](https://docs.ros2.org/latest/api/sensor_msgs/msg/BatteryState.html) | +| 🤖 | `_gps/heading` | Not supported for current configuration.
[*geometry_msgs/QuaternionStamped*](https://docs.ros2.org/latest/api/geometry_msgs/msg/QuaternionStamped.html)| | 🤖🖥️⚙️ | `_odometry/gps` | Transformed raw GPS data to odometry format.
[*nav_msgs/Odometry*](https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html) | ### Services diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index b3e2233f7..83df44fa3 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -22,4 +22,4 @@ repositories: husarion_gz_worlds: type: git url: https://github.com/husarion/husarion_gz_worlds.git - version: bba5074af5eabf404850245a70a1d60192912a3b + version: 9d514a09c74bca2afbfba76cf2c87134918bbf97 diff --git a/panther_battery/CMakeLists.txt b/panther_battery/CMakeLists.txt index 37fd6c814..4e12bab87 100644 --- a/panther_battery/CMakeLists.txt +++ b/panther_battery/CMakeLists.txt @@ -15,17 +15,17 @@ endforeach() include_directories(include ${panther_utils_INCLUDE_DIRS}) add_executable( - battery_node + battery_driver_node src/main.cpp - src/battery_node.cpp - src/adc_battery.cpp - src/roboteq_battery.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp) -ament_target_dependencies(battery_node ${PACKAGE_DEPENDENCIES}) + src/battery_driver_node.cpp + src/battery/adc_battery.cpp + src/battery/roboteq_battery.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp) +ament_target_dependencies(battery_driver_node ${PACKAGE_DEPENDENCIES}) -install(TARGETS battery_node DESTINATION lib/${PROJECT_NAME}) +install(TARGETS battery_driver_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) @@ -39,7 +39,7 @@ if(BUILD_TESTING) PUBLIC $ $) - ament_add_gtest(${PROJECT_NAME}_test_battery test/test_battery.cpp) + ament_add_gtest(${PROJECT_NAME}_test_battery test/battery/test_battery.cpp) target_include_directories( ${PROJECT_NAME}_test_battery PUBLIC $ @@ -47,8 +47,8 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_battery rclcpp sensor_msgs panther_msgs) - ament_add_gtest(${PROJECT_NAME}_test_adc_battery test/test_adc_battery.cpp - src/adc_battery.cpp) + ament_add_gtest(${PROJECT_NAME}_test_adc_battery + test/battery/test_adc_battery.cpp src/battery/adc_battery.cpp) target_include_directories( ${PROJECT_NAME}_test_adc_battery PUBLIC $ @@ -56,8 +56,9 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_adc_battery rclcpp sensor_msgs panther_msgs) - ament_add_gtest(${PROJECT_NAME}_test_roboteq_battery - test/test_roboteq_battery.cpp src/roboteq_battery.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_roboteq_battery test/battery/test_roboteq_battery.cpp + src/battery/roboteq_battery.cpp) target_include_directories( ${PROJECT_NAME}_test_roboteq_battery PUBLIC $ @@ -65,8 +66,10 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_roboteq_battery panther_msgs rclcpp sensor_msgs) - ament_add_gtest(${PROJECT_NAME}_test_battery_publisher - test/test_battery_publisher.cpp src/battery_publisher.cpp) + ament_add_gtest( + ${PROJECT_NAME}_test_battery_publisher + test/battery_publisher/test_battery_publisher.cpp + src/battery_publisher/battery_publisher.cpp) target_include_directories( ${PROJECT_NAME}_test_battery_publisher PUBLIC $ @@ -76,8 +79,10 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_single_battery_publisher - test/test_single_battery_publisher.cpp src/adc_battery.cpp - src/battery_publisher.cpp src/single_battery_publisher.cpp) + test/battery_publisher/test_single_battery_publisher.cpp + src/battery/adc_battery.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp) target_include_directories( ${PROJECT_NAME}_test_single_battery_publisher PUBLIC $ @@ -87,8 +92,10 @@ if(BUILD_TESTING) ament_add_gtest( ${PROJECT_NAME}_test_dual_battery_publisher - test/test_dual_battery_publisher.cpp src/adc_battery.cpp - src/battery_publisher.cpp src/dual_battery_publisher.cpp) + test/battery_publisher/test_dual_battery_publisher.cpp + src/battery/adc_battery.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp) target_include_directories( ${PROJECT_NAME}_test_dual_battery_publisher PUBLIC $ @@ -97,51 +104,51 @@ if(BUILD_TESTING) ${PACKAGE_DEPENDENCIES}) ament_add_gtest( - ${PROJECT_NAME}_test_battery_node - test/test_battery_node.cpp - src/adc_battery.cpp - src/battery_node.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp - src/roboteq_battery.cpp) + ${PROJECT_NAME}_test_battery_driver_node_adc_dual + test/test_battery_driver_node_adc_dual.cpp + src/battery/adc_battery.cpp + src/battery_driver_node.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp + src/battery/roboteq_battery.cpp) target_include_directories( - ${PROJECT_NAME}_test_battery_node + ${PROJECT_NAME}_test_battery_driver_node_adc_dual PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_battery_node + ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_dual ${PACKAGE_DEPENDENCIES}) ament_add_gtest( - ${PROJECT_NAME}_test_battery_node_roboteq - test/test_battery_node_roboteq.cpp - src/adc_battery.cpp - src/battery_node.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp - src/roboteq_battery.cpp) + ${PROJECT_NAME}_test_battery_driver_node_adc_single + test/test_battery_driver_node_adc_single.cpp + src/battery/adc_battery.cpp + src/battery_driver_node.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp + src/battery/roboteq_battery.cpp) target_include_directories( - ${PROJECT_NAME}_test_battery_node_roboteq + ${PROJECT_NAME}_test_battery_driver_node_adc_single PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_battery_node_roboteq + ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_adc_single ${PACKAGE_DEPENDENCIES}) ament_add_gtest( - ${PROJECT_NAME}_test_battery_node_dual_bat - test/test_battery_node_dual_bat.cpp - src/adc_battery.cpp - src/battery_node.cpp - src/battery_publisher.cpp - src/dual_battery_publisher.cpp - src/single_battery_publisher.cpp - src/roboteq_battery.cpp) + ${PROJECT_NAME}_test_battery_driver_node_roboteq + test/test_battery_driver_node_roboteq.cpp + src/battery/adc_battery.cpp + src/battery_driver_node.cpp + src/battery_publisher/battery_publisher.cpp + src/battery_publisher/dual_battery_publisher.cpp + src/battery_publisher/single_battery_publisher.cpp + src/battery/roboteq_battery.cpp) target_include_directories( - ${PROJECT_NAME}_test_battery_node_dual_bat + ${PROJECT_NAME}_test_battery_driver_node_roboteq PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_battery_node_dual_bat + ament_target_dependencies(${PROJECT_NAME}_test_battery_driver_node_roboteq ${PACKAGE_DEPENDENCIES}) endif() diff --git a/panther_battery/README.md b/panther_battery/README.md index 2523e3b99..6b0e8b447 100644 --- a/panther_battery/README.md +++ b/panther_battery/README.md @@ -10,7 +10,7 @@ This package contains: ## ROS Nodes -### battery_node +### battery_driver_node Publishes battery state read from ADC unit for Panther version 1.2 and above, or based on Roboteq motor controllers' data for earlier.versions of the robot. diff --git a/panther_battery/include/panther_battery/adc_battery.hpp b/panther_battery/include/panther_battery/battery/adc_battery.hpp similarity index 94% rename from panther_battery/include/panther_battery/adc_battery.hpp rename to panther_battery/include/panther_battery/battery/adc_battery.hpp index 23e1ceb5f..6aebfa097 100644 --- a/panther_battery/include/panther_battery/adc_battery.hpp +++ b/panther_battery/include/panther_battery/battery/adc_battery.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_ADC_BATTERY_HPP_ -#define PANTHER_BATTERY_ADC_BATTERY_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ +#define PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ #include #include @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery.hpp" +#include "panther_battery/battery/battery.hpp" #include "panther_utils/moving_average.hpp" namespace panther_battery @@ -97,4 +97,4 @@ class ADCBattery : public Battery } // namespace panther_battery -#endif // PANTHER_BATTERY_ADC_BATTERY_HPP_ +#endif // PANTHER_BATTERY_BATTERY_ADC_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery.hpp b/panther_battery/include/panther_battery/battery/battery.hpp similarity index 97% rename from panther_battery/include/panther_battery/battery.hpp rename to panther_battery/include/panther_battery/battery/battery.hpp index 9146ad93b..eb067aa61 100644 --- a/panther_battery/include/panther_battery/battery.hpp +++ b/panther_battery/include/panther_battery/battery/battery.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_HPP_ -#define PANTHER_BATTERY_BATTERY_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_BATTERY_HPP_ +#define PANTHER_BATTERY_BATTERY_BATTERY_HPP_ #include #include @@ -129,4 +129,4 @@ class Battery } // namespace panther_battery -#endif // PANTHER_BATTERY_BATTERY_HPP_ +#endif // PANTHER_BATTERY_BATTERY_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/roboteq_battery.hpp b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp similarity index 91% rename from panther_battery/include/panther_battery/roboteq_battery.hpp rename to panther_battery/include/panther_battery/battery/roboteq_battery.hpp index 7daa3d6b7..a0019fc53 100644 --- a/panther_battery/include/panther_battery/roboteq_battery.hpp +++ b/panther_battery/include/panther_battery/battery/roboteq_battery.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_ROBOTEQ_BATTERY_HPP_ -#define PANTHER_BATTERY_ROBOTEQ_BATTERY_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ +#define PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ #include #include @@ -23,7 +23,7 @@ #include "panther_msgs/msg/driver_state.hpp" -#include "panther_battery/battery.hpp" +#include "panther_battery/battery/battery.hpp" #include "panther_utils/moving_average.hpp" namespace panther_battery @@ -76,4 +76,4 @@ class RoboteqBattery : public Battery } // namespace panther_battery -#endif // PANTHER_BATTERY_ROBOTEQ_BATTERY_HPP_ +#endif // PANTHER_BATTERY_BATTERY_ROBOTEQ_BATTERY_HPP_ diff --git a/panther_battery/include/panther_battery/battery_node.hpp b/panther_battery/include/panther_battery/battery_driver_node.hpp similarity index 83% rename from panther_battery/include/panther_battery/battery_node.hpp rename to panther_battery/include/panther_battery/battery_driver_node.hpp index f431b5993..c13371c9d 100644 --- a/panther_battery/include/panther_battery/battery_node.hpp +++ b/panther_battery/include/panther_battery/battery_driver_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_NODE_HPP_ -#define PANTHER_BATTERY_BATTERY_NODE_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ +#define PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ #include #include @@ -24,18 +24,18 @@ #include "panther_msgs/msg/driver_state.hpp" #include "panther_battery/adc_data_reader.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { using DriverStateMsg = panther_msgs::msg::DriverState; -class BatteryNode : public rclcpp::Node +class BatteryDriverNode : public rclcpp::Node { public: - BatteryNode( + BatteryDriverNode( const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: @@ -62,4 +62,4 @@ class BatteryNode : public rclcpp::Node } // namespace panther_battery -#endif // PANTHER_BATTERY_BATTERY_NODE_HPP_ +#endif // PANTHER_BATTERY_BATTERY_DRIVER_NODE_HPP_ diff --git a/panther_battery/include/panther_battery/battery_publisher.hpp b/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp similarity index 91% rename from panther_battery/include/panther_battery/battery_publisher.hpp rename to panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp index 30a110603..688f1899c 100644 --- a/panther_battery/include/panther_battery/battery_publisher.hpp +++ b/panther_battery/include/panther_battery/battery_publisher/battery_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_BATTERY_PUBLISHER_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ +#define PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ #include @@ -73,4 +73,4 @@ class BatteryPublisher } // namespace panther_battery -#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_HPP_ +#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/include/panther_battery/dual_battery_publisher.hpp b/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp similarity index 87% rename from panther_battery/include/panther_battery/dual_battery_publisher.hpp rename to panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp index 786952528..9890465fb 100644 --- a/panther_battery/include/panther_battery/dual_battery_publisher.hpp +++ b/panther_battery/include/panther_battery/battery_publisher/dual_battery_publisher.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_DUAL_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_DUAL_BATTERY_PUBLISHER_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ +#define PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ #include #include #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { @@ -68,4 +68,4 @@ class DualBatteryPublisher : public BatteryPublisher } // namespace panther_battery -#endif // PANTHER_BATTERY_DUAL_BATTERY_PUBLISHER_HPP_ +#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_DUAL_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/include/panther_battery/single_battery_publisher.hpp b/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp similarity index 82% rename from panther_battery/include/panther_battery/single_battery_publisher.hpp rename to panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp index 957258d31..29be9bc5c 100644 --- a/panther_battery/include/panther_battery/single_battery_publisher.hpp +++ b/panther_battery/include/panther_battery/battery_publisher/single_battery_publisher.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_SINGLE_BATTERY_PUBLISHER_HPP_ -#define PANTHER_BATTERY_SINGLE_BATTERY_PUBLISHER_HPP_ +#ifndef PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ +#define PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ #include #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { @@ -53,4 +53,4 @@ class SingleBatteryPublisher : public BatteryPublisher } // namespace panther_battery -#endif // PANTHER_BATTERY_SINGLE_BATTERY_PUBLISHER_HPP_ +#endif // PANTHER_BATTERY_BATTERY_PUBLISHER_SINGLE_BATTERY_PUBLISHER_HPP_ diff --git a/panther_battery/launch/battery.launch.py b/panther_battery/launch/battery.launch.py index 8a3d7d708..3719d5872 100644 --- a/panther_battery/launch/battery.launch.py +++ b/panther_battery/launch/battery.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): battery_driver_node = Node( package="panther_battery", - executable="battery_node", + executable="battery_driver_node", name="battery_driver", parameters=[{"panther_version": panther_version}], namespace=namespace, diff --git a/panther_battery/src/adc_battery.cpp b/panther_battery/src/battery/adc_battery.cpp similarity index 99% rename from panther_battery/src/adc_battery.cpp rename to panther_battery/src/battery/adc_battery.cpp index 7bd76a762..968f48236 100644 --- a/panther_battery/src/adc_battery.cpp +++ b/panther_battery/src/battery/adc_battery.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/adc_battery.hpp" +#include "panther_battery/battery/adc_battery.hpp" #include #include diff --git a/panther_battery/src/roboteq_battery.cpp b/panther_battery/src/battery/roboteq_battery.cpp similarity index 98% rename from panther_battery/src/roboteq_battery.cpp rename to panther_battery/src/battery/roboteq_battery.cpp index e9097f8e8..6653208ec 100644 --- a/panther_battery/src/roboteq_battery.cpp +++ b/panther_battery/src/battery/roboteq_battery.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/roboteq_battery.hpp" +#include "panther_battery/battery/roboteq_battery.hpp" #include #include diff --git a/panther_battery/src/battery_node.cpp b/panther_battery/src/battery_driver_node.cpp similarity index 87% rename from panther_battery/src/battery_node.cpp rename to panther_battery/src/battery_driver_node.cpp index 3e9ff8d68..d1ff8acd2 100644 --- a/panther_battery/src/battery_node.cpp +++ b/panther_battery/src/battery_driver_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/battery_node.hpp" +#include "panther_battery/battery_driver_node.hpp" #include #include @@ -24,18 +24,19 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -#include "panther_battery/adc_battery.hpp" #include "panther_battery/adc_data_reader.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" -#include "panther_battery/dual_battery_publisher.hpp" -#include "panther_battery/roboteq_battery.hpp" -#include "panther_battery/single_battery_publisher.hpp" +#include "panther_battery/battery/adc_battery.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery/roboteq_battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" +#include "panther_battery/battery_publisher/dual_battery_publisher.hpp" +#include "panther_battery/battery_publisher/single_battery_publisher.hpp" namespace panther_battery { -BatteryNode::BatteryNode(const std::string & node_name, const rclcpp::NodeOptions & options) +BatteryDriverNode::BatteryDriverNode( + const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), diagnostic_updater_(std::make_shared(this)) { RCLCPP_INFO(this->get_logger(), "Constructing node."); @@ -46,14 +47,14 @@ BatteryNode::BatteryNode(const std::string & node_name, const rclcpp::NodeOption // Running at 10 Hz battery_pub_timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), std::bind(&BatteryNode::BatteryPubTimerCB, this)); + std::chrono::milliseconds(100), std::bind(&BatteryDriverNode::BatteryPubTimerCB, this)); diagnostic_updater_->setHardwareID("Battery"); RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } -void BatteryNode::Initialize() +void BatteryDriverNode::Initialize() { RCLCPP_INFO(this->get_logger(), "Initializing."); @@ -74,7 +75,7 @@ void BatteryNode::Initialize() RCLCPP_INFO(this->get_logger(), "Initialized successfully."); } -void BatteryNode::InitializeWithADCBattery() +void BatteryDriverNode::InitializeWithADCBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with ADC data."); @@ -127,7 +128,7 @@ void BatteryNode::InitializeWithADCBattery() RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data."); } -void BatteryNode::InitializeWithRoboteqBattery() +void BatteryDriverNode::InitializeWithRoboteqBattery() { RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data."); @@ -151,7 +152,7 @@ void BatteryNode::InitializeWithRoboteqBattery() RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data."); } -void BatteryNode::BatteryPubTimerCB() +void BatteryDriverNode::BatteryPubTimerCB() { if (!battery_publisher_) { Initialize(); diff --git a/panther_battery/src/battery_publisher.cpp b/panther_battery/src/battery_publisher/battery_publisher.cpp similarity index 98% rename from panther_battery/src/battery_publisher.cpp rename to panther_battery/src/battery_publisher/battery_publisher.cpp index a6a910ff6..e3ccb14b8 100644 --- a/panther_battery/src/battery_publisher.cpp +++ b/panther_battery/src/battery_publisher/battery_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" #include #include diff --git a/panther_battery/src/dual_battery_publisher.cpp b/panther_battery/src/battery_publisher/dual_battery_publisher.cpp similarity index 98% rename from panther_battery/src/dual_battery_publisher.cpp rename to panther_battery/src/battery_publisher/dual_battery_publisher.cpp index af2c95ed6..fba8198d2 100644 --- a/panther_battery/src/dual_battery_publisher.cpp +++ b/panther_battery/src/battery_publisher/dual_battery_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/dual_battery_publisher.hpp" +#include "panther_battery/battery_publisher/dual_battery_publisher.hpp" #include #include @@ -24,8 +24,8 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" #include "panther_utils/ros_utils.hpp" namespace panther_battery diff --git a/panther_battery/src/single_battery_publisher.cpp b/panther_battery/src/battery_publisher/single_battery_publisher.cpp similarity index 94% rename from panther_battery/src/single_battery_publisher.cpp rename to panther_battery/src/battery_publisher/single_battery_publisher.cpp index de136258a..4916e647c 100644 --- a/panther_battery/src/single_battery_publisher.cpp +++ b/panther_battery/src/battery_publisher/single_battery_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_battery/single_battery_publisher.hpp" +#include "panther_battery/battery_publisher/single_battery_publisher.hpp" #include #include @@ -23,8 +23,8 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" namespace panther_battery { diff --git a/panther_battery/src/main.cpp b/panther_battery/src/main.cpp index f2b87ad71..b759fd33a 100644 --- a/panther_battery/src/main.cpp +++ b/panther_battery/src/main.cpp @@ -18,16 +18,16 @@ #include "rclcpp/rclcpp.hpp" -#include "panther_battery/battery_node.hpp" +#include "panther_battery/battery_driver_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto battery_node = std::make_shared("battery_driver"); + auto battery_driver_node = std::make_shared("battery_driver"); try { - rclcpp::spin(battery_node); + rclcpp::spin(battery_driver_node); } catch (const std::runtime_error & e) { std::cerr << "[battery_driver] Caught exception: " << e.what() << std::endl; } diff --git a/panther_battery/test/test_adc_battery.cpp b/panther_battery/test/battery/test_adc_battery.cpp similarity index 98% rename from panther_battery/test/test_adc_battery.cpp rename to panther_battery/test/battery/test_adc_battery.cpp index 1c24fcfd9..986f7cdb1 100644 --- a/panther_battery/test/test_adc_battery.cpp +++ b/panther_battery/test/battery/test_adc_battery.cpp @@ -24,7 +24,7 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/adc_battery.hpp" +#include "panther_battery/battery/adc_battery.hpp" #include "panther_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -157,8 +157,6 @@ TEST_F(TestADCBattery, BatteryMsgValues) const auto voltage_factor = 25.04255; const auto current_factor = 20.0; const auto charge_factor = 2.5; - const auto V_bat_full = 41.4; - const auto V_bat_min = 32.0; const float voltage_raw_1 = 1.5; const float current_raw_1 = 0.01; diff --git a/panther_battery/test/test_battery.cpp b/panther_battery/test/battery/test_battery.cpp similarity index 99% rename from panther_battery/test/test_battery.cpp rename to panther_battery/test/battery/test_battery.cpp index 0c189f926..cdbdf3812 100644 --- a/panther_battery/test/test_battery.cpp +++ b/panther_battery/test/battery/test_battery.cpp @@ -24,7 +24,7 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/battery.hpp" +#include "panther_battery/battery/battery.hpp" #include "panther_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; diff --git a/panther_battery/test/test_roboteq_battery.cpp b/panther_battery/test/battery/test_roboteq_battery.cpp similarity index 98% rename from panther_battery/test/test_roboteq_battery.cpp rename to panther_battery/test/battery/test_roboteq_battery.cpp index dfdb6548d..b7059e07c 100644 --- a/panther_battery/test/test_roboteq_battery.cpp +++ b/panther_battery/test/battery/test_roboteq_battery.cpp @@ -26,7 +26,7 @@ #include "panther_msgs/msg/driver_state.hpp" -#include "panther_battery/roboteq_battery.hpp" +#include "panther_battery/battery/roboteq_battery.hpp" #include "panther_utils/test/test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; @@ -155,9 +155,6 @@ TEST_F(TestRoboteqBattery, BatteryMsgUnknown) TEST_F(TestRoboteqBattery, BatteryMsgValues) { - const float V_bat_full = 41.4; - const float V_bat_min = 32.0; - const float voltage_1 = 35.0; const float current_1 = 0.1; UpdateBattery(voltage_1, current_1); diff --git a/panther_battery/test/test_battery_publisher.cpp b/panther_battery/test/battery_publisher/test_battery_publisher.cpp similarity index 98% rename from panther_battery/test/test_battery_publisher.cpp rename to panther_battery/test/battery_publisher/test_battery_publisher.cpp index 45f74a61a..4ed567b16 100644 --- a/panther_battery/test/test_battery_publisher.cpp +++ b/panther_battery/test/battery_publisher/test_battery_publisher.cpp @@ -24,7 +24,7 @@ #include "panther_msgs/msg/io_state.hpp" -#include "panther_battery/battery_publisher.hpp" +#include "panther_battery/battery_publisher/battery_publisher.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using IOStateMsg = panther_msgs::msg::IOState; diff --git a/panther_battery/test/test_dual_battery_publisher.cpp b/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp similarity index 98% rename from panther_battery/test/test_dual_battery_publisher.cpp rename to panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp index 529fbddf0..0740b7fa9 100644 --- a/panther_battery/test/test_dual_battery_publisher.cpp +++ b/panther_battery/test/battery_publisher/test_dual_battery_publisher.cpp @@ -24,9 +24,9 @@ #include "panther_msgs/msg/charging_status.hpp" -#include "panther_battery/adc_battery.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/dual_battery_publisher.hpp" +#include "panther_battery/battery/adc_battery.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/dual_battery_publisher.hpp" #include "panther_utils/test/ros_test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; diff --git a/panther_battery/test/test_single_battery_publisher.cpp b/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp similarity index 94% rename from panther_battery/test/test_single_battery_publisher.cpp rename to panther_battery/test/battery_publisher/test_single_battery_publisher.cpp index 345f85086..f7a04c3fa 100644 --- a/panther_battery/test/test_single_battery_publisher.cpp +++ b/panther_battery/test/battery_publisher/test_single_battery_publisher.cpp @@ -21,9 +21,9 @@ #include "sensor_msgs/msg/battery_state.hpp" -#include "panther_battery/adc_battery.hpp" -#include "panther_battery/battery.hpp" -#include "panther_battery/single_battery_publisher.hpp" +#include "panther_battery/battery/adc_battery.hpp" +#include "panther_battery/battery/battery.hpp" +#include "panther_battery/battery_publisher/single_battery_publisher.hpp" #include "panther_utils/test/ros_test_utils.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; diff --git a/panther_battery/test/test_battery_node_dual_bat.cpp b/panther_battery/test/test_battery_driver_node_adc_dual.cpp similarity index 84% rename from panther_battery/test/test_battery_node_dual_bat.cpp rename to panther_battery/test/test_battery_driver_node_adc_dual.cpp index 9571633f3..414fce6ba 100644 --- a/panther_battery/test/test_battery_node_dual_bat.cpp +++ b/panther_battery/test/test_battery_driver_node_adc_dual.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "include/test_battery_node.hpp" +#include "utils/test_battery_driver_node.hpp" #include @@ -22,16 +22,16 @@ #include "panther_utils/test/ros_test_utils.hpp" -class TestBatteryNodeDualBattery : public TestBatteryNode +class TestBatteryNodeADCDual : public TestBatteryNode { public: - TestBatteryNodeDualBattery() : TestBatteryNode(1.2, true) {} + TestBatteryNodeADCDual() : TestBatteryNode(1.2, true) {} }; -TEST_F(TestBatteryNodeDualBattery, BatteryValues) +TEST_F(TestBatteryNodeADCDual, BatteryValues) { ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // This is done to check if channels of ADC readers were assigned correctly, not to verify // calculations. If any test performing calculations fails this test will most likely fail too. @@ -52,7 +52,7 @@ TEST_F(TestBatteryNodeDualBattery, BatteryValues) // stops matching WriteNumberToFile(1600, std::filesystem::path(device1_path_ / "in_voltage3_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_2_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->voltage - battery_2_state_->voltage) < std::numeric_limits::epsilon()); @@ -68,7 +68,7 @@ TEST_F(TestBatteryNodeDualBattery, BatteryValues) WriteNumberToFile(900, std::filesystem::path(device1_path_ / "in_voltage1_raw")); WriteNumberToFile(100, std::filesystem::path(device0_path_ / "in_voltage2_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_2_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->current - battery_2_state_->current) < std::numeric_limits::epsilon()); @@ -76,16 +76,16 @@ TEST_F(TestBatteryNodeDualBattery, BatteryValues) WriteNumberToFile(1000, std::filesystem::path(device0_path_ / "in_voltage0_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_2_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_2_state_, std::chrono::milliseconds(1000))); EXPECT_FALSE( fabs(battery_1_state_->temperature - battery_2_state_->temperature) < std::numeric_limits::epsilon()); } -TEST_F(TestBatteryNodeDualBattery, BatteryTimeout) +TEST_F(TestBatteryNodeADCDual, BatteryTimeout) { ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg should have some values EXPECT_FALSE(std::isnan(battery_state_->voltage)); @@ -99,7 +99,7 @@ TEST_F(TestBatteryNodeDualBattery, BatteryTimeout) std::filesystem::remove(std::filesystem::path(device1_path_ / "in_voltage2_raw")); std::this_thread::sleep_for(std::chrono::milliseconds(2500)); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -111,18 +111,18 @@ TEST_F(TestBatteryNodeDualBattery, BatteryTimeout) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); } -TEST_F(TestBatteryNodeDualBattery, BatteryCharging) +TEST_F(TestBatteryNodeADCDual, BatteryCharging) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Publish charger connected state IOStateMsg io_state; io_state.charger_connected = true; io_state_pub_->publish(io_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING); } diff --git a/panther_battery/test/test_battery_node.cpp b/panther_battery/test/test_battery_driver_node_adc_single.cpp similarity index 81% rename from panther_battery/test/test_battery_node.cpp rename to panther_battery/test/test_battery_driver_node_adc_single.cpp index 1c24b5550..6ddd48b38 100644 --- a/panther_battery/test/test_battery_node.cpp +++ b/panther_battery/test/test_battery_driver_node_adc_single.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "include/test_battery_node.hpp" +#include "utils/test_battery_driver_node.hpp" #include @@ -22,7 +22,13 @@ #include "panther_utils/test/ros_test_utils.hpp" -TEST_F(TestBatteryNode, BatteryValues) +class TestBatteryNodeADCSingle : public TestBatteryNode +{ +public: + TestBatteryNodeADCSingle() : TestBatteryNode(1.2, false) {} +}; + +TEST_F(TestBatteryNodeADCSingle, BatteryValues) { // Change some values for ADC channels used by second battery. // As this is test for single battery this should not affect published battery state. @@ -30,7 +36,7 @@ TEST_F(TestBatteryNode, BatteryValues) WriteNumberToFile(100, std::filesystem::path(device0_path_ / "in_voltage2_raw")); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // This is done to check if channels of ADC readers were assigned correctly, not to verify // calculations. If any test performing calculations fails this test will most likely fail too. @@ -48,10 +54,10 @@ TEST_F(TestBatteryNode, BatteryValues) EXPECT_FLOAT_EQ(battery_1_state_->charge, battery_state_->charge); } -TEST_F(TestBatteryNode, BatteryTimeout) +TEST_F(TestBatteryNodeADCSingle, BatteryTimeout) { ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg should have some values EXPECT_FALSE(std::isnan(battery_state_->voltage)); @@ -65,7 +71,7 @@ TEST_F(TestBatteryNode, BatteryTimeout) std::filesystem::remove(std::filesystem::path(device1_path_ / "in_voltage2_raw")); std::this_thread::sleep_for(std::chrono::milliseconds(2500)); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -77,41 +83,41 @@ TEST_F(TestBatteryNode, BatteryTimeout) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); } -TEST_F(TestBatteryNode, BatteryCharging) +TEST_F(TestBatteryNodeADCSingle, BatteryCharging) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Publish charger connected state IOStateMsg io_state; io_state.charger_connected = true; io_state_pub_->publish(io_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); EXPECT_NE(battery_state_->power_supply_status, BatteryStateMsg::POWER_SUPPLY_STATUS_DISCHARGING); } -TEST_F(TestBatteryNode, RoboteqInitOnADCFail) +TEST_F(TestBatteryNodeADCSingle, RoboteqInitOnADCFail) { // Remove ADC device std::filesystem::remove_all(device0_path_); // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state status should be UNKNOWN EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status); // Publish driver state that should update the battery message DriverStateMsg driver_state; - driver_state.header.stamp = battery_node_->get_clock()->now(); + driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state_pub_->publish(driver_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state status should be different than UNKNOWN EXPECT_NE(BatteryStateMsg::POWER_SUPPLY_STATUS_UNKNOWN, battery_state_->power_supply_status); diff --git a/panther_battery/test/test_battery_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp similarity index 88% rename from panther_battery/test/test_battery_node_roboteq.cpp rename to panther_battery/test/test_battery_driver_node_roboteq.cpp index 4240792ad..c4cf8f9c5 100644 --- a/panther_battery/test/test_battery_node_roboteq.cpp +++ b/panther_battery/test/test_battery_driver_node_roboteq.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "include/test_battery_node.hpp" +#include "utils/test_battery_driver_node.hpp" #include #include @@ -34,7 +34,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -45,7 +45,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) EXPECT_EQ(BatteryStateMsg::POWER_SUPPLY_HEALTH_UNKNOWN, battery_state_->power_supply_health); DriverStateMsg driver_state; - driver_state.header.stamp = battery_node_->get_clock()->now(); + driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state.front.voltage = 35.0f; driver_state.rear.voltage = 35.0f; driver_state.front.current = 0.1f; @@ -53,7 +53,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryValues) driver_state_pub_->publish(driver_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // This is done to check if values were read correctly, not to verify calculations. // If any test performing calculations fails this test will most likely fail too. @@ -71,7 +71,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) { // Wait for node to initialize ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(5000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(5000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); @@ -83,7 +83,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) // Publish some values DriverStateMsg driver_state; - driver_state.header.stamp = battery_node_->get_clock()->now(); + driver_state.header.stamp = battery_driver_node_->get_clock()->now(); driver_state.front.voltage = 35.0f; driver_state.rear.voltage = 35.0f; driver_state.front.current = 0.1f; @@ -91,7 +91,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) driver_state_pub_->publish(driver_state); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg should have some values EXPECT_FALSE(std::isnan(battery_state_->voltage)); @@ -102,7 +102,7 @@ TEST_F(TestBatteryNodeRoboteq, BatteryTimeout) // Wait for timeout std::this_thread::sleep_for(std::chrono::milliseconds(1500)); ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - battery_node_, battery_state_, std::chrono::milliseconds(1000))); + battery_driver_node_, battery_state_, std::chrono::milliseconds(1000))); // Battery state msg values should be NaN EXPECT_TRUE(std::isnan(battery_state_->voltage)); diff --git a/panther_battery/test/include/test_battery_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp similarity index 87% rename from panther_battery/test/include/test_battery_node.hpp rename to panther_battery/test/utils/test_battery_driver_node.hpp index a8a89305e..9b2385b2b 100644 --- a/panther_battery/test/include/test_battery_node.hpp +++ b/panther_battery/test/utils/test_battery_driver_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_BATTERY_TEST_BATTERY_NODE_ -#define PANTHER_BATTERY_TEST_BATTERY_NODE_ +#ifndef PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ +#define PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ #include #include @@ -31,7 +31,7 @@ #include "panther_msgs/msg/driver_state.hpp" #include "panther_msgs/msg/io_state.hpp" -#include "panther_battery/battery_node.hpp" +#include "panther_battery/battery_driver_node.hpp" using BatteryStateMsg = sensor_msgs::msg::BatteryState; using DriverStateMsg = panther_msgs::msg::DriverState; @@ -55,7 +55,7 @@ class TestBatteryNode : public testing::Test BatteryStateMsg::SharedPtr battery_state_; BatteryStateMsg::SharedPtr battery_1_state_; BatteryStateMsg::SharedPtr battery_2_state_; - std::shared_ptr battery_node_; + std::shared_ptr battery_driver_node_; rclcpp::Subscription::SharedPtr battery_sub_; rclcpp::Subscription::SharedPtr battery_1_sub_; rclcpp::Subscription::SharedPtr battery_2_sub_; @@ -104,21 +104,22 @@ TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_ba rclcpp::NodeOptions options; options.parameter_overrides(params); - battery_node_ = std::make_shared("battery_driver", options); + battery_driver_node_ = std::make_shared( + "battery_driver", options); - battery_sub_ = battery_node_->create_subscription( + battery_sub_ = battery_driver_node_->create_subscription( "battery/battery_status", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_state_ = msg; }); - battery_1_sub_ = battery_node_->create_subscription( + battery_1_sub_ = battery_driver_node_->create_subscription( "_battery/battery_1_status_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_1_state_ = msg; }); - battery_2_sub_ = battery_node_->create_subscription( + battery_2_sub_ = battery_driver_node_->create_subscription( "_battery/battery_2_status_raw", 10, [&](const BatteryStateMsg::SharedPtr msg) { battery_2_state_ = msg; }); - io_state_pub_ = battery_node_->create_publisher( + io_state_pub_ = battery_driver_node_->create_publisher( "hardware/io_state", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - driver_state_pub_ = battery_node_->create_publisher( + driver_state_pub_ = battery_driver_node_->create_publisher( "hardware/motor_controllers_state", 10); } @@ -145,4 +146,4 @@ void TestBatteryNode::WriteNumberToFile(const T number, const std::string & file } } -#endif // PANTHER_BATTERY_TEST_BATTERY_NODE_ +#endif // PANTHER_BATTERY_UTILS_TEST_BATTERY_DRIVER_NODE_HPP_ diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 20fd2664b..96fc04453 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -64,13 +64,13 @@ def generate_launch_description(): launch_arguments={"namespace": namespace}.items(), ) - system_status_launch = IncludeLaunchDescription( + system_monitor_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ FindPackageShare("panther_diagnostics"), "launch", - "system_status.launch.py", + "system_monitor.launch.py", ] ), ), @@ -108,7 +108,7 @@ def generate_launch_description(): manager_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [FindPackageShare("panther_manager"), "launch", "manager_bt.launch.py"] + [FindPackageShare("panther_manager"), "launch", "manager.launch.py"] ) ), condition=UnlessCondition(disable_manager), @@ -131,7 +131,7 @@ def generate_launch_description(): declare_use_ekf_arg, welcome_msg, controller_launch, - system_status_launch, + system_monitor_launch, delayed_action, ] diff --git a/panther_description/CMakeLists.txt b/panther_description/CMakeLists.txt index b0c336d73..87ff28f03 100644 --- a/panther_description/CMakeLists.txt +++ b/panther_description/CMakeLists.txt @@ -7,6 +7,6 @@ install(DIRECTORY config launch meshes rviz urdf DESTINATION share/${PROJECT_NAME}) ament_environment_hooks( - "${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") ament_package() diff --git a/panther_description/env-hooks/panther_description.sh.in b/panther_description/hooks/panther_description.sh.in similarity index 100% rename from panther_description/env-hooks/panther_description.sh.in rename to panther_description/hooks/panther_description.sh.in diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro index b8b2afdbb..e34de17a8 100644 --- a/panther_description/urdf/gazebo.urdf.xacro +++ b/panther_description/urdf/gazebo.urdf.xacro @@ -6,12 +6,7 @@ - - - - - - + @@ -42,6 +37,9 @@ ${config_file} ${namespace} + gz_ros2_control/e_stop:=hardware/e_stop + gz_ros2_control/e_stop_reset:=hardware/e_stop_reset + gz_ros2_control/e_stop_trigger:=hardware/e_stop_trigger imu_broadcaster/imu:=imu/data drive_controller/cmd_vel_unstamped:=cmd_vel drive_controller/odom:=odometry/wheels @@ -65,12 +63,7 @@ - - - - - - + diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro index 60b90ff09..fc6da39ee 100644 --- a/panther_description/urdf/panther_macro.urdf.xacro +++ b/panther_description/urdf/panther_macro.urdf.xacro @@ -17,12 +17,7 @@ - - - - - - + @@ -62,7 +57,8 @@ - ign_ros2_control/IgnitionSystem + panther_gazebo/GzPantherSystem + true diff --git a/panther_diagnostics/CMakeLists.txt b/panther_diagnostics/CMakeLists.txt index c94d1c1e4..8794efbed 100644 --- a/panther_diagnostics/CMakeLists.txt +++ b/panther_diagnostics/CMakeLists.txt @@ -37,33 +37,45 @@ set(ENV{PKG_CONFIG_PATH} "${CPPUPROFILE_PREFIX}/lib:$ENV{PKG_CONFIG_PATH}") pkg_check_modules(CPPUPROFILE REQUIRED IMPORTED_TARGET cppuprofile) -generate_parameter_library(system_status_parameters - config/system_status_parameters.yaml) +generate_parameter_library(system_monitor_parameters config/system_monitor.yaml) -add_executable(system_status_node src/main.cpp src/system_status_node.cpp) -target_include_directories(system_status_node +add_executable(system_monitor_node src/main.cpp src/system_monitor_node.cpp) +target_include_directories(system_monitor_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include) -ament_target_dependencies(system_status_node ${PACKAGE_DEPENDENCIES}) -target_link_libraries(system_status_node system_status_parameters +ament_target_dependencies(system_monitor_node ${PACKAGE_DEPENDENCIES}) +target_link_libraries(system_monitor_node system_monitor_parameters PkgConfig::CPPUPROFILE) -install(TARGETS system_status_node DESTINATION lib/${PROJECT_NAME}) +install(TARGETS system_monitor_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - - ament_add_gtest(${PROJECT_NAME}_test_system_status - test/test_system_status_node.cpp src/system_status_node.cpp) - target_include_directories( - ${PROJECT_NAME}_test_system_status - PUBLIC $ - $ ${CMAKE_INSTALL_PREFIX}/include) - ament_target_dependencies(${PROJECT_NAME}_test_system_status + find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) + + # Unit tests + ament_add_gmock( + ${PROJECT_NAME}_test_system_monitor test/unit/test_system_monitor_node.cpp + src/system_monitor_node.cpp) + target_include_directories(${PROJECT_NAME}_test_system_monitor + PUBLIC ${CMAKE_INSTALL_PREFIX}/include) + ament_target_dependencies(${PROJECT_NAME}_test_system_monitor + ament_cmake_gtest ${PACKAGE_DEPENDENCIES}) + target_link_libraries(${PROJECT_NAME}_test_system_monitor + system_monitor_parameters PkgConfig::CPPUPROFILE) + + ament_add_gmock(${PROJECT_NAME}_test_filesystem test/unit/test_filesystem.cpp) + target_include_directories(${PROJECT_NAME}_test_filesystem + PUBLIC ${CMAKE_INSTALL_PREFIX}/include) + ament_target_dependencies(${PROJECT_NAME}_test_filesystem ament_cmake_gtest ${PACKAGE_DEPENDENCIES}) - target_link_libraries(${PROJECT_NAME}_test_system_status - system_status_parameters PkgConfig::CPPUPROFILE) + + # Integration tests + option(TEST_INTEGRATION "Run integration tests" ON) + if(TEST_INTEGRATION) + add_ros_test(test/integration/system_monitor_node.test.py) + endif() endif() diff --git a/panther_diagnostics/README.md b/panther_diagnostics/README.md index e4d412464..d3e213bbd 100644 --- a/panther_diagnostics/README.md +++ b/panther_diagnostics/README.md @@ -4,28 +4,27 @@ Package containing nodes monitoring and publishing the Built-in Computer status ## Launch Files -- `system_status.launch.py`: Launch a node that analyzes the state of the most important components in the robot +- `system_monitor.launch.py`: Launch a node that analyzes the state of the most important components in the robot ## Configuration Files -- [`system_status_parameters.yaml`](./config/system_status_parameters.yaml): Defines parameters for `system_status_node`. +- [`system_monitor.yaml`](./config/system_monitor.yaml): Defines parameters for `system_monitor_node`. ## ROS Nodes -- [`system_status_node`](#system_status_node): Publishes system state of the Built-in Computer such as CPU usage, RAM usage, disk usage and CPU temperature. +### system_monitor_node -### system_status_node +Publishes the built-in computer system status , monitoring parameters as such as CPU usage, RAM usage, disk usage, and CPU temperature. #### Publishes -- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System status diagnostic messages. -- `system_status` [*panther_msgs/SystemStatus*]: State of the system, including Built-in Computer's CPU temperature and load. +- `diagnostics` [*diagnostic_msgs/DiagnosticArray*]: System monitor diagnostic messages. +- `system_status` [*panther_msgs/SystemStatus*]: Built-in computer system status, includes the most important computation-related parameters. #### Parameters -- `~frame_id` [*string*, default: **built_in_computer**]: Frame where computer is located. -- `~publish_rate` [*double*, default: **0.25**]: System status publish rate in seconds. -- `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. -- `~memory_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. - `~cpu_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for CPU usage warning in percentage. - `~cpu_temperature_warn_threshold` [*float*, default: **80.0**]: Threshold for CPU temperature warning in degrees Celsius. +- `~ram_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for memory usage warning in percentage. +- `~disk_usage_warn_threshold` [*float*, default: **95.0**]: Threshold for disk usage warning in percentage. +- `~publish_frequency` [*double*, default: **5.0**]: System status publishing frequency [Hz]. diff --git a/panther_diagnostics/config/system_monitor.yaml b/panther_diagnostics/config/system_monitor.yaml new file mode 100644 index 000000000..7253eea5b --- /dev/null +++ b/panther_diagnostics/config/system_monitor.yaml @@ -0,0 +1,26 @@ +system_monitor: + cpu_usage_warn_threshold: + type: double + default_value: 95.0 + description: Threshold for CPU usage warning in percentage. + validation: { bounds<>: [0.0, 100.0] } + cpu_temperature_warn_threshold: + type: double + default_value: 80.0 + description: Threshold for CPU temperature warning in degrees Celsius. + validation: { bounds<>: [0.0, 1000.0] } + ram_usage_warn_threshold: + type: double + default_value: 95.0 + description: Threshold for memory usage warning in percentage. + validation: { bounds<>: [0.0, 100.0] } + disk_usage_warn_threshold: + type: double + default_value: 95.0 + description: Threshold for disk usage warning in percentage. + validation: { bounds<>: [0.0, 100.0] } + publish_frequency: + type: double + default_value: 5.0 + description: System status publishing frequency [Hz]. + validation: { bounds<>: [0.1, 10.0] } diff --git a/panther_diagnostics/config/system_status_parameters.yaml b/panther_diagnostics/config/system_status_parameters.yaml deleted file mode 100644 index 0fddc5f4e..000000000 --- a/panther_diagnostics/config/system_status_parameters.yaml +++ /dev/null @@ -1,44 +0,0 @@ -system_status: - frame_id: - type: string - default_value: built_in_computer - description: Frame where computer is located. - hardware_id: - type: string - default_value: Built-in Computer - description: Name of hardware used in diagnostics. - publish_rate: - type: double - default_value: 0.25 - description: System status publish rate in seconds. - validation: { - bounds<>: [0.0, 1000.0] - } - disk_usage_warn_threshold: - type: double - default_value: 95.0 - description: Threshold for disk usage warning in percentage. - validation: { - bounds<>: [0.0, 100.0] - } - memory_usage_warn_threshold: - type: double - default_value: 95.0 - description: Threshold for memory usage warning in percentage. - validation: { - bounds<>: [0.0, 100.0] - } - cpu_usage_warn_threshold: - type: double - default_value: 95.0 - description: Threshold for CPU usage warning in percentage. - validation: { - bounds<>: [0.0, 100.0] - } - cpu_temperature_warn_threshold: - type: double - default_value: 80.0 - description: Threshold for CPU temperature warning in degrees Celsius. - validation: { - bounds<>: [0.0, 1000.0] - } diff --git a/panther_diagnostics/include/panther_diagnostics/filesystem.hpp b/panther_diagnostics/include/panther_diagnostics/filesystem.hpp new file mode 100644 index 000000000..fa2291d62 --- /dev/null +++ b/panther_diagnostics/include/panther_diagnostics/filesystem.hpp @@ -0,0 +1,115 @@ +// 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_DIAGNOSTICS_FILESYSTEM_HPP_ +#define PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ + +#include +#include + +namespace panther_diagnostics +{ + +/** + * @brief Abstract interface for the filesystem methods. + */ +class FilesystemInterface +{ +public: + /** + * @brief Virtual destructor for the FilesystemInterface class. + */ + virtual ~FilesystemInterface() = default; + + virtual uintmax_t GetSpaceCapacity(const std::string & filesystem_path) const = 0; + + virtual uintmax_t GetSpaceAvailable(const std::string & filesystem_path) const = 0; + + virtual std::string ReadFile(const std::string & file_path) const = 0; + + /** + * @brief Alias for a shared pointer to a FilesystemInterface object. + */ + using SharedPtr = std::shared_ptr; +}; + +/** + * @brief A class that provides functionality for interacting with the filesystem. + * + * This class inherits from the `FilesystemInterface` and implements its methods. + * It provides a simplified, facade-type way to interact with the `std::filesystem` library. + */ +class Filesystem : public FilesystemInterface +{ +public: + /** + * @brief Returns the space capacity in bytes of the filesystem at the specified path. + * + * @param filesystem_path The path to the filesystem. + * @return The space capacity in bytes. + */ + inline uintmax_t GetSpaceCapacity(const std::string & filesystem_path) const override + { + const auto path = std::filesystem::path(filesystem_path); + const auto space_info = std::filesystem::space(path); + + return space_info.capacity; + } + + /** + * @brief Returns the available space in bytes of the filesystem at the specified path. + * + * @param filesystem_path The path to the filesystem. + * @return The available space in bytes. + */ + inline uintmax_t GetSpaceAvailable(const std::string & filesystem_path) const override + { + const auto path = std::filesystem::path(filesystem_path); + const auto space_info = std::filesystem::space(path); + + return space_info.available; + } + + /** + * @brief Reads the contents of the file specified by the given file path. + * + * @param file_path The path to the file to be read. + * @return The contents of the file as a string. + * @throws `std::invalid_argument` If the file doesn't exist. + * @throws `std::runtime_error` If the file fails to open. + */ + std::string ReadFile(const std::string & file_path) const override + { + const auto path = std::filesystem::path(file_path); + + if (!std::filesystem::exists(path)) { + throw std::invalid_argument("File doesn't exist, given path " + path.string()); + } + + std::ifstream file(path); + if (!file.is_open()) { + throw std::runtime_error("Failed to open, given path " + path.string()); + } + + std::stringstream buffer; + buffer << file.rdbuf(); + + file.close(); + return buffer.str(); + } +}; + +} // namespace panther_diagnostics + +#endif // PANTHER_DIAGNOSTICS_FILESYSTEM_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp b/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp similarity index 51% rename from panther_diagnostics/include/panther_diagnostics/system_status_node.hpp rename to panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp index 235d5d417..c840a246b 100644 --- a/panther_diagnostics/include/panther_diagnostics/system_status_node.hpp +++ b/panther_diagnostics/include/panther_diagnostics/system_monitor_node.hpp @@ -12,59 +12,74 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ -#define PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ +#ifndef PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ +#define PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ #include -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include #include "panther_msgs/msg/system_status.hpp" -#include "system_status_parameters.hpp" +#include "system_monitor_parameters.hpp" + +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/types.hpp" using namespace std::chrono_literals; namespace panther_diagnostics { -class SystemStatusNode : public rclcpp::Node +class SystemMonitorNode : public rclcpp::Node { public: - SystemStatusNode(const std::string & node_name); - - struct SystemStatus - { - std::vector core_usages; - float cpu_mean_usage; - float cpu_temperature; - float memory_usage; - float disk_usage; - }; + SystemMonitorNode( + const std::string & node_name, FilesystemInterface::SharedPtr filesystem, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: + /** + * @brief Retrieves the system parameters and generate system status object describing the current + * system state. + * + * @return The system status. + */ SystemStatus GetSystemStatus() const; + std::vector GetCoresUsages() const; float GetCPUMeanUsage(const std::vector & usages) const; - float GetCPUTemperature(const std::string & filename) const; - float GetMemoryUsage() const; + float GetCPUTemperature() const; + float GetRAMUsage() const; float GetDiskUsage() const; + /** + * @brief Converts a SystemStatus object to a SystemStatus message. + * + * This function takes a SystemStatus object and converts it into a SystemStatus message. + * The resulting message can be used to publish the system status over a ROS topic. + * + * @param status The SystemStatus object to be converted. + * @return The converted SystemStatus message. + */ panther_msgs::msg::SystemStatus SystemStatusToMessage(const SystemStatus & status); private: void TimerCallback(); void DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status); + FilesystemInterface::SharedPtr filesystem_; + diagnostic_updater::Updater diagnostic_updater_; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr system_status_publisher_; - diagnostic_updater::Updater diagnostic_updater_; - system_status::Params params_; - std::shared_ptr param_listener_; + system_monitor::Params params_; + std::shared_ptr param_listener_; static constexpr char kTemperatureInfoFilename[] = "/sys/class/thermal/thermal_zone0/temp"; + static constexpr char kRootDirectory[] = "/"; }; } // namespace panther_diagnostics -#endif // PANTHER_DIAGNOSTICS_SYSTEM_STATUS_NODE_HPP_ +#endif // PANTHER_DIAGNOSTICS_SYSTEM_MONITOR_NODE_HPP_ diff --git a/panther_diagnostics/include/panther_diagnostics/types.hpp b/panther_diagnostics/include/panther_diagnostics/types.hpp new file mode 100644 index 000000000..1ed2c88c7 --- /dev/null +++ b/panther_diagnostics/include/panther_diagnostics/types.hpp @@ -0,0 +1,40 @@ +// 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_DIAGNOSTICS_TYPES_HPP_ +#define PANTHER_DIAGNOSTICS_TYPES_HPP_ + +#include + +namespace panther_diagnostics +{ + +/** + * @brief Represents the system status information. + * + * This struct contains various metrics related to the system status, such as CPU usage, CPU + * temperature, memory usage, and disk usage. + */ +struct SystemStatus +{ + std::vector core_usages; + float cpu_mean_usage; + float cpu_temperature; + float ram_usage; + float disk_usage; +}; + +} // namespace panther_diagnostics + +#endif // PANTHER_DIAGNOSTICS_TYPES_HPP_ diff --git a/panther_diagnostics/launch/system_status.launch.py b/panther_diagnostics/launch/system_monitor.launch.py similarity index 93% rename from panther_diagnostics/launch/system_status.launch.py rename to panther_diagnostics/launch/system_monitor.launch.py index 103acf598..e2d2ab706 100644 --- a/panther_diagnostics/launch/system_status.launch.py +++ b/panther_diagnostics/launch/system_monitor.launch.py @@ -28,9 +28,9 @@ def generate_launch_description(): description="Add namespace to all launched nodes", ) - system_status_node = Node( + system_monitor_node = Node( package="panther_diagnostics", - executable="system_status_node", + executable="system_monitor_node", name="system_monitor", namespace=namespace, remappings=[("/diagnostics", "diagnostics")], @@ -39,7 +39,7 @@ def generate_launch_description(): actions = [ declare_namespace_arg, - system_status_node, + system_monitor_node, ] return LaunchDescription(actions) diff --git a/panther_diagnostics/package.xml b/panther_diagnostics/package.xml index e6f1ba827..384d7b209 100644 --- a/panther_diagnostics/package.xml +++ b/panther_diagnostics/package.xml @@ -12,6 +12,7 @@ https://github.com/husarion/panther_ros/issues Jakub Delicat + Pawel Irzyk ament_cmake pkg-config @@ -25,6 +26,8 @@ std_msgs ament_cmake_gtest + google-mock + ros_testing ament_cmake diff --git a/panther_diagnostics/src/main.cpp b/panther_diagnostics/src/main.cpp index 69059d00c..8d09517f6 100644 --- a/panther_diagnostics/src/main.cpp +++ b/panther_diagnostics/src/main.cpp @@ -16,25 +16,27 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include -#include "panther_diagnostics/system_status_node.hpp" +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto system_status_node = - std::make_shared("system_monitor"); + auto filesystem = std::make_shared(); + auto system_monitor_node = std::make_shared( + "system_monitor", filesystem); try { - rclcpp::spin(system_status_node); + rclcpp::spin(system_monitor_node); } catch (const std::runtime_error & e) { - std::cerr << "[" << system_status_node->get_name() << "] Caught exception: " << e.what() + std::cerr << "[" << system_monitor_node->get_name() << "] Caught exception: " << e.what() << std::endl; } - std::cout << "[" << system_status_node->get_name() << "] Shutting down" << std::endl; + std::cout << "[" << system_monitor_node->get_name() << "] Shutting down" << std::endl; rclcpp::shutdown(); return 0; } diff --git a/panther_diagnostics/src/system_monitor_node.cpp b/panther_diagnostics/src/system_monitor_node.cpp new file mode 100644 index 000000000..bc921a75d --- /dev/null +++ b/panther_diagnostics/src/system_monitor_node.cpp @@ -0,0 +1,199 @@ +// 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_diagnostics/system_monitor_node.hpp" + +#include +#include + +#include + +#include +#include + +#include "panther_msgs/msg/system_status.hpp" + +#include "panther_utils/common_utilities.hpp" +#include "panther_utils/ros_utils.hpp" + +#include "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/types.hpp" + +namespace panther_diagnostics +{ + +SystemMonitorNode::SystemMonitorNode( + const std::string & node_name, FilesystemInterface::SharedPtr filesystem, + const rclcpp::NodeOptions & options) +: rclcpp::Node(node_name, options), filesystem_(filesystem), diagnostic_updater_(this) +{ + RCLCPP_INFO(this->get_logger(), "Initializing."); + + param_listener_ = + std::make_shared(this->get_node_parameters_interface()); + params_ = param_listener_->get_params(); + + system_status_publisher_ = this->create_publisher( + "system_status", 10); + + const auto timer_interval_ms = static_cast(1000.0 / params_.publish_frequency); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(timer_interval_ms), + std::bind(&SystemMonitorNode::TimerCallback, this)); + + diagnostic_updater_.setHardwareID("Built In Computer"); + diagnostic_updater_.add("OS status", this, &SystemMonitorNode::DiagnoseSystem); + + RCLCPP_INFO(this->get_logger(), "Initialized successfully."); +} + +void SystemMonitorNode::TimerCallback() +{ + const auto status = GetSystemStatus(); + auto message = SystemStatusToMessage(status); + + system_status_publisher_->publish(message); +} + +SystemStatus SystemMonitorNode::GetSystemStatus() const +{ + SystemStatus status; + + status.core_usages = GetCoresUsages(); + status.cpu_mean_usage = GetCPUMeanUsage(status.core_usages); + status.cpu_temperature = GetCPUTemperature(); + status.ram_usage = GetRAMUsage(); + status.disk_usage = GetDiskUsage(); + + return status; +} + +std::vector SystemMonitorNode::GetCoresUsages() const +{ + std::vector loads = uprofile::getInstantCpuUsage(); + + return loads; +} + +float SystemMonitorNode::GetCPUMeanUsage(const std::vector & usages) const +{ + if (usages.empty()) { + return std::numeric_limits::quiet_NaN(); + } + + std::for_each(usages.begin(), usages.end(), [](const float & usage) { + if (usage < 0.0 || usage > 100.0) { + throw std::invalid_argument{ + "At least one CPU core exceeds the valid usage range [0.0, 100.0]."}; + }; + }); + + auto sum = std::accumulate(usages.begin(), usages.end(), 0.0); + auto mean_usage = panther_utils::common_utilities::SetPrecision(sum / usages.size(), 2); + + return mean_usage; +} + +float SystemMonitorNode::GetCPUTemperature() const +{ + float temperature = std::numeric_limits::quiet_NaN(); + + try { + const auto temperature_str = filesystem_->ReadFile(kTemperatureInfoFilename); + temperature = panther_utils::common_utilities::SetPrecision( + std::stof(temperature_str) / 1000.0, 2); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "An exception occurred while reading CPU temperature: " << e.what()); + } + + return temperature; +} + +float SystemMonitorNode::GetRAMUsage() const +{ + int total = 0, free = 0, available = 0; + uprofile::getSystemMemory(total, available, free); + + const auto ram_usage = panther_utils::common_utilities::CountPercentage(total - available, total); + return ram_usage; +} + +float SystemMonitorNode::GetDiskUsage() const +{ + float disk_usage = std::numeric_limits::quiet_NaN(); + + try { + const auto capacity = filesystem_->GetSpaceCapacity(kRootDirectory); + const auto available = filesystem_->GetSpaceAvailable(kRootDirectory); + disk_usage = panther_utils::common_utilities::CountPercentage(capacity - available, capacity); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + this->get_logger(), "An exception occurred while reading disk usage: " << e.what()); + } + + return disk_usage; +} + +panther_msgs::msg::SystemStatus SystemMonitorNode::SystemStatusToMessage( + const SystemStatus & status) +{ + panther_msgs::msg::SystemStatus message; + + message.header.stamp = this->get_clock()->now(); + message.cpu_percent = status.core_usages; + message.avg_load_percent = status.cpu_mean_usage; + message.cpu_temp = status.cpu_temperature; + message.ram_usage_percent = status.ram_usage; + message.disc_usage_percent = status.disk_usage; + + return message; +} + +void SystemMonitorNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + auto error_level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string message = "System parameters are within acceptable limits."; + + params_ = param_listener_->get_params(); + auto system_status = GetSystemStatus(); + + status.add("CPU usage (%)", system_status.cpu_mean_usage); + status.add("CPU temperature (°C)", system_status.cpu_temperature); + status.add("RAM memory usage (%)", system_status.ram_usage); + status.add("Disk memory usage (%)", system_status.disk_usage); + + std::unordered_map limits = { + {params_.cpu_usage_warn_threshold, status.values[0]}, + {params_.cpu_temperature_warn_threshold, status.values[1]}, + {params_.ram_usage_warn_threshold, status.values[2]}, + {params_.disk_usage_warn_threshold, status.values[3]}, + }; + + for (const auto & [limit, key_value] : limits) { + if (std::isnan(std::stod(key_value.value))) { + message = "Detected system parameter with unknown value."; + error_level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; + break; + } else if (std::stod(key_value.value) > limit) { + message = "At least one system parameter is above the warning threshold."; + error_level = diagnostic_updater::DiagnosticStatusWrapper::WARN; + } + } + + status.summary(error_level, message); +} + +} // namespace panther_diagnostics diff --git a/panther_diagnostics/src/system_status_node.cpp b/panther_diagnostics/src/system_status_node.cpp deleted file mode 100644 index 51d1663f8..000000000 --- a/panther_diagnostics/src/system_status_node.cpp +++ /dev/null @@ -1,169 +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_diagnostics/system_status_node.hpp" - -#include -#include -#include -#include - -#include "cppuprofile/uprofile.h" - -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "panther_msgs/msg/system_status.hpp" -#include "panther_utils/common_utilities.hpp" -#include "panther_utils/ros_utils.hpp" - -namespace panther_diagnostics -{ - -SystemStatusNode::SystemStatusNode(const std::string & node_name) -: rclcpp::Node(node_name), diagnostic_updater_(this) -{ - RCLCPP_INFO(this->get_logger(), "Initializing."); - - param_listener_ = - std::make_shared(this->get_node_parameters_interface()); - params_ = param_listener_->get_params(); - - system_status_publisher_ = this->create_publisher( - "system_status", 10); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(params_.publish_rate * 1000)), - std::bind(&SystemStatusNode::TimerCallback, this)); - - diagnostic_updater_.setHardwareID(params_.hardware_id); - diagnostic_updater_.add("OS status", this, &SystemStatusNode::DiagnoseSystem); - - RCLCPP_INFO(this->get_logger(), "Initialized successfully."); -} - -void SystemStatusNode::TimerCallback() -{ - const auto status = GetSystemStatus(); - auto message = SystemStatusToMessage(status); - - system_status_publisher_->publish(message); -} - -SystemStatusNode::SystemStatus SystemStatusNode::GetSystemStatus() const -{ - SystemStatusNode::SystemStatus status; - - status.core_usages = GetCoresUsages(); - status.cpu_mean_usage = GetCPUMeanUsage(status.core_usages); - status.cpu_temperature = GetCPUTemperature(kTemperatureInfoFilename); - status.memory_usage = GetMemoryUsage(); - status.disk_usage = GetDiskUsage(); - - return status; -} - -std::vector SystemStatusNode::GetCoresUsages() const -{ - std::vector loads = uprofile::getInstantCpuUsage(); - return loads; -} - -float SystemStatusNode::GetCPUMeanUsage(const std::vector & usages) const -{ - auto sum = std::accumulate(usages.begin(), usages.end(), 0.0); - return sum / usages.size(); -} - -float SystemStatusNode::GetCPUTemperature(const std::string & filename) const -{ - try { - auto file = panther_utils::common_utilities::OpenFile(filename, std::ios_base::in); - float temperature; - file >> temperature; - file.close(); - return temperature / 1000.0; - } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "An exception occurred while reading CPU temperature: " << e.what()); - } - return std::numeric_limits::quiet_NaN(); -} - -float SystemStatusNode::GetMemoryUsage() const -{ - int total = 0, free = 0, available = 0; - uprofile::getSystemMemory(total, free, available); - return static_cast(total - available) / total * 100.0; -} - -float SystemStatusNode::GetDiskUsage() const -{ - const std::filesystem::directory_entry entry("/"); - const std::filesystem::space_info si = std::filesystem::space(entry.path()); - - return static_cast(si.capacity - si.available) / si.capacity * 100.0; -} - -panther_msgs::msg::SystemStatus SystemStatusNode::SystemStatusToMessage( - const SystemStatusNode::SystemStatus & status) -{ - panther_msgs::msg::SystemStatus message; - - message.header.stamp = this->get_clock()->now(); - message.header.frame_id = panther_utils::ros::AddNamespaceToFrameID( - params_.frame_id, std::string(this->get_namespace())); - message.cpu_percent = status.core_usages; - message.avg_load_percent = status.cpu_mean_usage; - message.cpu_temp = status.cpu_temperature; - message.ram_usage_percent = status.memory_usage; - message.disc_usage_percent = status.disk_usage; - - return message; -} - -void SystemStatusNode::DiagnoseSystem(diagnostic_updater::DiagnosticStatusWrapper & status) -{ - auto error_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string message = "System parameters are within acceptable limits."; - - params_ = param_listener_->get_params(); - auto system_status = GetSystemStatus(); - - status.add("CPU usage", system_status.cpu_mean_usage); - status.add("CPU temperature", system_status.cpu_temperature); - status.add("Disk memory usage", system_status.disk_usage); - status.add("RAM memory usage", system_status.memory_usage); - - std::unordered_map limits = { - {params_.cpu_usage_warn_threshold, status.values[0]}, - {params_.cpu_temperature_warn_threshold, status.values[1]}, - {params_.disk_usage_warn_threshold, status.values[2]}, - {params_.memory_usage_warn_threshold, status.values[3]}, - }; - - for (const auto & [limit, key_value] : limits) { - if (std::isnan(std::stod(key_value.value))) { - message = "Detected system parameter with unknown value."; - error_level = diagnostic_updater::DiagnosticStatusWrapper::ERROR; - break; - } else if (std::stod(key_value.value) > limit) { - message = "At least one system parameter is above the warning threshold."; - error_level = diagnostic_updater::DiagnosticStatusWrapper::WARN; - } - } - - status.summary(error_level, message); -} - -} // namespace panther_diagnostics diff --git a/panther_diagnostics/test/integration/system_monitor_node.test.py b/panther_diagnostics/test/integration/system_monitor_node.test.py new file mode 100644 index 000000000..6307e9724 --- /dev/null +++ b/panther_diagnostics/test/integration/system_monitor_node.test.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import unittest + +import launch +import launch_testing +import pytest +from diagnostic_msgs.msg import DiagnosticArray +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing_ros import WaitForTopics +from panther_msgs.msg import SystemStatus + + +@pytest.mark.launch_test +def generate_test_description(): + + system_monitor_node = Node( + package="panther_diagnostics", + executable="system_monitor_node", + ) + + # Start test after 1s + delay_timer = launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ) + + actions = [system_monitor_node, delay_timer] + + context = {} + + return ( + LaunchDescription(actions), + context, + ) + + +class TestNodeInitialization(unittest.TestCase): + def test_initialization_log(self, proc_output): + proc_output.assertWaitFor("Initialized successfully.") + + +class TestNode(unittest.TestCase): + def test_msg(self): + topic_list = [("system_status", SystemStatus), ("diagnostics", DiagnosticArray)] + + with WaitForTopics(topic_list, timeout=5.0) as wait_for_topics: + received_topics_str = ", ".join(wait_for_topics.topics_received()) + print("Received messages from the following topics: [" + received_topics_str + "]") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/panther_diagnostics/test/test_system_status_node.cpp b/panther_diagnostics/test/test_system_status_node.cpp deleted file mode 100644 index 9f606cc55..000000000 --- a/panther_diagnostics/test/test_system_status_node.cpp +++ /dev/null @@ -1,142 +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 "gtest/gtest.h" - -#include "panther_diagnostics/system_status_node.hpp" - -class SystemStatusNodeWrapper : public panther_diagnostics::SystemStatusNode -{ -public: - SystemStatusNodeWrapper() : panther_diagnostics::SystemStatusNode("test_system_statics") {} - - std::vector GetCoresUsages() const - { - return panther_diagnostics::SystemStatusNode::GetCoresUsages(); - } - - float GetCPUMeanUsage(const std::vector & usages) const - { - return panther_diagnostics::SystemStatusNode::GetCPUMeanUsage(usages); - } - - float GetCPUTemperature(const std::string & filename) const - { - return panther_diagnostics::SystemStatusNode::GetCPUTemperature(filename); - } - - float GetMemoryUsage() const { return panther_diagnostics::SystemStatusNode::GetMemoryUsage(); } - - float GetDiskUsage() const { return panther_diagnostics::SystemStatusNode::GetDiskUsage(); } - - panther_msgs::msg::SystemStatus SystemStatusToMessage( - const panther_diagnostics::SystemStatusNode::SystemStatus & status) - { - return panther_diagnostics::SystemStatusNode::SystemStatusToMessage(status); - } -}; - -class TestSystemStatusNode : public testing::Test -{ -public: - TestSystemStatusNode(); - -protected: - std::unique_ptr system_status_; -}; - -TestSystemStatusNode::TestSystemStatusNode() -{ - system_status_ = std::make_unique(); -} - -TEST_F(TestSystemStatusNode, CheckCoresUsages) -{ - const auto usages = system_status_->GetCoresUsages(); - - for (const auto & usage : usages) { - EXPECT_TRUE((usage >= 0.0) && (usage <= 100.0)); - } -} - -TEST_F(TestSystemStatusNode, CheckCPUMeanUsage) -{ - std::vector usages = {45.0, 55.0, 45.0, 55.0}; - - const auto mean = system_status_->GetCPUMeanUsage(usages); - EXPECT_FLOAT_EQ(mean, 50.0); -} - -TEST_F(TestSystemStatusNode, CheckTemperatureReadings) -{ - const std::string temperature_file_name = testing::TempDir() + "panther_diagnostics_temperature"; - - // Make sure that there is no random file with random value. - std::filesystem::remove(temperature_file_name); - - std::ofstream temperature_file(temperature_file_name, std::ofstream::out); - temperature_file << 36600 << std::endl; - temperature_file.close(); - - const auto temperature = system_status_->GetCPUTemperature(temperature_file_name); - std::filesystem::remove(temperature_file_name); - - EXPECT_FLOAT_EQ(temperature, 36.6); -} - -TEST_F(TestSystemStatusNode, CheckMemoryReadings) -{ - const auto memory = system_status_->GetMemoryUsage(); - - EXPECT_TRUE((memory >= 0.0) && (memory <= 100.0)); -} - -TEST_F(TestSystemStatusNode, CheckDiskReadings) -{ - const auto disk_usage = system_status_->GetDiskUsage(); - - EXPECT_TRUE((disk_usage >= 0.0) && (disk_usage <= 100.0)); -} - -TEST_F(TestSystemStatusNode, CheckSystemStatusToMessage) -{ - panther_diagnostics::SystemStatusNode::SystemStatus status; - status.core_usages = {50.0, 50.0, 50.0}; - status.cpu_mean_usage = 50.0; - status.cpu_temperature = 36.6; - status.disk_usage = 60.0; - status.memory_usage = 30.0; - - const auto message = system_status_->SystemStatusToMessage(status); - - EXPECT_EQ(message.cpu_percent, status.core_usages); - EXPECT_FLOAT_EQ(message.avg_load_percent, status.cpu_mean_usage); - EXPECT_FLOAT_EQ(message.cpu_temp, status.cpu_temperature); - EXPECT_FLOAT_EQ(message.disc_usage_percent, status.disk_usage); - EXPECT_FLOAT_EQ(message.ram_usage_percent, status.memory_usage); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(0, nullptr); - auto result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/panther_diagnostics/test/unit/test_filesystem.cpp b/panther_diagnostics/test/unit/test_filesystem.cpp new file mode 100644 index 000000000..3e9c87553 --- /dev/null +++ b/panther_diagnostics/test/unit/test_filesystem.cpp @@ -0,0 +1,98 @@ +// 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 "panther_diagnostics/filesystem.hpp" + +class TestFilesystem : public testing::Test +{ +public: + TestFilesystem(); + + std::string CreateTestFile(const std::string & content); + void RemoveTestFile(const std::string & file_path); + +protected: + std::shared_ptr filesystem_; + + static constexpr char kDummyString[] = "Hello World!"; +}; + +TestFilesystem::TestFilesystem() : filesystem_(std::make_shared()) +{ +} + +std::string TestFilesystem::CreateTestFile(const std::string & content) +{ + const std::string file_path = testing::TempDir() + "test_file.txt"; + + std::ofstream file(file_path); + file << content; + file.close(); + + return file_path; +} + +void TestFilesystem::RemoveTestFile(const std::string & file_path) +{ + std::remove(file_path.c_str()); +} + +TEST_F(TestFilesystem, ReadFileSuccess) +{ + const auto test_file_path = CreateTestFile(kDummyString); + + auto content = filesystem_->ReadFile(test_file_path); + + EXPECT_STREQ(kDummyString, content.c_str()); + + RemoveTestFile(test_file_path); +} + +TEST_F(TestFilesystem, ReadFileNonExistent) +{ + // File does not exist + auto const test_file_path = testing::TempDir() + "test_missing_file.txt"; + + EXPECT_THROW(filesystem_->ReadFile(test_file_path), std::invalid_argument); +} + +TEST_F(TestFilesystem, ReadFileLocked) +{ + const auto test_file_path = CreateTestFile(kDummyString); + + // Change file permissions to make it unreadable + std::filesystem::permissions(test_file_path, std::filesystem::perms::none); + + EXPECT_THROW(filesystem_->ReadFile(test_file_path), std::runtime_error); + + // Restore file permissions to allow deletion + std::filesystem::permissions(test_file_path, std::filesystem::perms::owner_all); + + RemoveTestFile(test_file_path); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + auto result = RUN_ALL_TESTS(); + + return result; +} diff --git a/panther_diagnostics/test/unit/test_system_monitor_node.cpp b/panther_diagnostics/test/unit/test_system_monitor_node.cpp new file mode 100644 index 000000000..8c93ff248 --- /dev/null +++ b/panther_diagnostics/test/unit/test_system_monitor_node.cpp @@ -0,0 +1,188 @@ +// 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 "panther_diagnostics/filesystem.hpp" +#include "panther_diagnostics/system_monitor_node.hpp" + +class MockFilesystem : public panther_diagnostics::FilesystemInterface +{ +public: + MOCK_METHOD( + uintmax_t, GetSpaceCapacity, (const std::string & filesystem_path), (const, override)); + + MOCK_METHOD( + uintmax_t, GetSpaceAvailable, (const std::string & filesystem_path), (const, override)); + + MOCK_METHOD(std::string, ReadFile, (const std::string & file_path), (const, override)); +}; + +class SystemMonitorNodeWrapper : public panther_diagnostics::SystemMonitorNode +{ +public: + SystemMonitorNodeWrapper(std::shared_ptr filesystem) + : panther_diagnostics::SystemMonitorNode("test_system_statics", filesystem) + { + } + + std::vector GetCoresUsages() const + { + return panther_diagnostics::SystemMonitorNode::GetCoresUsages(); + } + + float GetCPUMeanUsage(const std::vector & usages) const + { + return panther_diagnostics::SystemMonitorNode::GetCPUMeanUsage(usages); + } + + float GetCPUTemperature() const + { + return panther_diagnostics::SystemMonitorNode::GetCPUTemperature(); + } + + float GetRAMUsage() const { return panther_diagnostics::SystemMonitorNode::GetRAMUsage(); } + + float GetDiskUsage() const { return panther_diagnostics::SystemMonitorNode::GetDiskUsage(); } + + panther_msgs::msg::SystemStatus SystemStatusToMessage( + const panther_diagnostics::SystemStatus & status) + + { + return panther_diagnostics::SystemMonitorNode::SystemStatusToMessage(status); + } +}; + +class TestSystemMonitorNode : public testing::Test +{ +public: + TestSystemMonitorNode(); + +protected: + std::shared_ptr filesystem_; + std::unique_ptr system_monitor_; +}; + +TestSystemMonitorNode::TestSystemMonitorNode() +{ + filesystem_ = std::make_unique(); + system_monitor_ = std::make_unique(filesystem_); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageCorrectInput) +{ + const std::vector test_usages = {100.0, 0.0, 45.0, 55.0}; + const auto expected_mean_usage = 50.0; + + const auto mean_usage = system_monitor_->GetCPUMeanUsage(test_usages); + EXPECT_FLOAT_EQ(expected_mean_usage, mean_usage); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageEmptyInput) +{ + const std::vector test_usages = {}; + + const auto mean_usage = system_monitor_->GetCPUMeanUsage(test_usages); + + EXPECT_TRUE(std::isnan(mean_usage)); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageOverloaded) +{ + const std::vector test_usages = {150.0, 50.0, 50.0, 50.0}; + + EXPECT_THROW(system_monitor_->GetCPUMeanUsage(test_usages), std::invalid_argument); +} + +TEST_F(TestSystemMonitorNode, GetCPUMeanUsageUnderloaded) +{ + const std::vector test_usages = {-50.0, 50.0, 50.0, 50.0}; + + EXPECT_THROW(system_monitor_->GetCPUMeanUsage(test_usages), std::invalid_argument); +} + +TEST_F(TestSystemMonitorNode, GetCPUTemperatureValidFile) +{ + ON_CALL(*filesystem_, ReadFile(testing::_)).WillByDefault(testing::Return("25000")); + + const auto temperature = system_monitor_->GetCPUTemperature(); + + EXPECT_EQ(25.0, temperature); +} + +TEST_F(TestSystemMonitorNode, GetCPUTemperatureMissingFile) +{ + ON_CALL(*filesystem_, ReadFile(testing::_)) + .WillByDefault(testing::Throw(std::invalid_argument("File not found"))); + + const auto temperature = system_monitor_->GetCPUTemperature(); + + EXPECT_TRUE(std::isnan(temperature)); +} + +TEST_F(TestSystemMonitorNode, GetDiskUsageValidFilesystem) +{ + ON_CALL(*filesystem_, GetSpaceCapacity(testing::_)).WillByDefault(testing::Return(100000)); + ON_CALL(*filesystem_, GetSpaceAvailable(testing::_)).WillByDefault(testing::Return(50000)); + + const auto disk_usage = system_monitor_->GetDiskUsage(); + + EXPECT_EQ(50.0, disk_usage); +} + +TEST_F(TestSystemMonitorNode, GetDiskUsageInvalidFilesystem) +{ + ON_CALL(*filesystem_, GetSpaceCapacity(testing::_)).WillByDefault(testing::Return(100000)); + ON_CALL(*filesystem_, GetSpaceAvailable(testing::_)) + .WillByDefault(testing::Throw(std::invalid_argument{"Filesystem not found"})); + + const auto disk_usage = system_monitor_->GetDiskUsage(); + + EXPECT_TRUE(std::isnan(disk_usage)); +} + +TEST_F(TestSystemMonitorNode, SystemStatusToMessage) +{ + panther_diagnostics::SystemStatus test_status; + test_status.core_usages = {50.0, 50.0, 50.0}; + test_status.cpu_mean_usage = 50.0; + test_status.cpu_temperature = 36.6; + test_status.ram_usage = 30.0; + test_status.disk_usage = 60.0; + + const auto message = system_monitor_->SystemStatusToMessage(test_status); + + EXPECT_EQ(test_status.core_usages, message.cpu_percent); + EXPECT_FLOAT_EQ(test_status.cpu_mean_usage, message.avg_load_percent); + EXPECT_FLOAT_EQ(test_status.cpu_temperature, message.cpu_temp); + EXPECT_FLOAT_EQ(test_status.ram_usage, message.ram_usage_percent); + EXPECT_FLOAT_EQ(test_status.disk_usage, message.disc_usage_percent); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(0, nullptr); + + auto result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return result; +} diff --git a/panther_gazebo/CMakeLists.txt b/panther_gazebo/CMakeLists.txt index d0fa2e10c..31dc77d04 100644 --- a/panther_gazebo/CMakeLists.txt +++ b/panther_gazebo/CMakeLists.txt @@ -5,29 +5,86 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_DEPENDENCIES ament_cmake ignition-transport11 ignition-common4 - ignition-msgs8 panther_utils yaml-cpp) +set(PACKAGE_DEPENDENCIES + ament_cmake + hardware_interface + ignition-common4 + ignition-gazebo6 + ignition-msgs8 + ignition-plugin1 + ignition-transport11 + ign_ros2_control + pluginlib + panther_utils + rclcpp + std_msgs + std_srvs + yaml-cpp) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) endforeach() -include_directories(include) +find_package(Qt5 REQUIRED COMPONENTS Core Quick QuickControls2) -set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) -set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) -set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) +include_directories(include ${Qt5Core_INCLUDE_DIRS} ${Qt5Qml_INCLUDE_DIRS} + ${Qt5Quick_INCLUDE_DIRS} ${Qt5QuickControls2_INCLUDE_DIRS}) add_executable(gz_led_strip_manager src/main.cpp src/gz_led_strip_manager.cpp src/gz_led_strip.cpp) ament_target_dependencies(gz_led_strip_manager panther_utils) target_link_libraries( - gz_led_strip_manager ignition-transport${IGN_TRANSPORT_VER}::core - ignition-msgs${IGN_MSGS_VER} - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} yaml-cpp) + gz_led_strip_manager ignition-transport11::core ignition-msgs8 + ignition-common4::ignition-common4 yaml-cpp) + +add_library(panther_simulation_plugins SHARED src/gz_panther_system.cpp) +ament_target_dependencies( + panther_simulation_plugins + hardware_interface + ign_ros2_control + rclcpp + rclcpp_lifecycle + std_msgs + std_srvs) +target_link_libraries(panther_simulation_plugins ignition-gazebo6::core) + +set(CMAKE_AUTOMOC ON) +qt5_add_resources(resources_rcc src/plugins/EStop.qrc) + +add_library(EStop SHARED include/panther_gazebo/plugins/e_stop.hpp + src/plugins/e_stop.cpp ${resources_rcc}) +ament_target_dependencies(EStop ignition-common4 ignition-gazebo6 + ignition-plugin1 rclcpp std_srvs) +target_link_libraries(EStop ${Qt5Core_LIBRARIES} ${Qt5Qml_LIBRARIES} + ${Qt5Quick_LIBRARIES} ${Qt5QuickControls2_LIBRARIES}) install(TARGETS gz_led_strip_manager DESTINATION lib/${PROJECT_NAME}) +install( + TARGETS panther_simulation_plugins + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) + +install( + TARGETS EStop + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) + install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) + +pluginlib_export_plugin_description_file(ign_ros2_control + panther_simulation_plugins.xml) + +ament_environment_hooks( + "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in") + ament_package() diff --git a/panther_gazebo/CONFIGURATION.md b/panther_gazebo/CONFIGURATION.md index 8a8c4c198..c41cc924c 100644 --- a/panther_gazebo/CONFIGURATION.md +++ b/panther_gazebo/CONFIGURATION.md @@ -8,7 +8,7 @@ The NavSat sensors requires the spherical coordinates of the world origin to be To obtain GPS data in Ignition, follow these steps: -- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/external_antenna.urdf.xacro) by adding the following lines to your [components.yaml](../panther_description/config/components.yaml) file inside the `components` list: +- Include the [ANT02](https://github.com/husarion/ros_components_description/blob/ros2/urdf/teltonika_003R-00253.urdf.xacro) by adding the following lines to your [components.yaml](../panther_description/config/components.yaml) file inside the `components` list: ```yaml - type: ANT02 diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index 662f6ac49..36bf7bb30 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -14,3 +14,37 @@ The package contains a launch file and source files used to run the robot simula - [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration. - [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation. - [`led_strips.yaml`](./config/led_strips.yaml): Configure properties of led strips in simulation to animate lights. +- [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which adds E-Stop and Teleop widgets. + +## ROS Nodes + +### EStop + +`EStop` is a Gazebo GUI plugin responsible for easy and convenient changing of the robot's E-stop state. + +#### Service Clients + +- `hardware/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `hardware/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. + +### GzPantherSystem + +Plugin based on `ign_system` is responsible for handling sensor interfaces (only IMU for now) and sending requests for joints compatible with `ros2_control`. Plugin also adds E-Stop support. + +#### Publishers + +- `gz_ros2_control/e_stop` [*std_msgs/Bool*]: Current E-stop state. + +#### Service Servers + +- `gz_ros2_control/e_stop_reset` [*std_srvs/Trigger*]: Resets E-stop. +- `gz_ros2_control/e_stop_trigger` [*std_srvs/Trigger*]: Triggers E-stop. + +> [!NOTE] +> Above topics and services should be remapped to match real robot + +#### Parameters + +Required parameters are defined when including the interface in the URDF (you can check out [panther_macro.urdf.xacro](../panther_description/urdf/panther_macro.urdf.xacro)). + +- `e_stop_initial_state` [*bool*, default: **true**]: Initial state of E-stop. diff --git a/panther_gazebo/config/gz_bridge.yaml b/panther_gazebo/config/gz_bridge.yaml index 5f8fa9209..8b6c2328c 100644 --- a/panther_gazebo/config/gz_bridge.yaml +++ b/panther_gazebo/config/gz_bridge.yaml @@ -17,14 +17,12 @@ gz_type_name: ignition.msgs.Twist direction: GZ_TO_ROS -- ros_topic_name: lights/channel_1_frame - gz_topic_name: lights/channel_1_frame +- topic_name: lights/channel_1_frame ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image direction: ROS_TO_GZ -- ros_topic_name: lights/channel_2_frame - gz_topic_name: lights/channel_2_frame +- topic_name: lights/channel_2_frame ros_type_name: sensor_msgs/msg/Image gz_type_name: ignition.msgs.Image direction: ROS_TO_GZ diff --git a/panther_gazebo/config/led_strips.yaml b/panther_gazebo/config/led_strips.yaml index c5c24c254..c23e06e5f 100644 --- a/panther_gazebo/config/led_strips.yaml +++ b/panther_gazebo/config/led_strips.yaml @@ -1,5 +1,5 @@ chanel_1: - frequency: 15 # Setting to high frequency caused lags in the simulation + frequency: 12 # Setting to high frequency caused lags in the simulation world_name: husarion_world parent_link: panther position: [0.362, 0.0, 0.201] @@ -10,7 +10,7 @@ chanel_1: number_of_leds: 46 chanel_2: - frequency: 15 # Setting to high frequency caused lags in the simulation + frequency: 12 # Setting to high frequency caused lags in the simulation world_name: husarion_world parent_link: panther position: [-0.362, 0.0, 0.201] diff --git a/panther_gazebo/config/teleop_with_estop.config b/panther_gazebo/config/teleop_with_estop.config new file mode 100644 index 000000000..3bdf8538a --- /dev/null +++ b/panther_gazebo/config/teleop_with_estop.config @@ -0,0 +1,1266 @@ + + + + -1 + -1 + + 1850 + 1016 +