From c9faeb3fdcf82fc36546e1818711de6365f38906 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 10 Oct 2024 12:40:28 +0000 Subject: [PATCH 01/21] Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- panther_docking/config/panther_docking_server.yaml | 2 +- panther_docking/launch/docking.launch.py | 2 +- panther_docking/launch/station.launch.py | 13 +++++++++---- 5 files changed, 13 insertions(+), 8 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 27289d3b9..c6df86e10 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: b29f5637a243b0008ac197032575c8df47883b2c + version: 99e0189970293bb0110aad0a3b609bc659ecc663 ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index 83df44fa3..d6c6932fc 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: b29f5637a243b0008ac197032575c8df47883b2c + version: 99e0189970293bb0110aad0a3b609bc659ecc663 ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 4593b7167..7d3ecea15 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -37,7 +37,7 @@ docks: ["main_dock"] main_dock: type: panther_charging_dock - frame: /wibotic_station_link + frame: /main_wibotic_station_link pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot controller: diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 3583db648..514cfd4ad 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -63,7 +63,7 @@ def generate_launch_description(): parameters=[ {"panther_version": panther_version}, namespaced_docking_server_config, - {"use_sim_time": True}, + {"use_sim_time": use_sim}, ], namespace=namespace, emulate_tty=True, diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py index d9e6ac12d..f1303cdf8 100644 --- a/panther_docking/launch/station.launch.py +++ b/panther_docking/launch/station.launch.py @@ -26,6 +26,7 @@ PythonExpression, ) from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare from moms_apriltag import TagGenerator2 @@ -41,7 +42,7 @@ def generate_apriltag_and_get_path(tag_id): def launch_setup(context, *args, **kwargs): - namespace = LaunchConfiguration("namespace") + namespace = LaunchConfiguration("namespace").perform(context) apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context)) apriltag_size = LaunchConfiguration("apriltag_size").perform(context) @@ -58,8 +59,8 @@ def launch_setup(context, *args, **kwargs): "wibotic_station.urdf.xacro", ] ), - " namespace:=", - namespace, + " device_namespace:=", + "main", " apriltag_image_path:=", apriltag_image_path, " apriltag_size:=", @@ -69,12 +70,16 @@ def launch_setup(context, *args, **kwargs): namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"]) + station_description = { + "robot_description": ParameterValue(station_description_content, value_type=str) + } + station_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", name="wibotic_station_state_publisher", parameters=[ - {"robot_description": station_description_content}, + station_description, {"frame_prefix": namespace_ext}, ], remappings=[("robot_description", "station_description")], From 076a5dc95b26ca20d415e0dad1108014acc5595e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 29 Oct 2024 13:29:02 +0000 Subject: [PATCH 02/21] Changed GetDockPose approach Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 24 ++- .../config/panther_docking_server.yaml | 27 +-- .../panther_docking/panther_charging_dock.hpp | 26 +-- panther_docking/launch/docking.launch.py | 19 ++ panther_docking/launch/station.launch.py | 4 +- panther_docking/package.xml | 1 + .../src/dock_pose_publisher_node.cpp | 86 +++++++++ panther_docking/src/panther_charging_dock.cpp | 176 +++++------------- 8 files changed, 177 insertions(+), 186 deletions(-) create mode 100644 panther_docking/src/dock_pose_publisher_node.cpp diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index a82908ed2..749a2c807 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -43,16 +43,20 @@ install(TARGETS panther_charging_dock LIBRARY DESTINATION lib) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) -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 - panther_charging_dock) - ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock - ${PACKAGE_DEPENDENCIES}) -endif() +# 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 +# panther_charging_dock) +# ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock +# ${PACKAGE_DEPENDENCIES}) endif() + +add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp) +ament_target_dependencies(dock_pose_publisher rclcpp tf2_ros geometry_msgs + tf2_geometry_msgs) + +install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME}) ament_export_include_directories(include) ament_export_libraries(panther_charging_dock) diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 7d3ecea15..e2ef23ee1 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -17,22 +17,11 @@ panther_charging_dock: plugin: panther_docking::PantherChargingDock base_frame: "/base_link" - docking_distance_threshold: 0.15 - docking_yaw_threshold: 0.15 - staging_x_offset: -0.8 - staging_yaw_offset: 0.0 - - # TODO: @delihus Try to remove this parameters by using docking station description in the ros_components_description - # Transform between april tag frame and dock pose. An april tag Z+ faces always a camera - external_detection_timeout: 0.3 - external_detection_translation_x: 0.6 - external_detection_translation_y: 0.0 - external_detection_translation_z: 0.0 - external_detection_rotation_roll: 0.0 - external_detection_rotation_pitch: 0.0 - external_detection_rotation_yaw: 3.14 - - enable_charger_service_call_timeout: 1.0 + odom_frame: "/odom" + docking_distance_threshold: 0.08 + docking_yaw_threshold: 0.1 + staging_x_offset: -0.5 + external_detection_timeout: 0.1 docks: ["main_dock"] main_dock: @@ -43,6 +32,6 @@ controller: k_phi: 1.0 k_delta: 2.0 - v_linear_min: 0.05 - v_linear_max: 0.2 - v_angular_max: 0.3 + v_linear_min: 0.0 + v_linear_max: 0.25 + v_angular_max: 0.15 diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index fdef74de8..8cc3e3e55 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -161,24 +161,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ PoseStampedMsg offsetDetectedDockPose(const PoseStampedMsg & detected_dock_pose); - /** - * @brief Get the dock pose in a detection dock frame. - * - * This method retrieves the dock pose in a detection dock frame. - * - * @param frame The detection frame to get the dock pose in. - * @return The dock pose in the detection frame. - */ - PoseStampedMsg getDockPose(const std::string & frame); - - /** - * @brief Method to update the dock pose and publish it. - * - * This method makes all necessary transformations to update the dock pose and publishes it. - * - */ - void updateDockPoseAndPublish(); - /** * @brief Update the staging pose and publish it. * @@ -198,16 +180,12 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock tf2_ros::Buffer::SharedPtr tf2_buffer_; rclcpp::Publisher::SharedPtr staging_pose_pub_; - rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Subscription::SharedPtr dock_pose_sub_; PoseStampedMsg dock_pose_; PoseStampedMsg staging_pose_; double external_detection_timeout_; - tf2::Quaternion external_detection_rotation_; - double external_detection_translation_x_; - double external_detection_translation_y_; - double external_detection_translation_z_; std::shared_ptr pose_filter_; @@ -218,6 +196,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double staging_yaw_offset_; double pose_filter_coef_; + + builtin_interfaces::msg::Time request_detection_time_; }; } // namespace panther_docking diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 514cfd4ad..92c20a138 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -52,6 +52,13 @@ def generate_launch_description(): description=("Path to docking server configuration file."), ) + log_level = LaunchConfiguration("log_level") + declare_log_level = DeclareLaunchArgument( + "log_level", + default_value="info", + description="Logging level", + ) + namespaced_docking_server_config = ReplaceString( source_file=docking_server_config_path, replacements={"": namespace, "//": "/"}, @@ -65,6 +72,7 @@ def generate_launch_description(): namespaced_docking_server_config, {"use_sim_time": use_sim}, ], + arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], namespace=namespace, emulate_tty=True, ) @@ -85,6 +93,15 @@ def generate_launch_description(): namespace=namespace, ) + dock_pose_publisher = Node( + package="panther_docking", + executable="dock_pose_publisher", + name="dock_pose_publisher", + namespace=namespace, + emulate_tty=True, + arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], + ) + station_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -103,8 +120,10 @@ def generate_launch_description(): declare_use_sim_arg, declare_namespace_arg, declare_docking_server_config_path_arg, + declare_log_level, station_launch, docking_server_node, docking_server_activate_node, + dock_pose_publisher, ] ) diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py index f1303cdf8..ee92de913 100644 --- a/panther_docking/launch/station.launch.py +++ b/panther_docking/launch/station.launch.py @@ -99,13 +99,13 @@ def generate_launch_description(): declare_apriltag_id = DeclareLaunchArgument( "apriltag_id", - default_value="1", + default_value="0", description="ID of a generated apriltag on the station", ) declare_apriltag_size = DeclareLaunchArgument( "apriltag_size", - default_value="0.15", + default_value="0.07", description="Size in meters of a generated apriltag on the station", ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index edee3f62b..f665b0c6e 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -28,6 +28,7 @@ nav2_lifecycle_manager python3-imageio + xacro ament_cmake ament_cmake_gtest diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp new file mode 100644 index 000000000..b41cf2bc9 --- /dev/null +++ b/panther_docking/src/dock_pose_publisher_node.cpp @@ -0,0 +1,86 @@ +// Copyright (c) 2024 Husarion Sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include + +#include +#include + +class DockPosePublisherNode : public rclcpp::Node +{ +public: + DockPosePublisherNode() : Node("pose_publisher_node") + { + declare_parameter("device_namespace", "main"); + std::string device_namespace = get_parameter("device_namespace").as_string(); + std::string robot_namespace = std::string(get_namespace()).substr(1); + + source_frame_ = robot_namespace + "/" + device_namespace + + "_wibotic_receiver_requested_pose_link"; + target_frame_ = robot_namespace + "/odom"; + + pose_publisher_ = this->create_publisher( + "docking/dock_pose", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), std::bind(&DockPosePublisherNode::publishPose, this)); + } + +private: + void publishPose() + { + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform( + target_frame_, source_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_DEBUG(this->get_logger(), "Could not get transform: %s", ex.what()); + return; + } + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = this->now(); + pose_msg.header.frame_id = target_frame_; + pose_msg.pose.position.x = transform_stamped.transform.translation.x; + pose_msg.pose.position.y = transform_stamped.transform.translation.y; + pose_msg.pose.position.z = 0.0; + pose_msg.pose.orientation = transform_stamped.transform.rotation; + + pose_publisher_->publish(pose_msg); + } + + std::string target_frame_; + std::string source_frame_; + rclcpp::Publisher::SharedPtr pose_publisher_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 0e13221dd..a3c267809 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -51,50 +51,40 @@ void PantherChargingDock::configure( void PantherChargingDock::cleanup() { - dock_pose_pub_.reset(); + dock_pose_sub_.reset(); staging_pose_pub_.reset(); } void PantherChargingDock::activate() { auto node = node_.lock(); - dock_pose_pub_ = node->create_publisher("docking/dock_pose", 1); + dock_pose_sub_ = node->create_subscription( + "docking/dock_pose", 1, [this](const PoseStampedMsg::SharedPtr msg) { + auto filtered_pose = pose_filter_->update(*msg); + dock_pose_ = filtered_pose; + }); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); } void PantherChargingDock::deactivate() { - dock_pose_pub_.reset(); + dock_pose_sub_.reset(); staging_pose_pub_.reset(); } void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { - nav2_util::declare_parameter_if_not_declared( - node, "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_ + ".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_ + ".external_detection_timeout", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( node, name_ + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); nav2_util::declare_parameter_if_not_declared( @@ -107,77 +97,75 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { node->get_parameter(name_ + ".base_frame", base_frame_name_); + node->get_parameter(name_ + ".external_detection_timeout", external_detection_timeout_); - node->get_parameter( - name_ + ".external_detection_translation_x", external_detection_translation_x_); - node->get_parameter( - name_ + ".external_detection_translation_y", external_detection_translation_y_); - node->get_parameter( - name_ + ".external_detection_translation_z", external_detection_translation_z_); - - double yaw, pitch, roll; - node->get_parameter(name_ + ".external_detection_rotation_yaw", yaw); - node->get_parameter(name_ + ".external_detection_rotation_pitch", pitch); - node->get_parameter(name_ + ".external_detection_rotation_roll", roll); - - external_detection_rotation_.setRPY(roll, pitch, yaw); node->get_parameter(name_ + ".docking_distance_threshold", docking_distance_threshold_); node->get_parameter(name_ + ".docking_yaw_threshold", docking_yaw_threshold_); node->get_parameter(name_ + ".staging_x_offset", staging_x_offset_); - node->get_parameter(name_ + ".staging_yaw_offset", staging_yaw_offset_); node->get_parameter(name_ + ".filter_coef", pose_filter_coef_); } +// When there is no pose actual position of robot is a staging pose PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { - std::string stage_frame = frame; - // The pose in the argument is a pose defined in yaml file or passed in docking action. - // In our approach to docking we use local "base_link" frame as a reference frame. - // The pose is empty when the docking action is called from the action server. - // When a robot is undocking it passes odometry frame as a reference frame. - // It needed to be split to two cases to handle both situations. + RCLCPP_DEBUG(logger_, "Getting staging pose"); + + // When there is no global pose to reach thanks to nav2 if (pose == geometry_msgs::msg::Pose()) { dock_frame_ = frame; - stage_frame = base_frame_name_; - } - if (dock_frame_.empty()) { - throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); + if (dock_frame_.empty()) { + throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); + } + + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + staging_pose_ = dock_pose_; + staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose_.pose.position.y += sin(yaw) * staging_x_offset_; + + staging_pose_pub_->publish(staging_pose_); } - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(stage_frame); return staging_pose_; } bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) { - try { - updateDockPoseAndPublish(); - updateStagingPoseAndPublish(base_frame_name_); - pose = dock_pose_; - } catch (const opennav_docking_core::DockingException & e) { - RCLCPP_ERROR_STREAM(logger_, "An occurred error while getting refined pose: " << e.what()); + RCLCPP_DEBUG(logger_, "Getting refined pose"); + { + auto node = node_.lock(); + request_detection_time_ = node->now(); + } + + auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); + auto duration = rclcpp::Time(request_detection_time_) - rclcpp::Time(dock_pose_.header.stamp); + if (duration > timeout) { + RCLCPP_WARN_STREAM( + logger_, "Lost detection or did not detect: timeout exceeded: " << duration.seconds()); return false; } + pose = dock_pose_; return true; } bool PantherChargingDock::isDocked() { - updateDockPoseAndPublish(); + RCLCPP_DEBUG(logger_, "Checking if docked"); + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = base_frame_name_; + + robot_pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, robot_pose, "panther/odom"); - geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = dock_pose_.header.frame_id; return panther_utils::tf2_utils::ArePosesNear( - origin, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); + robot_pose, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); } bool PantherChargingDock::isCharging() { + RCLCPP_DEBUG(logger_, "Checking if charging"); try { return isDocked(); } catch (const opennav_docking_core::FailedToDetectDock & e) { @@ -189,82 +177,6 @@ bool PantherChargingDock::disableCharging() { return true; } bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } -PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetStagingPoseToDockPose( - const PoseStampedMsg & dock_pose) -{ - tf2::Transform staging_offset_transform; - staging_offset_transform.setOrigin(tf2::Vector3(staging_x_offset_, 0.0, 0.0)); - - tf2::Quaternion staging_yaw_rotation; - staging_yaw_rotation.setRPY(0, 0, staging_yaw_offset_); - staging_offset_transform.setRotation(staging_yaw_rotation); - - return panther_utils::tf2_utils::OffsetPose(dock_pose, staging_offset_transform); -} - -PantherChargingDock::PoseStampedMsg PantherChargingDock::offsetDetectedDockPose( - const PoseStampedMsg & detected_dock_pose) -{ - tf2::Transform offset; - offset.setOrigin(tf2::Vector3( - external_detection_translation_x_, external_detection_translation_y_, - external_detection_translation_z_)); - offset.setRotation(external_detection_rotation_); - - return panther_utils::tf2_utils::OffsetPose(detected_dock_pose, offset); -} - -PantherChargingDock::PoseStampedMsg PantherChargingDock::getDockPose(const std::string & frame) -{ - PoseStampedMsg filtered_offset_detected_dock_pose; - try { - PoseStampedMsg pose; - pose.header.frame_id = frame; - { - auto node = node_.lock(); - pose.header.stamp = node->get_clock()->now(); - } - auto offset_detected_dock_pose = offsetDetectedDockPose(pose); - - filtered_offset_detected_dock_pose = pose_filter_->update(offset_detected_dock_pose); - filtered_offset_detected_dock_pose = panther_utils::tf2_utils::TransformPose( - tf2_buffer_, filtered_offset_detected_dock_pose, base_frame_name_, - external_detection_timeout_); - - filtered_offset_detected_dock_pose.pose.position.z = 0.0; - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "An exception occurred while getting dock pose: " + std::string(e.what())); - } - - return filtered_offset_detected_dock_pose; -} - -void PantherChargingDock::updateDockPoseAndPublish() -{ - try { - dock_pose_ = getDockPose(dock_frame_); - dock_pose_pub_->publish(dock_pose_); - } catch (const std::runtime_error & e) { - throw opennav_docking_core::FailedToDetectDock( - "An exception occurred while updating dock pose: " + std::string(e.what())); - } -} - -void PantherChargingDock::updateStagingPoseAndPublish(const std::string & frame) -{ - try { - auto new_staging_pose = offsetStagingPoseToDockPose(dock_pose_); - staging_pose_ = panther_utils::tf2_utils::TransformPose( - tf2_buffer_, new_staging_pose, frame, external_detection_timeout_); - dock_pose_.pose.position.z = 0.0; - staging_pose_pub_->publish(staging_pose_); - } catch (const std::runtime_error & e) { - throw opennav_docking_core::FailedToStage( - "An exception occurred while transforming staging pose: " + std::string(e.what())); - } -} - } // namespace panther_docking #include "pluginlib/class_list_macros.hpp" From 1b09ff75e9cd0a7e0d91470a84c75b3575b00b49 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 29 Oct 2024 13:39:49 +0000 Subject: [PATCH 03/21] Cleanup charger Signed-off-by: Jakub Delicat --- .../config/panther_docking_server.yaml | 2 -- .../panther_docking/panther_charging_dock.hpp | 34 +------------------ panther_docking/src/panther_charging_dock.cpp | 21 ++++++------ 3 files changed, 11 insertions(+), 46 deletions(-) diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index e2ef23ee1..d78f045cb 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -16,8 +16,6 @@ dock_plugins: ["panther_charging_dock"] panther_charging_dock: plugin: panther_docking::PantherChargingDock - base_frame: "/base_link" - odom_frame: "/odom" docking_distance_threshold: 0.08 docking_yaw_threshold: 0.1 staging_x_offset: -0.5 diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 8cc3e3e55..3652eb725 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -139,38 +139,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); - /** - * @brief Offset the staging pose. - * - * This method offsets the staging dock pose by values described in a configuration. - * - * @param dock_pose The dock pose to offset. - * - * @return The offset staging pose. - */ - PoseStampedMsg offsetStagingPoseToDockPose(const PoseStampedMsg & dock_pose); - - /** - * @brief Offset the detected dock pose. - * - * This method offsets the detected dock pose by values described in a configuration. - * - * @param detected_dock_pose The detected dock pose to offset. - * - * @return The offset detected dock pose. - */ - PoseStampedMsg offsetDetectedDockPose(const PoseStampedMsg & detected_dock_pose); - - /** - * @brief 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 base_frame_name_; + std::string fixed_frame_name_; std::string dock_frame_; rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; @@ -196,8 +166,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double staging_yaw_offset_; double pose_filter_coef_; - - builtin_interfaces::msg::Time request_detection_time_; }; } // namespace panther_docking diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index a3c267809..076cb6d2e 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -75,7 +75,9 @@ void PantherChargingDock::deactivate() void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { nav2_util::declare_parameter_if_not_declared( - node, name_ + ".base_frame", rclcpp::ParameterValue("base_link")); + node, "base_frame", rclcpp::ParameterValue("base_link")); + + nav2_util::declare_parameter_if_not_declared(node, "fixed_frame", rclcpp::ParameterValue("odom")); nav2_util::declare_parameter_if_not_declared( node, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05)); @@ -96,7 +98,8 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { - node->get_parameter(name_ + ".base_frame", base_frame_name_); + node->get_parameter("base_frame", base_frame_name_); + node->get_parameter("fixed_frame", fixed_frame_name_); node->get_parameter(name_ + ".external_detection_timeout", external_detection_timeout_); node->get_parameter(name_ + ".docking_distance_threshold", docking_distance_threshold_); @@ -110,16 +113,10 @@ void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::S PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { - RCLCPP_DEBUG(logger_, "Getting staging pose"); + RCLCPP_DEBUG_STREAM(logger_, "Getting staging pose in frame: " << frame); // When there is no global pose to reach thanks to nav2 if (pose == geometry_msgs::msg::Pose()) { - dock_frame_ = frame; - - if (dock_frame_.empty()) { - throw opennav_docking_core::FailedToControl("Cannot undock before docking!"); - } - const double yaw = tf2::getYaw(dock_pose_.pose.orientation); staging_pose_ = dock_pose_; staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; @@ -134,13 +131,15 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) { RCLCPP_DEBUG(logger_, "Getting refined pose"); + rclcpp::Time request_detection_time; + { auto node = node_.lock(); - request_detection_time_ = node->now(); + request_detection_time = node->now(); } auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); - auto duration = rclcpp::Time(request_detection_time_) - rclcpp::Time(dock_pose_.header.stamp); + auto duration = rclcpp::Time(request_detection_time) - rclcpp::Time(dock_pose_.header.stamp); if (duration > timeout) { RCLCPP_WARN_STREAM( logger_, "Lost detection or did not detect: timeout exceeded: " << duration.seconds()); From 72a6b8156e6e69574985bd31b1c5695bbe699c3c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 29 Oct 2024 15:42:10 +0000 Subject: [PATCH 04/21] Added unit tests Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 19 +- .../config/panther_docking_server.yaml | 2 +- .../panther_docking/panther_charging_dock.hpp | 3 + panther_docking/src/panther_charging_dock.cpp | 22 +- .../test/test_panther_charging_dock.cpp | 418 ------------------ .../test/unit/test_panther_charging_dock.cpp | 209 +++++++++ 6 files changed, 241 insertions(+), 432 deletions(-) delete mode 100644 panther_docking/test/test_panther_charging_dock.cpp create mode 100644 panther_docking/test/unit/test_panther_charging_dock.cpp diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 749a2c807..e61f25791 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -43,14 +43,17 @@ install(TARGETS panther_charging_dock LIBRARY DESTINATION lib) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) -# 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 -# panther_charging_dock) -# ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock -# ${PACKAGE_DEPENDENCIES}) endif() +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock + test/unit/test_panther_charging_dock.cpp) + target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock + panther_charging_dock) + ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock + ${PACKAGE_DEPENDENCIES}) + +endif() add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp) ament_target_dependencies(dock_pose_publisher rclcpp tf2_ros geometry_msgs diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index d78f045cb..4b520ecd3 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -31,5 +31,5 @@ k_phi: 1.0 k_delta: 2.0 v_linear_min: 0.0 - v_linear_max: 0.25 + v_linear_max: 0.3 v_angular_max: 0.15 diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 3652eb725..5258c1aa8 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -139,6 +140,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); + void setDockPose(const PoseStampedMsg::SharedPtr pose); + std::string base_frame_name_; std::string fixed_frame_name_; std::string dock_frame_; diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 076cb6d2e..105ae9380 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -59,10 +59,8 @@ void PantherChargingDock::activate() { auto node = node_.lock(); dock_pose_sub_ = node->create_subscription( - "docking/dock_pose", 1, [this](const PoseStampedMsg::SharedPtr msg) { - auto filtered_pose = pose_filter_->update(*msg); - dock_pose_ = filtered_pose; - }); + "docking/dock_pose", 1, + std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1)); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); } @@ -117,6 +115,10 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( // When there is no global pose to reach thanks to nav2 if (pose == geometry_msgs::msg::Pose()) { + if (dock_pose_.header.frame_id.empty()) { + throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); + } + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); staging_pose_ = dock_pose_; staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; @@ -133,6 +135,10 @@ bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) RCLCPP_DEBUG(logger_, "Getting refined pose"); rclcpp::Time request_detection_time; + if (dock_pose_.header.frame_id.empty()) { + throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); + } + { auto node = node_.lock(); request_detection_time = node->now(); @@ -156,7 +162,7 @@ bool PantherChargingDock::isDocked() geometry_msgs::msg::PoseStamped robot_pose; robot_pose.header.frame_id = base_frame_name_; - robot_pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, robot_pose, "panther/odom"); + robot_pose = panther_utils::tf2_utils::TransformPose(tf2_buffer_, robot_pose, fixed_frame_name_); return panther_utils::tf2_utils::ArePosesNear( robot_pose, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); @@ -176,6 +182,12 @@ bool PantherChargingDock::disableCharging() { return true; } bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } +void PantherChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) +{ + auto filtered_pose = pose_filter_->update(*pose); + dock_pose_ = filtered_pose; +} + } // namespace panther_docking #include "pluginlib/class_list_macros.hpp" diff --git a/panther_docking/test/test_panther_charging_dock.cpp b/panther_docking/test/test_panther_charging_dock.cpp deleted file mode 100644 index 71656ee38..000000000 --- a/panther_docking/test/test_panther_charging_dock.cpp +++ /dev/null @@ -1,418 +0,0 @@ -// Copyright (c) 2024 Husarion Sp. z o.o. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "panther_docking/panther_charging_dock.hpp" -#include "panther_utils/test/ros_test_utils.hpp" -#include "panther_utils/tf2_utils.hpp" - -static constexpr char kBaseFrame[] = "base_link"; -static constexpr char kDockFrame[] = "test_dock"; -static constexpr char kOdomFrame[] = "odom"; - -class PantherChargingDockWrapper : public panther_docking::PantherChargingDock -{ -public: - using SharedPtr = std::shared_ptr; - using UniquePtr = std::unique_ptr; - PantherChargingDockWrapper() : panther_docking::PantherChargingDock() {} - - geometry_msgs::msg::PoseStamped offsetStagingPoseToDockPose( - const geometry_msgs::msg::PoseStamped & dock_pose) - { - return panther_docking::PantherChargingDock::offsetStagingPoseToDockPose(dock_pose); - } - - geometry_msgs::msg::PoseStamped offsetDetectedDockPose( - const geometry_msgs::msg::PoseStamped & detected_dock_pose) - { - return panther_docking::PantherChargingDock::offsetDetectedDockPose(detected_dock_pose); - } - - geometry_msgs::msg::PoseStamped getDockPose(const std::string & frame) - { - return panther_docking::PantherChargingDock::getDockPose(frame); - } - void updateDockPoseAndPublish() - { - panther_docking::PantherChargingDock::updateDockPoseAndPublish(); - } - - void updateStagingPoseAndPublish(const std::string & frame) - { - panther_docking::PantherChargingDock::updateStagingPoseAndPublish(frame); - } -}; - -class TestPantherChargingDock : public testing::Test -{ -public: - TestPantherChargingDock(); - ~TestPantherChargingDock(); - -protected: - void ConfigureAndActivateDock(); - void SetTransform( - const std::string & frame_id, const std::string & child_frame_id, - const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform); - void SetDetectionOffsets(); - void SetStagingOffsets(); - - rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - tf2_ros::Buffer::SharedPtr tf2_buffer_; - - PantherChargingDockWrapper::UniquePtr charging_dock_; - - rclcpp::Subscription::SharedPtr dock_pose_sub_; - rclcpp::Subscription::SharedPtr staging_pose_sub_; - - geometry_msgs::msg::PoseStamped::SharedPtr dock_pose_; - geometry_msgs::msg::PoseStamped::SharedPtr staging_pose_; - - geometry_msgs::msg::Transform base_link_to_odom_transform_; - geometry_msgs::msg::Transform dock_to_base_transform_; - geometry_msgs::msg::Transform dock_to_base_at_external_detection_transform_; - - bool charging_status_; - - double external_detection_translation_x_ = 0.3; - double external_detection_translation_y_ = 0.1; - double external_detection_translation_z_ = 0.0; - double external_detection_roll_ = 0.0; - double external_detection_pitch_ = 0.0; - double external_detection_yaw_ = 1.57; - double staging_x_offset_ = 0.5; - double staging_yaw_offset_ = 1.57; -}; - -TestPantherChargingDock::TestPantherChargingDock() -{ - node_ = std::make_shared("panther_charging_dock_test"); - - tf2_buffer_ = std::make_shared(node_->get_clock()); - - dock_pose_sub_ = node_->create_subscription( - "docking/dock_pose", 10, - [&](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { dock_pose_ = msg; }); - - staging_pose_sub_ = node_->create_subscription( - "docking/staging_pose", 10, - [&](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { staging_pose_ = msg; }); - - // Silence error about dedicated thread's being necessary - tf2_buffer_->setUsingDedicatedThread(true); - - charging_dock_ = std::make_unique(); - - tf2::Quaternion rotation; - base_link_to_odom_transform_.translation.x = 0.3; - base_link_to_odom_transform_.translation.y = 0.2; - base_link_to_odom_transform_.translation.z = 0.1; - base_link_to_odom_transform_.rotation.x = 0.0; - base_link_to_odom_transform_.rotation.y = 0.0; - base_link_to_odom_transform_.rotation.z = 0.0; - base_link_to_odom_transform_.rotation.w = 1.0; - - dock_to_base_transform_.translation.x = 1.0; - dock_to_base_transform_.translation.y = 2.0; - dock_to_base_transform_.translation.z = 3.0; - rotation.setRPY(0.0, 0.0, 1.57); - dock_to_base_transform_.rotation = tf2::toMsg(rotation); - - dock_to_base_at_external_detection_transform_.translation.x = -external_detection_translation_y_; - dock_to_base_at_external_detection_transform_.translation.y = external_detection_translation_x_; - dock_to_base_at_external_detection_transform_.translation.z = external_detection_translation_z_; - rotation.setRPY(0.0, 0.0, -external_detection_yaw_); - dock_to_base_at_external_detection_transform_.rotation = tf2::toMsg(rotation); -} - -TestPantherChargingDock::~TestPantherChargingDock() -{ - if (charging_dock_) { - charging_dock_->deactivate(); - charging_dock_->cleanup(); - } - - if (executor_) { - executor_->cancel(); - } -} - -void TestPantherChargingDock::ConfigureAndActivateDock() -{ - charging_dock_->configure(node_, kDockFrame, tf2_buffer_); - charging_dock_->activate(); -} - -void TestPantherChargingDock::SetTransform( - const std::string & frame_id, const std::string & child_frame_id, - const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform) -{ - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.header.stamp = stamp; - transform_stamped.header.frame_id = frame_id; - transform_stamped.child_frame_id = child_frame_id; - transform_stamped.transform = transform; - - tf2_buffer_->setTransform(transform_stamped, "unittest", true); -} - -void TestPantherChargingDock::SetDetectionOffsets() -{ - node_->declare_parameter( - "test_dock.external_detection_translation_x", - rclcpp::ParameterValue(external_detection_translation_x_)); - node_->declare_parameter( - "test_dock.external_detection_translation_y", - rclcpp::ParameterValue(external_detection_translation_y_)); - node_->declare_parameter( - "test_dock.external_detection_translation_z", - rclcpp::ParameterValue(external_detection_translation_z_)); - node_->declare_parameter( - "test_dock.external_detection_rotation_roll", rclcpp::ParameterValue(external_detection_roll_)); - node_->declare_parameter( - "test_dock.external_detection_rotation_pitch", - rclcpp::ParameterValue(external_detection_pitch_)); - node_->declare_parameter( - "test_dock.external_detection_rotation_yaw", rclcpp::ParameterValue(external_detection_yaw_)); -} - -void TestPantherChargingDock::SetStagingOffsets() -{ - node_->declare_parameter("test_dock.staging_x_offset", rclcpp::ParameterValue(staging_x_offset_)); - node_->declare_parameter( - "test_dock.staging_yaw_offset", rclcpp::ParameterValue(staging_yaw_offset_)); -} - -TEST_F(TestPantherChargingDock, OffsetStagingPoseToDockPose) -{ - SetStagingOffsets(); - ConfigureAndActivateDock(); - - geometry_msgs::msg::PoseStamped pose; - pose = charging_dock_->offsetStagingPoseToDockPose(pose); - EXPECT_NEAR(pose.pose.position.x, staging_x_offset_, 0.01); - EXPECT_NEAR(pose.pose.position.y, 0.0, 0.01); - EXPECT_NEAR(tf2::getYaw(pose.pose.orientation), staging_yaw_offset_, 0.01); -} - -TEST_F(TestPantherChargingDock, OffsetDetectedDockPose) -{ - SetDetectionOffsets(); - ConfigureAndActivateDock(); - geometry_msgs::msg::PoseStamped pose; - pose.header.frame_id = kDockFrame; - pose = charging_dock_->offsetDetectedDockPose(pose); - - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header = pose.header; - expected_pose.pose.position.x = external_detection_translation_x_; - expected_pose.pose.position.y = external_detection_translation_y_; - expected_pose.pose.position.z = external_detection_translation_z_; - tf2::Quaternion expected_rotation; - expected_rotation.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, GetDockPose) -{ - SetDetectionOffsets(); - - ConfigureAndActivateDock(); - - EXPECT_THROW({ charging_dock_->getDockPose("wrong_dock_pose"); }, std::runtime_error); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - - geometry_msgs::msg::PoseStamped pose; - ASSERT_NO_THROW({ pose = charging_dock_->getDockPose(kDockFrame); };); - - ASSERT_EQ(pose.header.frame_id, kBaseFrame); - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header.frame_id = pose.header.frame_id; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_; - expected_pose.pose.position.z = 0.0; - tf2::Quaternion expected_rotation; - // 1.57 is from transformation from base_link to test_dock - expected_rotation.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); - - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, UpdateDockPoseAndStagingPosePublish) -{ - SetDetectionOffsets(); - SetStagingOffsets(); - - ConfigureAndActivateDock(); - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - SetTransform(kOdomFrame, kBaseFrame, node_->now(), base_link_to_odom_transform_); - - ASSERT_NO_THROW({ - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - charging_dock_->updateDockPoseAndPublish(); - charging_dock_->updateStagingPoseAndPublish(kBaseFrame); - }); - - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, dock_pose_, std::chrono::milliseconds(100))); - - ASSERT_EQ(dock_pose_->header.frame_id, kBaseFrame); - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header = dock_pose_->header; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_; - expected_pose.pose.position.z = 0.0; - tf2::Quaternion expected_rotation; - expected_rotation.setRPY(0.0, 0.0, external_detection_yaw_ + 1.57); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(*dock_pose_, expected_pose, 0.01, 0.01)); - - ASSERT_EQ(staging_pose_->header.frame_id, kBaseFrame); - expected_pose.header = staging_pose_->header; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_ - staging_x_offset_; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_; - expected_pose.pose.position.z = 0.0; - // 1.57 is from transformation from base_link to test_dock - expected_rotation.setRPY(0.0, 0.0, -1.57); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(*staging_pose_, expected_pose, 0.01, 0.01)); - - ASSERT_NO_THROW({ charging_dock_->updateStagingPoseAndPublish(kOdomFrame); }); - ASSERT_TRUE( - panther_utils::test_utils::WaitForMsg(node_, staging_pose_, std::chrono::milliseconds(100))); - - ASSERT_EQ(staging_pose_->header.frame_id, kOdomFrame); - expected_pose.header = staging_pose_->header; - expected_pose.pose.position.x = 1.0 - external_detection_translation_y_ - staging_x_offset_ + 0.3; - expected_pose.pose.position.y = 2.0 + external_detection_translation_x_ + 0.2; - expected_pose.pose.position.z = 0.1; - - // It is 4.71 but tf2::getYaw moves by 2pi what is 6.28 - expected_rotation.setRPY(0.0, 0.0, -1.57); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(*staging_pose_, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, GetStagingPose) -{ - SetStagingOffsets(); - ConfigureAndActivateDock(); - - geometry_msgs::msg::PoseStamped pose; - ASSERT_THROW( - { charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); }, - opennav_docking_core::FailedToDetectDock); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - ASSERT_NO_THROW( - { pose = charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); }); - ASSERT_EQ(pose.header.frame_id, kBaseFrame); - - geometry_msgs::msg::PoseStamped expected_pose; - expected_pose.header.frame_id = pose.header.frame_id; - expected_pose.pose.position.x = 1.0; - expected_pose.pose.position.y = 2.0 + staging_x_offset_; - expected_pose.pose.position.z = 0.0; - - tf2::Quaternion expected_rotation; - expected_rotation.setRPY(0.0, 0.0, 1.57 + staging_yaw_offset_); - expected_pose.pose.orientation = tf2::toMsg(expected_rotation); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, expected_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, GetRefinedPose) -{ - SetDetectionOffsets(); - ConfigureAndActivateDock(); - - geometry_msgs::msg::PoseStamped pose; - ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - - ASSERT_FALSE(charging_dock_->getRefinedPose(pose)); - - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - ASSERT_TRUE(charging_dock_->getRefinedPose(pose)); - - ASSERT_EQ(pose.header.frame_id, kBaseFrame); - tf2::Quaternion external_detection_rotation_; - // 1.57 is from transformation from base_link to test_dock - external_detection_rotation_.setRPY( - external_detection_roll_, external_detection_pitch_, external_detection_yaw_ + 1.57); - geometry_msgs::msg::PoseStamped refined_pose; - refined_pose.header = pose.header; - refined_pose.pose.position.x = 1.0 - external_detection_translation_y_; - refined_pose.pose.position.y = 2.0 + external_detection_translation_x_; - refined_pose.pose.position.z = 0.0; - refined_pose.pose.orientation = tf2::toMsg(external_detection_rotation_); - EXPECT_TRUE(panther_utils::tf2_utils::ArePosesNear(pose, refined_pose, 0.01, 0.01)); -} - -TEST_F(TestPantherChargingDock, IsDocked) -{ - SetDetectionOffsets(); - ConfigureAndActivateDock(); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_transform_); - SetTransform(kOdomFrame, kBaseFrame, node_->now(), base_link_to_odom_transform_); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - ASSERT_FALSE(charging_dock_->isDocked()); - - SetTransform(kBaseFrame, kDockFrame, node_->now(), dock_to_base_at_external_detection_transform_); - charging_dock_->getStagingPose(geometry_msgs::msg::Pose(), kDockFrame); - - EXPECT_TRUE(charging_dock_->isDocked()); -} - -TEST_F(TestPantherChargingDock, FailedConfigureCannotLockNode) -{ - rclcpp_lifecycle::LifecycleNode::SharedPtr node; - ASSERT_THROW({ charging_dock_->configure(node, kDockFrame, tf2_buffer_); }, std::runtime_error); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - - auto run_tests = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - return run_tests; -} diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp new file mode 100644 index 000000000..609497b34 --- /dev/null +++ b/panther_docking/test/unit/test_panther_charging_dock.cpp @@ -0,0 +1,209 @@ +// Copyright (c) 2024 Husarion Sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +#include +#include + +#include + +static constexpr char kBaseFrame[] = "base_link"; +static constexpr char kOdomFrame[] = "odom"; + +class PantherChargingDockWrapper : public panther_docking::PantherChargingDock +{ +public: + void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) + { + panther_docking::PantherChargingDock::setDockPose(pose); + } +}; + +class TestPantherChargingDock : public ::testing::Test +{ +protected: + TestPantherChargingDock(); + void SetTransform( + const std::string & frame_id, const std::string & child_frame_id, + const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr dock_; + rclcpp::Publisher::SharedPtr dock_pose_pub; + tf2_ros::Buffer::SharedPtr tf_buffer_; +}; + +TestPantherChargingDock::TestPantherChargingDock() +{ + node_ = std::make_shared("test_node"); + tf_buffer_ = std::make_shared(node_->get_clock()); + + // Silence error about dedicated thread's being necessary + tf_buffer_->setUsingDedicatedThread(true); + + dock_ = std::make_shared(); + dock_pose_pub = node_->create_publisher("dock_pose", 10); + + node_->configure(); + node_->activate(); +} + +void TestPantherChargingDock::SetTransform( + const std::string & frame_id, const std::string & child_frame_id, + const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = stamp; + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform = transform; + + tf_buffer_->setTransform(transform_stamped, "unittest", true); +} + +TEST_F(TestPantherChargingDock, FailConfigure) +{ + node_.reset(); + + ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, FailConfigureNoNode) +{ + node_.reset(); + ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, FailConfigureNoTfBuffer) +{ + tf_buffer_.reset(); + ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); +} + +TEST_F(TestPantherChargingDock, GetStagingPoseLocal) +{ + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); + + geometry_msgs::msg::PoseStamped::SharedPtr dock_pose = + std::make_shared(); + dock_pose->pose.position.x = 1.0; + dock_pose->pose.position.y = 1.0; + dock_pose->pose.position.z = 0.0; + dock_pose->pose.orientation.w = 1.0; + + dock_->setDockPose(dock_pose); + geometry_msgs::msg::PoseStamped pose; + + geometry_msgs::msg::PoseStamped staging_pose; + ASSERT_THROW( + { staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); }, + opennav_docking_core::FailedToDetectDock); + + dock_pose->header.frame_id = kOdomFrame; + dock_->setDockPose(dock_pose); + + staging_pose = dock_->getStagingPose(pose.pose, kOdomFrame); + + ASSERT_FLOAT_EQ(staging_pose.pose.position.x, 0.3); + ASSERT_FLOAT_EQ(staging_pose.pose.position.y, 1.0); + ASSERT_FLOAT_EQ(staging_pose.pose.position.z, 0.0); +} + +// TODO: fill after nav2 tests +// TEST_F(TestPantherChargingDock, GetStagingPoseGlobal){ +// } + +TEST_F(TestPantherChargingDock, GetRefinedPose) +{ + node_->declare_parameter("dock.external_detection_timeout", 0.5); + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); + + geometry_msgs::msg::PoseStamped::SharedPtr dock_pose = + std::make_shared(); + dock_pose->pose.position.x = 1.0; + dock_pose->pose.position.y = 1.0; + dock_pose->pose.position.z = 0.0; + dock_pose->pose.orientation.w = 1.0; + + dock_->setDockPose(dock_pose); + + geometry_msgs::msg::PoseStamped pose; + + ASSERT_THROW({ dock_->getRefinedPose(pose); }, opennav_docking_core::FailedToDetectDock); + + dock_pose->header.frame_id = kOdomFrame; + dock_->setDockPose(dock_pose); + ASSERT_FALSE(dock_->getRefinedPose(pose)); + + dock_pose->header.stamp = node_->now(); + dock_->setDockPose(dock_pose); + ASSERT_TRUE(dock_->getRefinedPose(pose)); + + ASSERT_FLOAT_EQ(pose.pose.position.x, 1.0); + ASSERT_FLOAT_EQ(pose.pose.position.y, 1.0); + ASSERT_FLOAT_EQ(pose.pose.position.z, 0.0); +} + +TEST_F(TestPantherChargingDock, IsDocked) +{ + node_->declare_parameter("dock.external_detection_timeout", 0.5); + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); + + auto transform = geometry_msgs::msg::Transform(); + transform.translation.x = 1.0; + transform.translation.y = 2.0; + transform.translation.z = 3.0; + + SetTransform(kOdomFrame, kBaseFrame, node_->now(), transform); + geometry_msgs::msg::PoseStamped::SharedPtr dock_pose = + std::make_shared(); + dock_pose->header.frame_id = kOdomFrame; + dock_pose->header.stamp = node_->now(); + dock_pose->pose.position.x = transform.translation.x - 0.1; + dock_pose->pose.position.y = transform.translation.y; + dock_pose->pose.position.z = transform.translation.z; + dock_->setDockPose(dock_pose); + + ASSERT_FALSE(dock_->isDocked()); + + dock_pose->pose.position.x = transform.translation.x; + dock_pose->pose.position.y = transform.translation.y; + dock_pose->pose.position.z = transform.translation.z; + // Set dock pose 10 times to ensure that filter stabilize the pose + for (std::size_t i = 0; i < 10; i++) { + dock_->setDockPose(dock_pose); + } + + ASSERT_TRUE(dock_->isDocked()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + + auto run_tests = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return run_tests; +} From d212d796f5fa53e102afcc6ef9cb3a3188ec5c20 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 30 Oct 2024 11:44:31 +0000 Subject: [PATCH 05/21] Update docs: Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 5 +++++ panther_docking/README.md | 29 +++++++++++++---------------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index e61f25791..a5f36370d 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -35,10 +35,15 @@ pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) add_library(panther_charging_dock SHARED src/panther_charging_dock.cpp) ament_target_dependencies(panther_charging_dock ${PACKAGE_DEPENDENCIES}) + # TODO @delihus how to link the library what is not a name of a package target_link_libraries(panther_charging_dock /opt/ros/humble/lib/libpose_filter.so) +target_include_directories( + panther_charging_dock PUBLIC $ + $) + install(TARGETS panther_charging_dock LIBRARY DESTINATION lib) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) diff --git a/panther_docking/README.md b/panther_docking/README.md index d41c232ce..98a76f46f 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -13,38 +13,35 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht ## ROS Nodes +- `DockPosePublisherNode`: A node listens to `tf` and republishes position of `dock_pose` in the fixed frame. - `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. -### PantherChargingDock +### DockPosePublisherNode #### Publishes - `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. -- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station. #### Subscribers -- `battery/charging_status` [*panther_msgs/ChargingStatus*]: A charging status of Panther robot. -- `hardware/io_state` [*panther_msgs/IOState*]: States of GPIOs of Panther robot. The `PantherChargingDock` subscribes this topic to check if a robot charges. +- `tf` [*tf2_msgs/TFMessage*]: Tf tree with a detected dock transform. + +### PantherChargingDock + +#### Publishes -#### Service Clients +- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station. -- `hardware/charger_enable` [*std_srvs/SetBoot*]: This service client enables charging in a robot. +#### Subscribers + +- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. #### 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 a Z axis between a detected frame and a dock pose. -- `~.external_detection_rotation_roll` [*double*, default: **0.0**]: A rotation over an X axis between a detected frame and a dock pose. -- `~.external_detection_rotation_pitch` [*double*, default: **0.0**]: A rotation over an Y axis between a detected frame and a dock pose. -- `~.external_detection_rotation_yaw` [*double*, default: **0.0**]: A rotation over a Z axis between a detected frame and a dock pose. +- `~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. - `~.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations. - `~.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed. - `~.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed. - `~.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. -- `~.staging_yaw_offset` [*double*, default: **0.0**]: A staging pose is defined by offsetting a yaw angle. -- `~.enable_charger_service_call_timeout` [*double*, default: **0.2**]: A timeout for calling enable charging service. A robot is unable to dock if excised. From d4edaaf86f61572a969feb20a6fa3c6123f7b4c0 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 31 Oct 2024 15:12:25 +0000 Subject: [PATCH 06/21] Fix upside down Signed-off-by: Jakub Delicat --- panther_description/config/docking_components.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_description/config/docking_components.yaml b/panther_description/config/docking_components.yaml index 169b3a13e..940eb137e 100644 --- a/panther_description/config/docking_components.yaml +++ b/panther_description/config/docking_components.yaml @@ -9,12 +9,12 @@ components: - type: WCH02 parent_link: world xyz: 3.0 -2.0 1.0 - rpy: -1.57 0.0 1.57 + rpy: 1.57 0.0 -1.57 device_namespace: wireless_charger - type: CAM01 name: camera - parent_link: lights_channel_1_link - xyz: 0.05 0.0 0.0 + parent_link: front_bumper_link + xyz: 0.0 0.0 -0.06 rpy: 0.0 0.0 0.0 device_namespace: camera From 4a04543de3c309f326aa3f2e03cfc4642a4ce4a5 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 31 Oct 2024 15:16:03 +0000 Subject: [PATCH 07/21] Added updateAndPublishStagingPose method Signed-off-by: Jakub Delicat --- .../panther_docking/panther_charging_dock.hpp | 2 ++ panther_docking/launch/station.launch.py | 4 ++-- panther_docking/src/panther_charging_dock.cpp | 19 +++++++++++++------ 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 5258c1aa8..ca4b37c07 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -140,6 +140,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); + void updateAndPublishStagingPose(); + void setDockPose(const PoseStampedMsg::SharedPtr pose); std::string base_frame_name_; diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py index ee92de913..b60fbeb4a 100644 --- a/panther_docking/launch/station.launch.py +++ b/panther_docking/launch/station.launch.py @@ -33,7 +33,7 @@ def generate_apriltag_and_get_path(tag_id): tag_generator = TagGenerator2("tag36h11") - tag_image = tag_generator.generate(tag_id, scale=100) + tag_image = tag_generator.generate(tag_id, scale=1000) path = f"/tmp/tag_{tag_id}.png" @@ -105,7 +105,7 @@ def generate_launch_description(): declare_apriltag_size = DeclareLaunchArgument( "apriltag_size", - default_value="0.07", + default_value="0.06", description="Size in meters of a generated apriltag on the station", ) diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 105ae9380..40788b690 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -119,12 +119,7 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( throw opennav_docking_core::FailedToDetectDock("No dock pose detected"); } - const double yaw = tf2::getYaw(dock_pose_.pose.orientation); - staging_pose_ = dock_pose_; - staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; - staging_pose_.pose.position.y += sin(yaw) * staging_x_offset_; - - staging_pose_pub_->publish(staging_pose_); + updateAndPublishStagingPose(); } return staging_pose_; @@ -153,6 +148,8 @@ bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) } pose = dock_pose_; + updateAndPublishStagingPose(); + return true; } @@ -188,6 +185,16 @@ void PantherChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) dock_pose_ = filtered_pose; } +void PantherChargingDock::updateAndPublishStagingPose() +{ + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + staging_pose_ = dock_pose_; + staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose_.pose.position.y += sin(yaw) * staging_x_offset_; + + staging_pose_pub_->publish(staging_pose_); +} + } // namespace panther_docking #include "pluginlib/class_list_macros.hpp" From 4a23171a982359036c68fa20f593cde9c0835c40 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 31 Oct 2024 16:21:03 +0000 Subject: [PATCH 08/21] Added suggestins | added reading docking_server parameter is dock pose publisher Signed-off-by: Jakub Delicat --- panther_docking/CMakeLists.txt | 3 +- panther_docking/README.md | 20 +++++++----- .../config/panther_docking_server.yaml | 9 +++--- panther_docking/launch/docking.launch.py | 5 +++ .../src/dock_pose_publisher_node.cpp | 31 ++++++++++++++----- panther_docking/src/panther_charging_dock.cpp | 8 ++--- 6 files changed, 51 insertions(+), 25 deletions(-) diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index a5f36370d..6295b9038 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -61,8 +61,7 @@ if(BUILD_TESTING) endif() add_executable(dock_pose_publisher src/dock_pose_publisher_node.cpp) -ament_target_dependencies(dock_pose_publisher rclcpp tf2_ros geometry_msgs - tf2_geometry_msgs) +ament_target_dependencies(dock_pose_publisher ${PACKAGE_DEPENDENCIES}) install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME}) diff --git a/panther_docking/README.md b/panther_docking/README.md index 98a76f46f..5aa076fac 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -26,6 +26,12 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `tf` [*tf2_msgs/TFMessage*]: Tf tree with a detected dock transform. +#### Parameters + +- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. +- `.type` [*string*, default: **panther_charging_dock**]: It checks if this dock with name `dock_name` is a type of `panther_charging_dock`. +- `.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose` + ### PantherChargingDock #### Publishes @@ -38,10 +44,10 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht #### 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. -- `~.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. +- `base_frame` [*string*, default: **base_link**]: A base frame id of a robot. +- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. +- `.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id. +- `.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed. +- `.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed. +- `.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. +- `.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations. diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 4b520ecd3..8bc198f16 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -16,15 +16,16 @@ dock_plugins: ["panther_charging_dock"] panther_charging_dock: plugin: panther_docking::PantherChargingDock + external_detection_timeout: 0.1 docking_distance_threshold: 0.08 docking_yaw_threshold: 0.1 staging_x_offset: -0.5 - external_detection_timeout: 0.1 + filter_coef: 0.1 - docks: ["main_dock"] - main_dock: + docks: ["main"] + main: type: panther_charging_dock - frame: /main_wibotic_station_link + frame: /main_wibotic_receiver_requested_pose_link pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot controller: diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 05739f10e..5e510ac1f 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -55,6 +55,7 @@ def generate_launch_description(): "log_level", default_value="info", description="Logging level", + choices=["debug", "info", "warning", "error"], ) namespaced_docking_server_config = ReplaceString( @@ -93,6 +94,10 @@ def generate_launch_description(): dock_pose_publisher = Node( package="panther_docking", executable="dock_pose_publisher", + parameters=[ + namespaced_docking_server_config, + {"use_sim_time": use_sim}, + ], name="dock_pose_publisher", namespace=namespace, emulate_tty=True, diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp index b41cf2bc9..2784ebf03 100644 --- a/panther_docking/src/dock_pose_publisher_node.cpp +++ b/panther_docking/src/dock_pose_publisher_node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -27,13 +28,29 @@ class DockPosePublisherNode : public rclcpp::Node public: DockPosePublisherNode() : Node("pose_publisher_node") { - declare_parameter("device_namespace", "main"); - std::string device_namespace = get_parameter("device_namespace").as_string(); - std::string robot_namespace = std::string(get_namespace()).substr(1); + declare_parameter("publish_rate", 10.0); + declare_parameter("docks", std::vector({"main"})); + declare_parameter("fixed_frame", "odom"); - source_frame_ = robot_namespace + "/" + device_namespace + - "_wibotic_receiver_requested_pose_link"; - target_frame_ = robot_namespace + "/odom"; + const auto fixed_frame = get_parameter("fixed_frame").as_string(); + const auto docks = get_parameter("docks").as_string_array(); + + std::string dock_pose_frame_id; + for (const auto & dock : docks) { + declare_parameter(dock + ".type", "panther_charging_dock"); + declare_parameter(dock + ".frame", "main_wibotic_receiver_requested_pose_link"); + + const auto dock_type = get_parameter(dock + ".type").as_string(); + if (dock_type == "panther_charging_dock") { + dock_pose_frame_id = get_parameter(dock + ".frame").as_string(); + } + } + + const auto publish_rate = get_parameter("publish_rate").as_double(); + const auto publish_period = std::chrono::duration(1.0 / publish_rate); + + source_frame_ = dock_pose_frame_id; + target_frame_ = fixed_frame; pose_publisher_ = this->create_publisher( "docking/dock_pose", 10); @@ -42,7 +59,7 @@ class DockPosePublisherNode : public rclcpp::Node tf_listener_ = std::make_shared(*tf_buffer_); timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), std::bind(&DockPosePublisherNode::publishPose, this)); + publish_period, std::bind(&DockPosePublisherNode::publishPose, this)); } private: diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 40788b690..f6bc0c8ea 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -77,18 +77,16 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod nav2_util::declare_parameter_if_not_declared(node, "fixed_frame", rclcpp::ParameterValue("odom")); + 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_ + ".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_ + ".external_detection_timeout", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( node, name_ + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); - nav2_util::declare_parameter_if_not_declared( - node, name_ + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); nav2_util::declare_parameter_if_not_declared( node, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); From 38186f8ff09549156c66feb84af86adcec00f4dd Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 4 Nov 2024 08:45:37 +0000 Subject: [PATCH 09/21] Remove redundant tests Signed-off-by: Jakub Delicat --- panther_docking/test/unit/test_panther_charging_dock.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp index 609497b34..435e7bdb6 100644 --- a/panther_docking/test/unit/test_panther_charging_dock.cpp +++ b/panther_docking/test/unit/test_panther_charging_dock.cpp @@ -78,13 +78,6 @@ void TestPantherChargingDock::SetTransform( tf_buffer_->setTransform(transform_stamped, "unittest", true); } -TEST_F(TestPantherChargingDock, FailConfigure) -{ - node_.reset(); - - ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); -} - TEST_F(TestPantherChargingDock, FailConfigureNoNode) { node_.reset(); From 5b6f5c7e89eb0f35a88d3af40ecb1783effdd6d1 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 7 Oct 2024 13:03:19 +0000 Subject: [PATCH 10/21] Added reading wibotic_info Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 4 ++ panther_docking/CMakeLists.txt | 3 +- panther_docking/README.md | 4 +- .../config/panther_docking_server.yaml | 2 + .../panther_docking/panther_charging_dock.hpp | 31 ++++++++ panther_docking/launch/docking.launch.py | 18 ++++- panther_docking/package.xml | 1 + panther_docking/src/panther_charging_dock.cpp | 49 ++++++++++++- .../test/unit/test_panther_charging_dock.cpp | 70 ++++++++++++++++++- 9 files changed, 176 insertions(+), 6 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index c6df86e10..8e4810696 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -19,3 +19,7 @@ repositories: type: git url: https://github.com/husarion/ros2_controllers/ version: 9da42a07a83bbf3faf5cad11793fff22f25068af + wibotic_ros: + type: git + url: https://github.com/husarion/wibotic_ros/ + version: 0.1.0 diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 6295b9038..7af73bfb4 100644 --- a/panther_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -16,7 +16,8 @@ set(PACKAGE_DEPENDENCIES sensor_msgs std_srvs tf2_geometry_msgs - tf2_ros) + tf2_ros + wibotic_msgs) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) diff --git a/panther_docking/README.md b/panther_docking/README.md index 5aa076fac..a8a314f0c 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -46,8 +46,10 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `base_frame` [*string*, default: **base_link**]: A base frame id of a robot. - `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. -- `.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id. +- `.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for dock pose. - `.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed. - `.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed. - `.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. - `.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations. +- `.use_wibotic_info` [*bool*, default: **True**]: Use readings from `wibotic_info` topics to ensure that a robot is charging. +- `.wibotic_info_timeout` [*double*, default: **0.2**]: A timeout in seconds for wibotic_info. diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 8bc198f16..b960218c0 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -21,6 +21,8 @@ docking_yaw_threshold: 0.1 staging_x_offset: -0.5 filter_coef: 0.1 + use_wibotic: + wibotic_info_timeout: 0.5 docks: ["main"] main: diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index ca4b37c07..7523d8fa4 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -32,9 +32,13 @@ #include #include +#include "wibotic_msgs/msg/wibotic_info.hpp" + namespace panther_docking { +constexpr double kWiboticChargingCurrentThreshold = 0.0; + /** * @class PantherChargingDock * @brief A class to represent a Panther charging dock. @@ -45,6 +49,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock using SharedPtr = std::shared_ptr; using UniquePtr = std::unique_ptr; using PoseStampedMsg = geometry_msgs::msg::PoseStamped; + using WiboticInfoMsg = wibotic_msgs::msg::WiboticInfo; /** * @brief Configure the dock with the necessary information. @@ -140,10 +145,31 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); + /** + * @brief Method to update and publish the staging pose. + * + * Uses staging_x_offset_ and staging_yaw_offset_ to calculate the staging pose. + */ void updateAndPublishStagingPose(); + /** + * @brief Set the dock pose. + * + * This method sets the dock pose. It can be used as a callback for a subscription. + * + * @param pose The dock pose. + */ void setDockPose(const PoseStampedMsg::SharedPtr pose); + /** + * @brief Set the Wibotic info. + * + * This method sets the Wibotic info. It can be used as a callback for a subscription. + * + * @param msg The Wibotic info message. + */ + void setWiboticInfo(const WiboticInfoMsg::SharedPtr msg); + std::string base_frame_name_; std::string fixed_frame_name_; std::string dock_frame_; @@ -156,9 +182,11 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock rclcpp::Publisher::SharedPtr staging_pose_pub_; rclcpp::Subscription::SharedPtr dock_pose_sub_; + rclcpp::Subscription::SharedPtr wibotic_info_sub_; PoseStampedMsg dock_pose_; PoseStampedMsg staging_pose_; + WiboticInfoMsg::SharedPtr wibotic_info_; double external_detection_timeout_; @@ -171,6 +199,9 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double staging_yaw_offset_; double pose_filter_coef_; + + bool use_wibotic_info_; + double wibotic_info_timeout_; }; } // namespace panther_docking diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 5e510ac1f..c24c37f7c 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -19,6 +19,7 @@ EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, + PythonExpression, ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -58,9 +59,23 @@ def generate_launch_description(): choices=["debug", "info", "warning", "error"], ) + use_wibotic_info = LaunchConfiguration("use_wibotic_info") + declare_use_wibotic_info_arg = DeclareLaunchArgument( + "use_wibotic_info", + default_value="False", + description="Whether Wibotic information is used", + choices=[True, False, "True", "False", "true", "false", "1", "0"], + ) + namespaced_docking_server_config = ReplaceString( source_file=docking_server_config_path, - replacements={"": namespace, "//": "/"}, + replacements={ + "": namespace, + "//": "/", + "": PythonExpression( + ["'false' if '", use_sim, "' else '", use_wibotic_info, "'"] + ), + }, ) docking_server_node = Node( @@ -123,6 +138,7 @@ def generate_launch_description(): declare_namespace_arg, declare_docking_server_config_path_arg, declare_log_level, + declare_use_wibotic_info_arg, station_launch, docking_server_node, docking_server_activate_node, diff --git a/panther_docking/package.xml b/panther_docking/package.xml index f665b0c6e..8aba21037 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -25,6 +25,7 @@ sensor_msgs std_srvs tf2_ros + wibotic_msgs nav2_lifecycle_manager python3-imageio diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index f6bc0c8ea..0566bdc17 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -62,6 +62,12 @@ void PantherChargingDock::activate() "docking/dock_pose", 1, std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1)); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); + + if (use_wibotic_info_) { + wibotic_info_sub_ = node->create_subscription( + "wibotic_info", 1, + std::bind(&PantherChargingDock::setWiboticInfo, this, std::placeholders::_1)); + } } void PantherChargingDock::deactivate() @@ -90,6 +96,12 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod nav2_util::declare_parameter_if_not_declared( node, name_ + ".filter_coef", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".use_wibotic_info", rclcpp::ParameterValue(true)); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".wibotic_info_timeout", rclcpp::ParameterValue(1.5)); } void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) @@ -103,6 +115,9 @@ void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::S node->get_parameter(name_ + ".staging_x_offset", staging_x_offset_); node->get_parameter(name_ + ".filter_coef", pose_filter_coef_); + + node->get_parameter(name_ + ".use_wibotic_info", use_wibotic_info_); + node->get_parameter(name_ + ".wibotic_info_timeout", wibotic_info_timeout_); } // When there is no pose actual position of robot is a staging pose @@ -167,10 +182,37 @@ bool PantherChargingDock::isCharging() { RCLCPP_DEBUG(logger_, "Checking if charging"); try { - return isDocked(); + if (!use_wibotic_info_) { + return isDocked(); + } + + if (!wibotic_info_) { + throw opennav_docking_core::FailedToCharge("No Wibotic info received."); + } + + rclcpp::Time requested_wibotic_info_time; + { + auto node = node_.lock(); + requested_wibotic_info_time = node->now(); + } + + const auto duration = requested_wibotic_info_time - wibotic_info_->header.stamp; + if (duration > rclcpp::Duration::from_seconds(wibotic_info_timeout_)) { + RCLCPP_WARN_STREAM( + logger_, "Wibotic info is outdated. Time difference is: " + << duration.seconds() << "s but timeout is " << wibotic_info_timeout_ << "s."); + return false; + } + + if (wibotic_info_->i_charger > kWiboticChargingCurrentThreshold) { + return true; + } } catch (const opennav_docking_core::FailedToDetectDock & e) { + RCLCPP_ERROR_STREAM(logger_, "An occurred error while checking if charging: " << e.what()); return false; } + + return false; } bool PantherChargingDock::disableCharging() { return true; } @@ -193,6 +235,11 @@ void PantherChargingDock::updateAndPublishStagingPose() staging_pose_pub_->publish(staging_pose_); } +void PantherChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg) +{ + wibotic_info_ = std::make_shared(*msg); +} + } // namespace panther_docking #include "pluginlib/class_list_macros.hpp" diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp index 435e7bdb6..80a7a7593 100644 --- a/panther_docking/test/unit/test_panther_charging_dock.cpp +++ b/panther_docking/test/unit/test_panther_charging_dock.cpp @@ -30,9 +30,14 @@ static constexpr char kOdomFrame[] = "odom"; class PantherChargingDockWrapper : public panther_docking::PantherChargingDock { public: - void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) + void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr msg) { - panther_docking::PantherChargingDock::setDockPose(pose); + panther_docking::PantherChargingDock::setDockPose(msg); + } + + void setWiboticInfo(wibotic_msgs::msg::WiboticInfo::SharedPtr msg) + { + panther_docking::PantherChargingDock::setWiboticInfo(msg); } }; @@ -44,6 +49,8 @@ class TestPantherChargingDock : public ::testing::Test const std::string & frame_id, const std::string & child_frame_id, const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform); + void ActivateWiboticInfo(); + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; std::shared_ptr dock_; rclcpp::Publisher::SharedPtr dock_pose_pub; @@ -78,6 +85,14 @@ void TestPantherChargingDock::SetTransform( tf_buffer_->setTransform(transform_stamped, "unittest", true); } +void TestPantherChargingDock::ActivateWiboticInfo() +{ + node_->declare_parameter("dock.use_wibotic_info", true); + node_->declare_parameter("dock.wibotic_info_timeout", 1.0); + dock_->configure(node_, "dock", tf_buffer_); + dock_->activate(); +} + TEST_F(TestPantherChargingDock, FailConfigureNoNode) { node_.reset(); @@ -190,6 +205,57 @@ TEST_F(TestPantherChargingDock, IsDocked) ASSERT_TRUE(dock_->isDocked()); } +TEST_F(TestPantherChargingDock, IsChargingNoWiboticInfo) +{ + ActivateWiboticInfo(); + ASSERT_THROW({ dock_->isCharging(); }, opennav_docking_core::FailedToCharge); +} + +TEST_F(TestPantherChargingDock, IsChargingTimeout) +{ + ActivateWiboticInfo(); + + wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = + std::make_shared(); + dock_->setWiboticInfo(wibotic_info); + ASSERT_FALSE(dock_->isCharging()); +} + +TEST_F(TestPantherChargingDock, IsChargingCurrentZero) +{ + ActivateWiboticInfo(); + wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = + std::make_shared(); + wibotic_info->header.stamp = node_->now(); + wibotic_info->i_charger = 0.0; + + dock_->setWiboticInfo(wibotic_info); + ASSERT_FALSE(dock_->isCharging()); +} + +TEST_F(TestPantherChargingDock, IsChargingTimeoutWithGoodCurrent) +{ + ActivateWiboticInfo(); + wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = + std::make_shared(); + wibotic_info->i_charger = 0.1; + + dock_->setWiboticInfo(wibotic_info); + ASSERT_FALSE(dock_->isCharging()); +} + +TEST_F(TestPantherChargingDock, IsChargingGoodCurrentWithoutTimeout) +{ + ActivateWiboticInfo(); + wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = + std::make_shared(); + wibotic_info->i_charger = 0.1; + wibotic_info->header.stamp = node_->now(); + + dock_->setWiboticInfo(wibotic_info); + ASSERT_TRUE(dock_->isCharging()); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From 6e966851d848961e74047da404b20de3200b990f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 5 Nov 2024 08:48:13 +0000 Subject: [PATCH 11/21] Added launching wibotic_connector_can Signed-off-by: Jakub Delicat --- panther_docking/launch/docking.launch.py | 9 +++++++++ panther_docking/package.xml | 1 + 2 files changed, 10 insertions(+) diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index c24c37f7c..778cb64b1 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -132,6 +132,14 @@ def generate_launch_description(): launch_arguments={"namespace": namespace}.items(), ) + wibotic_connector_can = Node( + package="wibotic_connector_can", + executable="wibotic_connector_can", + namespace=namespace, + emulate_tty=True, + condition=PythonExpression(["'false' if '", use_sim, "' else '", use_wibotic_info, "'"]), + ) + return LaunchDescription( [ declare_use_sim_arg, @@ -143,5 +151,6 @@ def generate_launch_description(): docking_server_node, docking_server_activate_node, dock_pose_publisher, + wibotic_connector_can, ] ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 8aba21037..2130b3d3d 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -29,6 +29,7 @@ nav2_lifecycle_manager python3-imageio + wibotic_connector_can xacro ament_cmake From 12ed57149320042828566aa25e032a86ba0b90fc Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 5 Nov 2024 09:30:41 +0000 Subject: [PATCH 12/21] Added right condition to wibotic_connector_can Signed-off-by: Jakub Delicat --- panther_docking/launch/docking.launch.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 778cb64b1..5749c0b64 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( EnvironmentVariable, @@ -62,7 +63,7 @@ def generate_launch_description(): use_wibotic_info = LaunchConfiguration("use_wibotic_info") declare_use_wibotic_info_arg = DeclareLaunchArgument( "use_wibotic_info", - default_value="False", + default_value="True", description="Whether Wibotic information is used", choices=[True, False, "True", "False", "true", "false", "1", "0"], ) @@ -137,7 +138,8 @@ def generate_launch_description(): executable="wibotic_connector_can", namespace=namespace, emulate_tty=True, - condition=PythonExpression(["'false' if '", use_sim, "' else '", use_wibotic_info, "'"]), + arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], + condition=IfCondition(PythonExpression(["not ", use_sim, " and ", use_wibotic_info])), ) return LaunchDescription( From def8fd261e808a0a25b3d6f65cb3b8a245a09f38 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 12 Nov 2024 19:51:40 +0000 Subject: [PATCH 13/21] wibotic_ros is needed to build Signed-off-by: Jakub Delicat --- panther/panther_simulation.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index d6c6932fc..33a8726af 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -23,3 +23,7 @@ repositories: type: git url: https://github.com/husarion/husarion_gz_worlds.git version: 9d514a09c74bca2afbfba76cf2c87134918bbf97 + wibotic_ros: + type: git + url: https://github.com/husarion/wibotic_ros/ + version: 0.1.0 From 7ae2294a98fc4c9ce9599c72f789ed85cae0cf8b Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 12 Nov 2024 22:25:15 +0100 Subject: [PATCH 14/21] Update repose, update parameters Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- panther_docking/config/panther_docking_server.yaml | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 8e4810696..4bd5c9de1 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: 99e0189970293bb0110aad0a3b609bc659ecc663 + version: b29f41ac00ab1a6fbac3e1d03602575094de277a ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index 33a8726af..47db255bf 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: 99e0189970293bb0110aad0a3b609bc659ecc663 + version: b29f41ac00ab1a6fbac3e1d03602575094de277a ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index b960218c0..dd32a8303 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -2,7 +2,7 @@ ros__parameters: controller_frequency: 50.0 initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 + wait_charge_timeout: 10.0 dock_approach_timeout: 20.0 undock_linear_tolerance: 0.08 undock_angular_tolerance: 0.08 @@ -17,12 +17,12 @@ panther_charging_dock: plugin: panther_docking::PantherChargingDock external_detection_timeout: 0.1 - docking_distance_threshold: 0.08 - docking_yaw_threshold: 0.1 + docking_distance_threshold: 0.12 + docking_yaw_threshold: 0.15 staging_x_offset: -0.5 filter_coef: 0.1 use_wibotic: - wibotic_info_timeout: 0.5 + wibotic_info_timeout: 1.0 docks: ["main"] main: @@ -34,5 +34,5 @@ k_phi: 1.0 k_delta: 2.0 v_linear_min: 0.0 - v_linear_max: 0.3 + v_linear_max: 0.1 v_angular_max: 0.15 From bef1139d6cdd34f0a384f5ae3e4abc945041595a Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 12 Nov 2024 21:38:41 +0000 Subject: [PATCH 15/21] Added condition for use_docking Signed-off-by: Jakub Delicat --- panther_docking/launch/docking.launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index d9b9764cc..2b090111e 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -116,6 +116,7 @@ def generate_launch_description(): namespace=namespace, emulate_tty=True, arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], + condition=IfCondition(use_docking), ) station_launch = IncludeLaunchDescription( @@ -137,7 +138,9 @@ def generate_launch_description(): namespace=namespace, emulate_tty=True, arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], - condition=IfCondition(PythonExpression(["not ", use_sim, " and ", use_wibotic_info])), + condition=IfCondition( + PythonExpression(["not ", use_sim, " and ", use_wibotic_info, " and ", use_docking]) + ), ) return LaunchDescription( From 4977ccb63eb9774af3e2c8f35dff9e137ef472f9 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 12 Nov 2024 21:47:18 +0000 Subject: [PATCH 16/21] update repose Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 4bd5c9de1..43d0fa813 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: b29f41ac00ab1a6fbac3e1d03602575094de277a + version: fa3a4a9a09d793b16ed23cbeb09cf4c4a892b9e3 ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index 47db255bf..f6939ad09 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: b29f41ac00ab1a6fbac3e1d03602575094de277a + version: fa3a4a9a09d793b16ed23cbeb09cf4c4a892b9e3 ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ From b6624d41f0dc7de9a41145016cbe0721843d6b17 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Nov 2024 09:36:51 +0100 Subject: [PATCH 17/21] Fixed use_wibotic_info param Signed-off-by: Jakub Delicat --- .../config/panther_docking_server.yaml | 2 +- panther_docking/launch/docking.launch.py | 17 +++++++++++++++++ panther_docking/package.xml | 3 +++ panther_docking/src/panther_charging_dock.cpp | 5 +++++ 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index dd32a8303..b0c3791e0 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -21,7 +21,7 @@ docking_yaw_threshold: 0.15 staging_x_offset: -0.5 filter_coef: 0.1 - use_wibotic: + use_wibotic_info: wibotic_info_timeout: 1.0 docks: ["main"] diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 5749c0b64..0959fe2f3 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -142,6 +142,22 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(["not ", use_sim, " and ", use_wibotic_info])), ) + # FIXME: Remove before release + panther_manager_dir = FindPackageShare("panther_manager") + docking_manager_node = Node( + package="panther_manager", + executable="docking_manager_node", + name="docking_manager", + parameters=[ + PathJoinSubstitution([panther_manager_dir, "config", "docking_manager.yaml"]), + {"bt_project_path": PathJoinSubstitution( + [panther_manager_dir, "behavior_trees", "DockingBT.btproj"])}, + ], + namespace=namespace, + emulate_tty=True, + ) + + return LaunchDescription( [ declare_use_sim_arg, @@ -154,5 +170,6 @@ def generate_launch_description(): docking_server_activate_node, dock_pose_publisher, wibotic_connector_can, + docking_manager_node ] ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 2130b3d3d..2f5b65732 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -27,6 +27,9 @@ tf2_ros wibotic_msgs + + panther_manager + nav2_lifecycle_manager python3-imageio wibotic_connector_can diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 0566bdc17..e9d167414 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -45,6 +45,11 @@ void PantherChargingDock::configure( declareParameters(node); getParameters(node); + if(!use_wibotic_info_){ + RCLCPP_INFO(logger_, "Wibotic info is disabled."); + } + + pose_filter_ = std::make_unique( pose_filter_coef_, external_detection_timeout_); } From 0a84793dc84c9ea5b350055f2827be506b7dadd8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Nov 2024 10:47:36 +0000 Subject: [PATCH 18/21] Rafal's suggestions applied Signed-off-by: Jakub Delicat --- README.md | 1 + panther_docking/README.md | 14 +++++++------- .../panther_docking/panther_charging_dock.hpp | 6 ++---- panther_docking/launch/docking.launch.py | 10 ++++++---- panther_docking/package.xml | 2 +- panther_docking/src/panther_charging_dock.cpp | 3 +-- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 3e7a1f2e8..a61cfcdac 100644 --- a/README.md +++ b/README.md @@ -109,6 +109,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `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:*** `''` | +| 🤖🖥️ | `panther_charging_dock.use_wibotic_info` | Use readings from `wibotic_info` topics to ensure that a robot is charging.
**bool**: `True` | | 🤖🖥️ | `wheel_config_path` | Path to wheel configuration file.
***string:*** [`{wheel_type}.yaml`](./panther_description/config) | | 🤖🖥️ | `wheel_type` | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels.
***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) | | 🖥️ | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | diff --git a/panther_docking/README.md b/panther_docking/README.md index 0fa6c19f1..0e3059cd0 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -46,10 +46,10 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `base_frame` [*string*, default: **base_link**]: A base frame id of a robot. - `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. -- `.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id. -- `.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed. -- `.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed. -- `.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. -- `.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations. -- `.use_wibotic_info` [*bool*, default: **True**]: Use readings from `wibotic_info` topics to ensure that a robot is charging. -- `.wibotic_info_timeout` [*double*, default: **0.2**]: A timeout in seconds for wibotic_info. +- `panther_charging_dock.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. +- `panther_charging_dock.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. +- `panther_charging_dock.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. +- `panther_charging_dock.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. +- `panther_charging_dock.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. +- `panther_charging_dock.use_wibotic_info` [*bool*, default: **True**]: Use readings from `wibotic_info` topics to ensure that a robot is charging. +- `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive wibotic_info. diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 7523d8fa4..2897fda4c 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -155,16 +155,14 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock /** * @brief Set the dock pose. * - * This method sets the dock pose. It can be used as a callback for a subscription. + * This method sets the dock pose. It can be used as a callback for a subscription. * * @param pose The dock pose. */ void setDockPose(const PoseStampedMsg::SharedPtr pose); /** - * @brief Set the Wibotic info. - * - * This method sets the Wibotic info. It can be used as a callback for a subscription. + * @brief Wibotic info callback, used when `use_wibotic_info` is enabled. * * @param msg The Wibotic info message. */ diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 189c9786d..68690e3fc 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -151,14 +151,16 @@ def generate_launch_description(): name="docking_manager", parameters=[ PathJoinSubstitution([panther_manager_dir, "config", "docking_manager.yaml"]), - {"bt_project_path": PathJoinSubstitution( - [panther_manager_dir, "behavior_trees", "DockingBT.btproj"])}, + { + "bt_project_path": PathJoinSubstitution( + [panther_manager_dir, "behavior_trees", "DockingBT.btproj"] + ) + }, ], namespace=namespace, emulate_tty=True, ) - return LaunchDescription( [ declare_use_docking_arg, @@ -170,6 +172,6 @@ def generate_launch_description(): docking_server_activate_node, dock_pose_publisher, wibotic_connector_can, - docking_manager_node + docking_manager_node, ] ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 2f5b65732..eb9ece005 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -17,6 +17,7 @@ geometry_msgs nav2_util opennav_docking + panther_manager panther_utils pluginlib python3-pip @@ -28,7 +29,6 @@ wibotic_msgs - panther_manager nav2_lifecycle_manager python3-imageio diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index e9d167414..9890e97e4 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -45,11 +45,10 @@ void PantherChargingDock::configure( declareParameters(node); getParameters(node); - if(!use_wibotic_info_){ + if (!use_wibotic_info_) { RCLCPP_INFO(logger_, "Wibotic info is disabled."); } - pose_filter_ = std::make_unique( pose_filter_coef_, external_detection_timeout_); } From 48374e17850a6bfa14b77343ce2ce4d57001686c Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 19 Nov 2024 08:44:50 +0000 Subject: [PATCH 19/21] README update and comment delete Signed-off-by: Jakub Delicat --- README.md | 2 +- panther_docking/README.md | 2 +- panther_docking/package.xml | 2 -- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index a61cfcdac..f64a41180 100644 --- a/README.md +++ b/README.md @@ -109,7 +109,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev | 🤖🖥️ | `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:*** `''` | -| 🤖🖥️ | `panther_charging_dock.use_wibotic_info` | Use readings from `wibotic_info` topics to ensure that a robot is charging.
**bool**: `True` | +| 🤖🖥️ | `use_wibotic_info` |Whether Wibotic information is used.
**bool**: `True` | | 🤖🖥️ | `wheel_config_path` | Path to wheel configuration file.
***string:*** [`{wheel_type}.yaml`](./panther_description/config) | | 🤖🖥️ | `wheel_type` | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels.
***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) | | 🖥️ | `x` | Initial robot position in the global 'x' axis.
***float:*** `0.0` | diff --git a/panther_docking/README.md b/panther_docking/README.md index 0e3059cd0..54d16899c 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -51,5 +51,5 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht - `panther_charging_dock.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. - `panther_charging_dock.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. - `panther_charging_dock.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. -- `panther_charging_dock.use_wibotic_info` [*bool*, default: **True**]: Use readings from `wibotic_info` topics to ensure that a robot is charging. +- `panther_charging_dock.use_wibotic_info` [*bool*, default: **True**]: Whether Wibotic information is used. - `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive wibotic_info. diff --git a/panther_docking/package.xml b/panther_docking/package.xml index eb9ece005..11351ff4d 100644 --- a/panther_docking/package.xml +++ b/panther_docking/package.xml @@ -28,8 +28,6 @@ tf2_ros wibotic_msgs - - nav2_lifecycle_manager python3-imageio wibotic_connector_can From f2af2f8a3a7a41c06740beec2f23f195a7e79bd6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 19 Nov 2024 08:45:54 +0000 Subject: [PATCH 20/21] Update docs Signed-off-by: Jakub Delicat --- .../include/panther_docking/panther_charging_dock.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 2897fda4c..8e0648b61 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -153,9 +153,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock void updateAndPublishStagingPose(); /** - * @brief Set the dock pose. - * - * This method sets the dock pose. It can be used as a callback for a subscription. + * @brief Dock pose callback, used for external detection. * * @param pose The dock pose. */ From 8f828374a9146f7eb1570cd24b8314d357a413e6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 19 Nov 2024 10:19:23 +0000 Subject: [PATCH 21/21] Update .repose Signed-off-by: Jakub Delicat --- panther/panther_hardware.repos | 2 +- panther/panther_simulation.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/panther/panther_hardware.repos b/panther/panther_hardware.repos index 43d0fa813..87fd010dc 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: fa3a4a9a09d793b16ed23cbeb09cf4c4a892b9e3 + version: 0813c3eebec410c2635b1db3ab87b094c38658c6 ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/ diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index f6939ad09..50c918d6d 100644 --- a/panther/panther_simulation.repos +++ b/panther/panther_simulation.repos @@ -14,7 +14,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: fa3a4a9a09d793b16ed23cbeb09cf4c4a892b9e3 + version: 0813c3eebec410c2635b1db3ab87b094c38658c6 ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity type: git url: https://github.com/husarion/ros2_controllers/