diff --git a/panther_docking/CMakeLists.txt b/husarion_ugv_docking/CMakeLists.txt similarity index 62% rename from panther_docking/CMakeLists.txt rename to husarion_ugv_docking/CMakeLists.txt index 243718fb..c3935a22 100644 --- a/panther_docking/CMakeLists.txt +++ b/husarion_ugv_docking/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(panther_docking) +project(husarion_ugv_docking) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -36,29 +36,27 @@ include_directories(include) 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}) +add_library(charging_dock SHARED src/charging_dock.cpp) +ament_target_dependencies(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_link_libraries(charging_dock /opt/ros/humble/lib/libpose_filter.so) target_include_directories( - panther_charging_dock PUBLIC $ - $) + charging_dock PUBLIC $ + $) -install(TARGETS panther_charging_dock LIBRARY DESTINATION lib) +install(TARGETS 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/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 + ament_add_gtest(${PROJECT_NAME}_test_charging_dock + test/unit/test_charging_dock.cpp) + target_link_libraries(${PROJECT_NAME}_test_charging_dock charging_dock) + ament_target_dependencies(${PROJECT_NAME}_test_charging_dock ${PACKAGE_DEPENDENCIES}) endif() @@ -70,6 +68,6 @@ ament_target_dependencies(dock_pose_publisher ${PACKAGE_DEPENDENCIES}) install(TARGETS dock_pose_publisher DESTINATION lib/${PROJECT_NAME}) ament_export_include_directories(include) -ament_export_libraries(panther_charging_dock) +ament_export_libraries(charging_dock) ament_package() diff --git a/husarion_ugv_docking/README.md b/husarion_ugv_docking/README.md new file mode 100644 index 00000000..29560b66 --- /dev/null +++ b/husarion_ugv_docking/README.md @@ -0,0 +1,59 @@ +# husarion_ugv_docking + +The package contains a `ChargingDock` plugin for the [opennav_docking](https://github.com/open-navigation/opennav_docking) project. Thanks to this package, Panther can dock to a charging station. + +## Launch Files + +- `docking.launch.py`: Launch a node that creates `docking_server` and runs a `ChargingDock` plugin. Also it launches `station.launch.py`. +- `station.launch.py`: Launch a node that creates a charging station description with generated apriltag. + +## Configuration Files + +- [`husarion_ugv_docking_server.yaml`](./config/docking_server.yaml): Defines parameters for a `docking_server` and a `ChargingDock` plugin. Defines poses where charging docks are spawned in the Gazebo. + + +## ROS Nodes + +- `DockPosePublisherNode`: A lifecycle node listens to `tf` and republishes position of `dock_pose` in the fixed frame when it is activated. +- `ChargingDock`: A plugin for a Panther robot what is responsible for a charger service. + +### DockPosePublisherNode + +#### Publishes + +- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. + +#### Subscribers + +- `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: **charging_dock**]: It checks if this dock with name `dock_name` is a type of `charging_dock`. +- `.frame` [*string*, default: **main_wibotic_transmitter_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose`. A frame id of a wireless transmitter. + +### ChargingDock + +#### Publishes + +- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station. + +#### Subscribers + +- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. + +#### Parameters + +- `base_frame` [*string*, default: **base_link**]: A frame id of a wireless receiver. +- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. +- `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. +- `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. +- `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. +- `charging_dock.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X. +- `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. +- `charging_dock.use_wibotic_info` [*bool*, default: **True**]: Whether Wibotic information is used. +- `charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive a wibotic_info. +- `.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock. +- `.dock_frame` [*string*, default: **main_wibotic_transmitter_link**]: A frame id to compare with fixed frame if docked. +- `.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used a dock is spawned in this pose. diff --git a/panther_docking/config/apriltag.yaml b/husarion_ugv_docking/config/apriltag.yaml similarity index 100% rename from panther_docking/config/apriltag.yaml rename to husarion_ugv_docking/config/apriltag.yaml diff --git a/panther_docking/config/panther_docking_server.yaml b/husarion_ugv_docking/config/docking_server.yaml similarity index 87% rename from panther_docking/config/panther_docking_server.yaml rename to husarion_ugv_docking/config/docking_server.yaml index 5f295466..d19260b8 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/husarion_ugv_docking/config/docking_server.yaml @@ -13,9 +13,9 @@ dock_prestaging_tolerance: 0.5 - dock_plugins: ["panther_charging_dock"] - panther_charging_dock: - plugin: panther_docking::PantherChargingDock + dock_plugins: ["charging_dock"] + charging_dock: + plugin: husarion_ugv_docking::ChargingDock external_detection_timeout: 0.2 docking_distance_threshold: 0.04 docking_yaw_threshold: 0.2 @@ -26,14 +26,14 @@ docks: ["main", "backup"] main: - type: panther_charging_dock + type: charging_dock frame: /map dock_frame: main_wibotic_transmitter_link pose: [1.0, 1.20, 1.57] # [x, y, yaw] of the dock on the map. Used also for spawning dock in the simulation. apriltag_id: 0 backup: - type: panther_charging_dock + type: charging_dock frame: /map dock_frame: backup_wibotic_transmitter_link pose: [-1.0, 1.20, 1.57] # [x, y, yaw] of the dock on the map. Used also for spawning dock in the simulation. diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/husarion_ugv_docking/include/husarion_ugv_docking/charging_dock.hpp similarity index 92% rename from panther_docking/include/panther_docking/panther_charging_dock.hpp rename to husarion_ugv_docking/include/husarion_ugv_docking/charging_dock.hpp index c58ed184..194448ab 100644 --- a/panther_docking/include/panther_docking/panther_charging_dock.hpp +++ b/husarion_ugv_docking/include/husarion_ugv_docking/charging_dock.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ -#define PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ +#ifndef HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_CHARGING_DOCK_HPP_ +#define HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_CHARGING_DOCK_HPP_ #include #include @@ -36,20 +36,20 @@ #include "wibotic_msgs/msg/wibotic_info.hpp" -namespace panther_docking +namespace husarion_ugv_docking { constexpr double kWiboticChargingCurrentThreshold = 0.0; /** - * @class PantherChargingDock + * @class ChargingDock * @brief A class to represent a Panther charging dock. */ -class PantherChargingDock : public opennav_docking_core::ChargingDock +class ChargingDock : public opennav_docking_core::ChargingDock { public: - using SharedPtr = std::shared_ptr; - using UniquePtr = std::unique_ptr; + using SharedPtr = std::shared_ptr; + using UniquePtr = std::unique_ptr; using PoseStampedMsg = geometry_msgs::msg::PoseStamped; using WiboticInfoMsg = wibotic_msgs::msg::WiboticInfo; @@ -185,7 +185,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock std::string fixed_frame_name_; std::string dock_frame_; - rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; + rclcpp::Logger logger_{rclcpp::get_logger("ChargingDock")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; rclcpp_lifecycle::LifecycleNode::WeakPtr node_; @@ -219,6 +219,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock double wibotic_info_timeout_; }; -} // namespace panther_docking +} // namespace husarion_ugv_docking -#endif // PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ +#endif // HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_CHARGING_DOCK_HPP_ diff --git a/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp b/husarion_ugv_docking/include/husarion_ugv_docking/dock_pose_publisher_node.hpp similarity index 87% rename from panther_docking/include/panther_docking/dock_pose_publisher_node.hpp rename to husarion_ugv_docking/include/husarion_ugv_docking/dock_pose_publisher_node.hpp index 6e7c579e..bd133a9e 100644 --- a/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp +++ b/husarion_ugv_docking/include/husarion_ugv_docking/dock_pose_publisher_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ -#define PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ +#ifndef HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ +#define HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ #include #include @@ -26,7 +26,7 @@ #include #include -namespace panther_docking +namespace husarion_ugv_docking { constexpr double kMinimalDetectionDistance = 1.0; @@ -60,6 +60,6 @@ class DockPosePublisherNode : public rclcpp_lifecycle::LifecycleNode std::unique_ptr tf_buffer_; rclcpp::TimerBase::SharedPtr timer_; }; -} // namespace panther_docking +} // namespace husarion_ugv_docking -#endif // PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ +#endif // HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ diff --git a/panther_docking/launch/docking.launch.py b/husarion_ugv_docking/launch/docking.launch.py similarity index 95% rename from panther_docking/launch/docking.launch.py rename to husarion_ugv_docking/launch/docking.launch.py index f6c4b730..45e60bbc 100644 --- a/panther_docking/launch/docking.launch.py +++ b/husarion_ugv_docking/launch/docking.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): declare_docking_server_config_path_arg = DeclareLaunchArgument( "docking_server_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] + [FindPackageShare("husarion_ugv_docking"), "config", "docking_server.yaml"] ), description=("Path to docking server configuration file."), ) @@ -40,7 +40,7 @@ def generate_launch_description(): declare_apriltag_config_path_arg = DeclareLaunchArgument( "apriltag_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_docking"), "config", "apriltag.yaml"] + [FindPackageShare("husarion_ugv_docking"), "config", "apriltag.yaml"] ), description=("Path to apriltag configuration file. Only used in simulation."), ) @@ -108,7 +108,7 @@ def generate_launch_description(): ) dock_pose_publisher = Node( - package="panther_docking", + package="husarion_ugv_docking", executable="dock_pose_publisher", parameters=[ namespaced_docking_server_config, @@ -139,7 +139,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("panther_docking"), + FindPackageShare("husarion_ugv_docking"), "launch", "station.launch.py", ] diff --git a/panther_docking/launch/station.launch.py b/husarion_ugv_docking/launch/station.launch.py similarity index 100% rename from panther_docking/launch/station.launch.py rename to husarion_ugv_docking/launch/station.launch.py diff --git a/panther_docking/package.xml b/husarion_ugv_docking/package.xml similarity index 94% rename from panther_docking/package.xml rename to husarion_ugv_docking/package.xml index 2d68586d..c90df49d 100644 --- a/panther_docking/package.xml +++ b/husarion_ugv_docking/package.xml @@ -1,7 +1,7 @@ - panther_docking + husarion_ugv_docking 2.1.0 Integration Panther with Wibotic charger and Opennav Docking framework @@ -41,6 +41,6 @@ ament_cmake - + diff --git a/panther_docking/plugin.xml b/husarion_ugv_docking/plugin.xml similarity index 59% rename from panther_docking/plugin.xml rename to husarion_ugv_docking/plugin.xml index 91823ea7..092cfbe9 100644 --- a/panther_docking/plugin.xml +++ b/husarion_ugv_docking/plugin.xml @@ -1,6 +1,6 @@ - - + A dock plugin interface for Panther robot diff --git a/panther_docking/src/panther_charging_dock.cpp b/husarion_ugv_docking/src/charging_dock.cpp similarity index 84% rename from panther_docking/src/panther_charging_dock.cpp rename to husarion_ugv_docking/src/charging_dock.cpp index b0eb1df5..0f46fcd7 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/husarion_ugv_docking/src/charging_dock.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_docking/panther_charging_dock.hpp" +#include "husarion_ugv_docking/charging_dock.hpp" #include @@ -21,17 +21,17 @@ #include "panther_utils/common_utilities.hpp" #include "panther_utils/tf2_utils.hpp" -namespace panther_docking +namespace husarion_ugv_docking { -void PantherChargingDock::configure( +void ChargingDock::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf) { name_ = name; if (!tf) { - throw std::runtime_error("PantherChargingDock requires a TF buffer"); + throw std::runtime_error("ChargingDock requires a TF buffer"); } tf2_buffer_ = tf; @@ -53,18 +53,17 @@ void PantherChargingDock::configure( pose_filter_coef_, external_detection_timeout_); } -void PantherChargingDock::cleanup() +void ChargingDock::cleanup() { dock_pose_sub_.reset(); staging_pose_pub_.reset(); } -void PantherChargingDock::activate() +void ChargingDock::activate() { auto node = node_.lock(); dock_pose_sub_ = node->create_subscription( - "docking/dock_pose", 1, - std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1)); + "docking/dock_pose", 1, std::bind(&ChargingDock::setDockPose, this, std::placeholders::_1)); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); dock_pose_publisher_change_state_client_ = @@ -72,21 +71,20 @@ void PantherChargingDock::activate() if (use_wibotic_info_) { wibotic_info_sub_ = node->create_subscription( - "wibotic_info", 1, - std::bind(&PantherChargingDock::setWiboticInfo, this, std::placeholders::_1)); + "wibotic_info", 1, std::bind(&ChargingDock::setWiboticInfo, this, std::placeholders::_1)); } setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); } -void PantherChargingDock::deactivate() +void ChargingDock::deactivate() { dock_pose_sub_.reset(); staging_pose_pub_.reset(); dock_pose_publisher_change_state_client_.reset(); } -void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) +void ChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { nav2_util::declare_parameter_if_not_declared( node, "base_frame", rclcpp::ParameterValue("base_link")); @@ -114,7 +112,7 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod node, name_ + ".wibotic_info_timeout", rclcpp::ParameterValue(1.5)); } -void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) +void ChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { node->get_parameter("base_frame", base_frame_name_); node->get_parameter("fixed_frame", fixed_frame_name_); @@ -131,7 +129,7 @@ void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::S } // When there is no pose actual position of robot is a staging pose -PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( +ChargingDock::PoseStampedMsg ChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { RCLCPP_DEBUG_STREAM(logger_, "Getting staging pose in frame: " << frame); @@ -147,7 +145,7 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( return staging_pose_; } -bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) +bool ChargingDock::getRefinedPose(PoseStampedMsg & pose) { RCLCPP_DEBUG(logger_, "Getting refined pose"); setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); @@ -180,7 +178,7 @@ bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) return true; } -bool PantherChargingDock::isDocked() +bool ChargingDock::isDocked() { RCLCPP_DEBUG(logger_, "Checking if docked"); geometry_msgs::msg::PoseStamped robot_pose; @@ -192,7 +190,7 @@ bool PantherChargingDock::isDocked() robot_pose, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); } -bool PantherChargingDock::isCharging() +bool ChargingDock::isCharging() { RCLCPP_DEBUG(logger_, "Checking if charging"); try { @@ -220,17 +218,17 @@ bool PantherChargingDock::isCharging() return false; } -bool PantherChargingDock::disableCharging() { return true; } +bool ChargingDock::disableCharging() { return true; } -bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } +bool ChargingDock::hasStoppedCharging() { return !isCharging(); } -void PantherChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) +void ChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) { auto filtered_pose = pose_filter_->update(*pose); dock_pose_ = filtered_pose; } -void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame) +void ChargingDock::updateAndPublishStagingPose(const std::string & frame) { const double yaw = tf2::getYaw(dock_pose_.pose.orientation); RCLCPP_DEBUG_STREAM( @@ -251,12 +249,12 @@ void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame) staging_pose_pub_->publish(staging_pose_); } -void PantherChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg) +void ChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg) { wibotic_info_ = std::make_shared(*msg); } -void PantherChargingDock::setDockPosePublisherState(std::uint8_t state) +void ChargingDock::setDockPosePublisherState(std::uint8_t state) { if (dock_pose_publisher_state_ == state) { return; @@ -270,7 +268,7 @@ void PantherChargingDock::setDockPosePublisherState(std::uint8_t state) dock_pose_publisher_change_state_client_->async_send_request(request); } -bool PantherChargingDock::IsWiboticInfoTimeout() +bool ChargingDock::IsWiboticInfoTimeout() { if (!wibotic_info_) { RCLCPP_ERROR_STREAM( @@ -294,7 +292,7 @@ bool PantherChargingDock::IsWiboticInfoTimeout() return false; } -} // namespace panther_docking +} // namespace husarion_ugv_docking #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(panther_docking::PantherChargingDock, opennav_docking_core::ChargingDock) +PLUGINLIB_EXPORT_CLASS(husarion_ugv_docking::ChargingDock, opennav_docking_core::ChargingDock) diff --git a/panther_docking/src/dock_pose_publisher_main.cpp b/husarion_ugv_docking/src/dock_pose_publisher_main.cpp similarity index 88% rename from panther_docking/src/dock_pose_publisher_main.cpp rename to husarion_ugv_docking/src/dock_pose_publisher_main.cpp index 1a1f5855..c5757b3a 100644 --- a/panther_docking/src/dock_pose_publisher_main.cpp +++ b/husarion_ugv_docking/src/dock_pose_publisher_main.cpp @@ -18,14 +18,14 @@ #include -#include "panther_docking/dock_pose_publisher_node.hpp" +#include "husarion_ugv_docking/dock_pose_publisher_node.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto dock_pose_publisher_node = - std::make_shared("dock_pose_publisher_node"); + std::make_shared("dock_pose_publisher_node"); try { rclcpp::spin(dock_pose_publisher_node->get_node_base_interface()); diff --git a/panther_docking/src/dock_pose_publisher_node.cpp b/husarion_ugv_docking/src/dock_pose_publisher_node.cpp similarity index 93% rename from panther_docking/src/dock_pose_publisher_node.cpp rename to husarion_ugv_docking/src/dock_pose_publisher_node.cpp index c4558c6d..26a46704 100644 --- a/panther_docking/src/dock_pose_publisher_node.cpp +++ b/husarion_ugv_docking/src/dock_pose_publisher_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "panther_docking/dock_pose_publisher_node.hpp" +#include "husarion_ugv_docking/dock_pose_publisher_node.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace panther_docking +namespace husarion_ugv_docking { DockPosePublisherNode::DockPosePublisherNode(const std::string & name) : rclcpp_lifecycle::LifecycleNode(name) @@ -37,22 +37,22 @@ DockPosePublisherNode::on_configure(const rclcpp_lifecycle::State &) declare_parameter("docks", std::vector({"main"})); declare_parameter("fixed_frame", "odom"); declare_parameter("base_frame", "base_link"); - declare_parameter("panther_charging_dock.external_detection_timeout", 0.1); + declare_parameter("charging_dock.external_detection_timeout", 0.1); const auto fixed_frame = get_parameter("fixed_frame").as_string(); const auto docks = get_parameter("docks").as_string_array(); const auto publish_rate = get_parameter("publish_rate").as_double(); publish_period_ = std::chrono::duration(1.0 / publish_rate); - timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1; + timeout_ = get_parameter("charging_dock.external_detection_timeout").as_double() * 0.1; base_frame_ = get_parameter("base_frame").as_string(); for (const auto & dock : docks) { - declare_parameter(dock + ".type", "panther_charging_dock"); + declare_parameter(dock + ".type", "charging_dock"); declare_parameter(dock + ".dock_frame", "main_wibotic_transmitter_link"); const auto dock_type = get_parameter(dock + ".type").as_string(); - if (dock_type == "panther_charging_dock") { + if (dock_type == "charging_dock") { const auto dock_pose_frame_id = get_parameter(dock + ".dock_frame").as_string(); RCLCPP_INFO_STREAM( this->get_logger(), "Adding dock " << dock << " with frame " << dock_pose_frame_id); @@ -164,4 +164,4 @@ void DockPosePublisherNode::publishPose() pose_publisher_->publish(pose_msg); } -} // namespace panther_docking +} // namespace husarion_ugv_docking diff --git a/panther_docking/test/unit/test_panther_charging_dock.cpp b/husarion_ugv_docking/test/unit/test_charging_dock.cpp similarity index 85% rename from panther_docking/test/unit/test_panther_charging_dock.cpp rename to husarion_ugv_docking/test/unit/test_charging_dock.cpp index 4d3711e0..8cce464f 100644 --- a/panther_docking/test/unit/test_panther_charging_dock.cpp +++ b/husarion_ugv_docking/test/unit/test_charging_dock.cpp @@ -22,29 +22,29 @@ #include #include -#include +#include static constexpr char kBaseFrame[] = "base_link"; static constexpr char kOdomFrame[] = "odom"; -class PantherChargingDockWrapper : public panther_docking::PantherChargingDock +class ChargingDockWrapper : public husarion_ugv_docking::ChargingDock { public: void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr msg) { - panther_docking::PantherChargingDock::setDockPose(msg); + husarion_ugv_docking::ChargingDock::setDockPose(msg); } void setWiboticInfo(wibotic_msgs::msg::WiboticInfo::SharedPtr msg) { - panther_docking::PantherChargingDock::setWiboticInfo(msg); + husarion_ugv_docking::ChargingDock::setWiboticInfo(msg); } }; -class TestPantherChargingDock : public ::testing::Test +class TestChargingDock : public ::testing::Test { protected: - TestPantherChargingDock(); + TestChargingDock(); 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); @@ -52,12 +52,12 @@ class TestPantherChargingDock : public ::testing::Test void ActivateWiboticInfo(); rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - std::shared_ptr dock_; + std::shared_ptr dock_; rclcpp::Publisher::SharedPtr dock_pose_pub; tf2_ros::Buffer::SharedPtr tf_buffer_; }; -TestPantherChargingDock::TestPantherChargingDock() +TestChargingDock::TestChargingDock() { node_ = std::make_shared("test_node"); tf_buffer_ = std::make_shared(node_->get_clock()); @@ -65,14 +65,14 @@ TestPantherChargingDock::TestPantherChargingDock() // Silence error about dedicated thread's being necessary tf_buffer_->setUsingDedicatedThread(true); - dock_ = std::make_shared(); + dock_ = std::make_shared(); dock_pose_pub = node_->create_publisher("dock_pose", 10); node_->configure(); node_->activate(); } -void TestPantherChargingDock::SetTransform( +void TestChargingDock::SetTransform( const std::string & frame_id, const std::string & child_frame_id, const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform) { @@ -85,7 +85,7 @@ void TestPantherChargingDock::SetTransform( tf_buffer_->setTransform(transform_stamped, "unittest", true); } -void TestPantherChargingDock::ActivateWiboticInfo() +void TestChargingDock::ActivateWiboticInfo() { node_->declare_parameter("dock.use_wibotic_info", true); node_->declare_parameter("dock.wibotic_info_timeout", 1.0); @@ -93,19 +93,19 @@ void TestPantherChargingDock::ActivateWiboticInfo() dock_->activate(); } -TEST_F(TestPantherChargingDock, FailConfigureNoNode) +TEST_F(TestChargingDock, FailConfigureNoNode) { node_.reset(); ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); } -TEST_F(TestPantherChargingDock, FailConfigureNoTfBuffer) +TEST_F(TestChargingDock, FailConfigureNoTfBuffer) { tf_buffer_.reset(); ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); } -TEST_F(TestPantherChargingDock, GetStagingPoseLocal) +TEST_F(TestChargingDock, GetStagingPoseLocal) { dock_->configure(node_, "dock", tf_buffer_); dock_->activate(); @@ -128,10 +128,10 @@ TEST_F(TestPantherChargingDock, GetStagingPoseLocal) } // TODO: @delihus fill after nav2 tests -// TEST_F(TestPantherChargingDock, GetStagingPoseGlobal){ +// TEST_F(TestChargingDock, GetStagingPoseGlobal){ // } -TEST_F(TestPantherChargingDock, GetRefinedPose) +TEST_F(TestChargingDock, GetRefinedPose) { node_->declare_parameter("dock.external_detection_timeout", 0.5); dock_->configure(node_, "dock", tf_buffer_); @@ -163,7 +163,7 @@ TEST_F(TestPantherChargingDock, GetRefinedPose) ASSERT_FLOAT_EQ(pose.pose.position.z, 0.0); } -TEST_F(TestPantherChargingDock, IsDocked) +TEST_F(TestChargingDock, IsDocked) { node_->declare_parameter("dock.external_detection_timeout", 0.5); dock_->configure(node_, "dock", tf_buffer_); @@ -197,13 +197,13 @@ TEST_F(TestPantherChargingDock, IsDocked) ASSERT_TRUE(dock_->isDocked()); } -TEST_F(TestPantherChargingDock, IsChargingNoWiboticInfo) +TEST_F(TestChargingDock, IsChargingNoWiboticInfo) { ActivateWiboticInfo(); ASSERT_THROW({ dock_->isCharging(); }, opennav_docking_core::FailedToCharge); } -TEST_F(TestPantherChargingDock, IsChargingTimeout) +TEST_F(TestChargingDock, IsChargingTimeout) { ActivateWiboticInfo(); @@ -213,7 +213,7 @@ TEST_F(TestPantherChargingDock, IsChargingTimeout) ASSERT_FALSE(dock_->isCharging()); } -TEST_F(TestPantherChargingDock, IsChargingCurrentZero) +TEST_F(TestChargingDock, IsChargingCurrentZero) { ActivateWiboticInfo(); wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = @@ -225,7 +225,7 @@ TEST_F(TestPantherChargingDock, IsChargingCurrentZero) ASSERT_FALSE(dock_->isCharging()); } -TEST_F(TestPantherChargingDock, IsChargingTimeoutWithGoodCurrent) +TEST_F(TestChargingDock, IsChargingTimeoutWithGoodCurrent) { ActivateWiboticInfo(); wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = @@ -236,7 +236,7 @@ TEST_F(TestPantherChargingDock, IsChargingTimeoutWithGoodCurrent) ASSERT_FALSE(dock_->isCharging()); } -TEST_F(TestPantherChargingDock, IsChargingGoodCurrentWithoutTimeout) +TEST_F(TestChargingDock, IsChargingGoodCurrentWithoutTimeout) { ActivateWiboticInfo(); wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = diff --git a/panther_docking/README.md b/panther_docking/README.md deleted file mode 100644 index 2aa1b668..00000000 --- a/panther_docking/README.md +++ /dev/null @@ -1,59 +0,0 @@ -# panther_docking - -The package contains a `PantherChargingDock` plugin for the [opennav_docking](https://github.com/open-navigation/opennav_docking) project. Thanks to this package, Panther can dock to a charging station. - -## Launch Files - -- `docking.launch.py`: Launch a node that creates `docking_server` and runs a `PantherChargingDock` plugin. Also it launches `station.launch.py`. -- `station.launch.py`: Launch a node that creates a charging station description with generated apriltag. - -## Configuration Files - -- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. Defines poses where charging docks are spawned in the Gazebo. - - -## ROS Nodes - -- `DockPosePublisherNode`: A lifecycle node listens to `tf` and republishes position of `dock_pose` in the fixed frame when it is activated. -- `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. - -### DockPosePublisherNode - -#### Publishes - -- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. - -#### Subscribers - -- `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_transmitter_link** ]: Then look for transformation between `fixed_frame` and `.frame` to publish `dock_pose`. A frame id of a wireless transmitter. - -### PantherChargingDock - -#### Publishes - -- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station. - -#### Subscribers - -- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose. - -#### Parameters - -- `base_frame` [*string*, default: **base_link**]: A frame id of a wireless receiver. -- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. -- `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 a wibotic_info. -- `.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock. -- `.dock_frame` [*string*, default: **main_wibotic_transmitter_link**]: A frame id to compare with fixed frame if docked. -- `.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used a dock is spawned in this pose. diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index aa55478a..6613d884 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -189,7 +189,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("panther_docking"), + FindPackageShare("husarion_ugv_docking"), "launch", "docking.launch.py", ] diff --git a/panther_gazebo/launch/spawn_charging_docks.launch.py b/panther_gazebo/launch/spawn_charging_docks.launch.py index 2d5720d3..0c56fbde 100644 --- a/panther_gazebo/launch/spawn_charging_docks.launch.py +++ b/panther_gazebo/launch/spawn_charging_docks.launch.py @@ -99,7 +99,11 @@ def generate_launch_description(): declare_docking_server_config_path_arg = DeclareLaunchArgument( "docking_server_config_path", default_value=PathJoinSubstitution( - [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] + [ + FindPackageShare("husarion_ugv_docking"), + "config", + "docking_server.yaml", + ] ), description=("Path to docking server configuration file."), ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 184e07c1..69d4673d 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -29,12 +29,12 @@ controller_manager husarion_gz_worlds + husarion_ugv_docking launch launch_ros nav2_common panther_controller panther_description - panther_docking panther_lights panther_localization panther_manager diff --git a/panther_manager/behavior_trees/docking.xml b/panther_manager/behavior_trees/docking.xml index 042e221b..e345f35d 100644 --- a/panther_manager/behavior_trees/docking.xml +++ b/panther_manager/behavior_trees/docking.xml @@ -12,7 +12,7 @@ @@ -24,7 +24,7 @@ axes="" buttons="1;0;0;0;1;1;0;0;0;0;0;0"/>