diff --git a/README.md b/README.md index 3e7a1f2e..f64a4118 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:*** `''` | +| 🤖🖥️ | `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/panther_hardware.repos b/panther/panther_hardware.repos index 0e267fba..87fd010d 100644 --- a/panther/panther_hardware.repos +++ b/panther/panther_hardware.repos @@ -14,8 +14,12 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: b29f41ac00ab1a6fbac3e1d03602575094de277a + 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/ version: 9da42a07a83bbf3faf5cad11793fff22f25068af + wibotic_ros: + type: git + url: https://github.com/husarion/wibotic_ros/ + version: 0.1.0 diff --git a/panther/panther_simulation.repos b/panther/panther_simulation.repos index 24751fd7..50c918d6 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: 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/ @@ -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 diff --git a/panther_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt index 6295b903..7af73bfb 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 5aa076fa..54d16899 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. -- `.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. +- `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**]: 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/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index ea56b06b..5725a323 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 @@ -16,7 +16,7 @@ dock_plugins: ["panther_charging_dock"] panther_charging_dock: plugin: panther_docking::PantherChargingDock - external_detection_timeout: 0.2 + external_detection_timeout: 0.1 docking_distance_threshold: 0.12 docking_yaw_threshold: 0.2 staging_x_offset: -0.8 diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp index 4a09a881..8e0648b6 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,27 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock */ void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); - void updateAndPublishStagingPose(const std::string & frame); + /** + * @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 Dock pose callback, used for external detection. + * + * @param pose The dock pose. + */ void setDockPose(const PoseStampedMsg::SharedPtr pose); + /** + * @brief Wibotic info callback, used when `use_wibotic_info` is enabled. + * + * @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 +178,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 +195,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 19bf939b..0daf9609 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -64,9 +64,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="True", + 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( @@ -141,16 +155,29 @@ 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, + arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"], + condition=IfCondition( + PythonExpression(["not ", use_sim, " and ", use_wibotic_info, " and ", use_docking]) + ), + ) + return LaunchDescription( [ declare_apriltag_config_path_arg, - declare_use_docking_arg, declare_docking_server_config_path_arg, + declare_use_docking_arg, + declare_use_wibotic_info_arg, declare_log_level, station_launch, docking_server_node, docking_server_activate_node, dock_pose_publisher, apriltag_node, + wibotic_connector_can, ] ) diff --git a/panther_docking/package.xml b/panther_docking/package.xml index 5849076d..cd602a63 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 @@ -25,10 +26,12 @@ sensor_msgs std_srvs tf2_ros + wibotic_msgs apriltag_ros nav2_lifecycle_manager python3-imageio + wibotic_connector_can xacro ament_cmake diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index 7108efd4..f33f0922 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -45,6 +45,10 @@ 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_); } @@ -62,6 +66,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 +100,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,8 +119,12 @@ 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 PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { @@ -166,10 +186,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; } @@ -201,6 +248,11 @@ void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame) 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 435e7bdb..80a7a759 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); diff --git a/panther_gazebo/config/apriltag.yaml b/panther_gazebo/config/apriltag.yaml new file mode 100644 index 00000000..920c6f99 --- /dev/null +++ b/panther_gazebo/config/apriltag.yaml @@ -0,0 +1,23 @@ +--- +/**: + ros__parameters: + image_transport: raw + family: 36h11 + size: 0.06 + profile: false + + + max_hamming: 0 + detector: + threads: 1 + decimate: 2.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + + + tag: + ids: [0] + frames: [/main_apriltag_link] + sizes: [0.06] diff --git a/panther_gazebo/launch/apriltag.launch.py b/panther_gazebo/launch/apriltag.launch.py new file mode 100644 index 00000000..deeaa24e --- /dev/null +++ b/panther_gazebo/launch/apriltag.launch.py @@ -0,0 +1,62 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + use_docking = LaunchConfiguration("use_docking") + use_sim = LaunchConfiguration("use_sim") + namespace = LaunchConfiguration("namespace") + + apriltag_config_path = LaunchConfiguration("apriltag_config_path") + apriltag_config_path_arg = DeclareLaunchArgument( + "apriltag_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("panther_gazebo"), "config", "apriltag.yaml"] + ), + description=("Path to apriltag configuration file."), + ) + + namespaced_apriltag_config_path = ReplaceString( + source_file=apriltag_config_path, + replacements={"": namespace, "//": "/"}, + ) + + apriltag_node = Node( + package="apriltag_ros", + executable="apriltag_node", + parameters=[{"use_sim_time": use_sim}, namespaced_apriltag_config_path], + namespace=namespace, + emulate_tty=True, + remappings={ + "camera_info": "camera/color/camera_info", + "image_rect": "camera/color/image_raw", + "detections": "docking/april_tags", + }.items(), + condition=IfCondition(use_docking), + ) + + return LaunchDescription( + [ + apriltag_config_path_arg, + apriltag_node, + ] + ) diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index aa55478a..8f7fa280 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -198,6 +198,19 @@ def generate_launch_description(): launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), ) + apriltag_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("panther_gazebo"), + "launch", + "apriltag.launch.py", + ] + ), + ), + launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), + ) + return LaunchDescription( [ declare_battery_config_path_arg, @@ -214,5 +227,6 @@ def generate_launch_description(): simulate_components, gz_bridge, docking_launch, + apriltag_launch, ] ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 184e07c1..ecadf84c 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -27,6 +27,7 @@ std_msgs std_srvs + apriltag_ros controller_manager husarion_gz_worlds launch