diff --git a/husarion_ugv_docking/README.md b/husarion_ugv_docking/README.md deleted file mode 100644 index b706d1553..000000000 --- a/husarion_ugv_docking/README.md +++ /dev/null @@ -1,59 +0,0 @@ -# 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/husarion_ugv_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/husarion_ugv_docking/CMakeLists.txt b/panther_docking/CMakeLists.txt similarity index 62% rename from husarion_ugv_docking/CMakeLists.txt rename to panther_docking/CMakeLists.txt index c3935a221..243718fb4 100644 --- a/husarion_ugv_docking/CMakeLists.txt +++ b/panther_docking/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(husarion_ugv_docking) +project(panther_docking) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -36,27 +36,29 @@ include_directories(include) pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml) -add_library(charging_dock SHARED src/charging_dock.cpp) -ament_target_dependencies(charging_dock ${PACKAGE_DEPENDENCIES}) +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(charging_dock /opt/ros/humble/lib/libpose_filter.so) +target_link_libraries(panther_charging_dock + /opt/ros/humble/lib/libpose_filter.so) target_include_directories( - charging_dock PUBLIC $ - $) + panther_charging_dock PUBLIC $ + $) -install(TARGETS charging_dock LIBRARY DESTINATION lib) +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_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 + 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() @@ -68,6 +70,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(charging_dock) +ament_export_libraries(panther_charging_dock) ament_package() diff --git a/panther_docking/README.md b/panther_docking/README.md new file mode 100644 index 000000000..2aa1b668f --- /dev/null +++ b/panther_docking/README.md @@ -0,0 +1,59 @@ +# 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/husarion_ugv_docking/config/apriltag.yaml b/panther_docking/config/apriltag.yaml similarity index 100% rename from husarion_ugv_docking/config/apriltag.yaml rename to panther_docking/config/apriltag.yaml diff --git a/husarion_ugv_docking/config/docking_server.yaml b/panther_docking/config/panther_docking_server.yaml similarity index 87% rename from husarion_ugv_docking/config/docking_server.yaml rename to panther_docking/config/panther_docking_server.yaml index d19260b89..5f295466a 100644 --- a/husarion_ugv_docking/config/docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -13,9 +13,9 @@ dock_prestaging_tolerance: 0.5 - dock_plugins: ["charging_dock"] - charging_dock: - plugin: husarion_ugv_docking::ChargingDock + dock_plugins: ["panther_charging_dock"] + panther_charging_dock: + plugin: panther_docking::PantherChargingDock external_detection_timeout: 0.2 docking_distance_threshold: 0.04 docking_yaw_threshold: 0.2 @@ -26,14 +26,14 @@ docks: ["main", "backup"] main: - type: charging_dock + type: panther_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: charging_dock + type: panther_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/husarion_ugv_docking/include/husarion_ugv_docking/dock_pose_publisher_node.hpp b/panther_docking/include/panther_docking/dock_pose_publisher_node.hpp similarity index 87% rename from husarion_ugv_docking/include/husarion_ugv_docking/dock_pose_publisher_node.hpp rename to panther_docking/include/panther_docking/dock_pose_publisher_node.hpp index bd133a9ed..6e7c579e2 100644 --- a/husarion_ugv_docking/include/husarion_ugv_docking/dock_pose_publisher_node.hpp +++ b/panther_docking/include/panther_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 HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ -#define HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ +#ifndef PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ +#define PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ #include #include @@ -26,7 +26,7 @@ #include #include -namespace husarion_ugv_docking +namespace panther_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 husarion_ugv_docking +} // namespace panther_docking -#endif // HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ +#endif // PANTHER_DOCKING_DOCK_POSE_PUBLISHER_NODE_HPP_ diff --git a/husarion_ugv_docking/include/husarion_ugv_docking/charging_dock.hpp b/panther_docking/include/panther_docking/panther_charging_dock.hpp similarity index 92% rename from husarion_ugv_docking/include/husarion_ugv_docking/charging_dock.hpp rename to panther_docking/include/panther_docking/panther_charging_dock.hpp index 194448ab0..c58ed1845 100644 --- a/husarion_ugv_docking/include/husarion_ugv_docking/charging_dock.hpp +++ b/panther_docking/include/panther_docking/panther_charging_dock.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_CHARGING_DOCK_HPP_ -#define HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_CHARGING_DOCK_HPP_ +#ifndef PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ +#define PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ #include #include @@ -36,20 +36,20 @@ #include "wibotic_msgs/msg/wibotic_info.hpp" -namespace husarion_ugv_docking +namespace panther_docking { constexpr double kWiboticChargingCurrentThreshold = 0.0; /** - * @class ChargingDock + * @class PantherChargingDock * @brief A class to represent a Panther charging dock. */ -class ChargingDock : public opennav_docking_core::ChargingDock +class PantherChargingDock : 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 ChargingDock : public opennav_docking_core::ChargingDock std::string fixed_frame_name_; std::string dock_frame_; - rclcpp::Logger logger_{rclcpp::get_logger("ChargingDock")}; + rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")}; rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; rclcpp_lifecycle::LifecycleNode::WeakPtr node_; @@ -219,6 +219,6 @@ class ChargingDock : public opennav_docking_core::ChargingDock double wibotic_info_timeout_; }; -} // namespace husarion_ugv_docking +} // namespace panther_docking -#endif // HUSARION_UGV_DOCKING_HUSARION_UGV_DOCKING_CHARGING_DOCK_HPP_ +#endif // PANTHER_DOCKING_PANTHER_CHARGING_DOCK_HPP_ diff --git a/husarion_ugv_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py similarity index 95% rename from husarion_ugv_docking/launch/docking.launch.py rename to panther_docking/launch/docking.launch.py index 45e60bbcf..f6c4b730a 100644 --- a/husarion_ugv_docking/launch/docking.launch.py +++ b/panther_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("husarion_ugv_docking"), "config", "docking_server.yaml"] + [FindPackageShare("panther_docking"), "config", "panther_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("husarion_ugv_docking"), "config", "apriltag.yaml"] + [FindPackageShare("panther_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="husarion_ugv_docking", + package="panther_docking", executable="dock_pose_publisher", parameters=[ namespaced_docking_server_config, @@ -139,7 +139,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("husarion_ugv_docking"), + FindPackageShare("panther_docking"), "launch", "station.launch.py", ] diff --git a/husarion_ugv_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py similarity index 100% rename from husarion_ugv_docking/launch/station.launch.py rename to panther_docking/launch/station.launch.py diff --git a/husarion_ugv_docking/package.xml b/panther_docking/package.xml similarity index 94% rename from husarion_ugv_docking/package.xml rename to panther_docking/package.xml index c90df49d6..2d68586dc 100644 --- a/husarion_ugv_docking/package.xml +++ b/panther_docking/package.xml @@ -1,7 +1,7 @@ - husarion_ugv_docking + panther_docking 2.1.0 Integration Panther with Wibotic charger and Opennav Docking framework @@ -41,6 +41,6 @@ ament_cmake - + diff --git a/husarion_ugv_docking/plugin.xml b/panther_docking/plugin.xml similarity index 59% rename from husarion_ugv_docking/plugin.xml rename to panther_docking/plugin.xml index 092cfbe92..91823ea7b 100644 --- a/husarion_ugv_docking/plugin.xml +++ b/panther_docking/plugin.xml @@ -1,6 +1,6 @@ - - + A dock plugin interface for Panther robot diff --git a/husarion_ugv_docking/src/dock_pose_publisher_main.cpp b/panther_docking/src/dock_pose_publisher_main.cpp similarity index 88% rename from husarion_ugv_docking/src/dock_pose_publisher_main.cpp rename to panther_docking/src/dock_pose_publisher_main.cpp index c5757b3af..1a1f58557 100644 --- a/husarion_ugv_docking/src/dock_pose_publisher_main.cpp +++ b/panther_docking/src/dock_pose_publisher_main.cpp @@ -18,14 +18,14 @@ #include -#include "husarion_ugv_docking/dock_pose_publisher_node.hpp" +#include "panther_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/husarion_ugv_docking/src/dock_pose_publisher_node.cpp b/panther_docking/src/dock_pose_publisher_node.cpp similarity index 93% rename from husarion_ugv_docking/src/dock_pose_publisher_node.cpp rename to panther_docking/src/dock_pose_publisher_node.cpp index 26a46704f..c4558c6de 100644 --- a/husarion_ugv_docking/src/dock_pose_publisher_node.cpp +++ b/panther_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 "husarion_ugv_docking/dock_pose_publisher_node.hpp" +#include "panther_docking/dock_pose_publisher_node.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace husarion_ugv_docking +namespace panther_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("charging_dock.external_detection_timeout", 0.1); + declare_parameter("panther_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("charging_dock.external_detection_timeout").as_double() * 0.1; + timeout_ = get_parameter("panther_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", "charging_dock"); + declare_parameter(dock + ".type", "panther_charging_dock"); declare_parameter(dock + ".dock_frame", "main_wibotic_transmitter_link"); const auto dock_type = get_parameter(dock + ".type").as_string(); - if (dock_type == "charging_dock") { + if (dock_type == "panther_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 husarion_ugv_docking +} // namespace panther_docking diff --git a/husarion_ugv_docking/src/charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp similarity index 84% rename from husarion_ugv_docking/src/charging_dock.cpp rename to panther_docking/src/panther_charging_dock.cpp index 0f46fcd72..b0eb1df51 100644 --- a/husarion_ugv_docking/src/charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "husarion_ugv_docking/charging_dock.hpp" +#include "panther_docking/panther_charging_dock.hpp" #include @@ -21,17 +21,17 @@ #include "panther_utils/common_utilities.hpp" #include "panther_utils/tf2_utils.hpp" -namespace husarion_ugv_docking +namespace panther_docking { -void ChargingDock::configure( +void PantherChargingDock::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf) { name_ = name; if (!tf) { - throw std::runtime_error("ChargingDock requires a TF buffer"); + throw std::runtime_error("PantherChargingDock requires a TF buffer"); } tf2_buffer_ = tf; @@ -53,17 +53,18 @@ void ChargingDock::configure( pose_filter_coef_, external_detection_timeout_); } -void ChargingDock::cleanup() +void PantherChargingDock::cleanup() { dock_pose_sub_.reset(); staging_pose_pub_.reset(); } -void ChargingDock::activate() +void PantherChargingDock::activate() { auto node = node_.lock(); dock_pose_sub_ = node->create_subscription( - "docking/dock_pose", 1, std::bind(&ChargingDock::setDockPose, this, std::placeholders::_1)); + "docking/dock_pose", 1, + std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1)); staging_pose_pub_ = node->create_publisher("docking/staging_pose", 1); dock_pose_publisher_change_state_client_ = @@ -71,20 +72,21 @@ void ChargingDock::activate() if (use_wibotic_info_) { wibotic_info_sub_ = node->create_subscription( - "wibotic_info", 1, std::bind(&ChargingDock::setWiboticInfo, this, std::placeholders::_1)); + "wibotic_info", 1, + std::bind(&PantherChargingDock::setWiboticInfo, this, std::placeholders::_1)); } setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); } -void ChargingDock::deactivate() +void PantherChargingDock::deactivate() { dock_pose_sub_.reset(); staging_pose_pub_.reset(); dock_pose_publisher_change_state_client_.reset(); } -void ChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) +void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { nav2_util::declare_parameter_if_not_declared( node, "base_frame", rclcpp::ParameterValue("base_link")); @@ -112,7 +114,7 @@ void ChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::Shar node, name_ + ".wibotic_info_timeout", rclcpp::ParameterValue(1.5)); } -void ChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) +void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) { node->get_parameter("base_frame", base_frame_name_); node->get_parameter("fixed_frame", fixed_frame_name_); @@ -129,7 +131,7 @@ void ChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPt } // When there is no pose actual position of robot is a staging pose -ChargingDock::PoseStampedMsg ChargingDock::getStagingPose( +PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame) { RCLCPP_DEBUG_STREAM(logger_, "Getting staging pose in frame: " << frame); @@ -145,7 +147,7 @@ ChargingDock::PoseStampedMsg ChargingDock::getStagingPose( return staging_pose_; } -bool ChargingDock::getRefinedPose(PoseStampedMsg & pose) +bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) { RCLCPP_DEBUG(logger_, "Getting refined pose"); setDockPosePublisherState(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); @@ -178,7 +180,7 @@ bool ChargingDock::getRefinedPose(PoseStampedMsg & pose) return true; } -bool ChargingDock::isDocked() +bool PantherChargingDock::isDocked() { RCLCPP_DEBUG(logger_, "Checking if docked"); geometry_msgs::msg::PoseStamped robot_pose; @@ -190,7 +192,7 @@ bool ChargingDock::isDocked() robot_pose, dock_pose_, docking_distance_threshold_, docking_yaw_threshold_); } -bool ChargingDock::isCharging() +bool PantherChargingDock::isCharging() { RCLCPP_DEBUG(logger_, "Checking if charging"); try { @@ -218,17 +220,17 @@ bool ChargingDock::isCharging() return false; } -bool ChargingDock::disableCharging() { return true; } +bool PantherChargingDock::disableCharging() { return true; } -bool ChargingDock::hasStoppedCharging() { return !isCharging(); } +bool PantherChargingDock::hasStoppedCharging() { return !isCharging(); } -void ChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) +void PantherChargingDock::setDockPose(const PoseStampedMsg::SharedPtr pose) { auto filtered_pose = pose_filter_->update(*pose); dock_pose_ = filtered_pose; } -void ChargingDock::updateAndPublishStagingPose(const std::string & frame) +void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame) { const double yaw = tf2::getYaw(dock_pose_.pose.orientation); RCLCPP_DEBUG_STREAM( @@ -249,12 +251,12 @@ void ChargingDock::updateAndPublishStagingPose(const std::string & frame) staging_pose_pub_->publish(staging_pose_); } -void ChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg) +void PantherChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg) { wibotic_info_ = std::make_shared(*msg); } -void ChargingDock::setDockPosePublisherState(std::uint8_t state) +void PantherChargingDock::setDockPosePublisherState(std::uint8_t state) { if (dock_pose_publisher_state_ == state) { return; @@ -268,7 +270,7 @@ void ChargingDock::setDockPosePublisherState(std::uint8_t state) dock_pose_publisher_change_state_client_->async_send_request(request); } -bool ChargingDock::IsWiboticInfoTimeout() +bool PantherChargingDock::IsWiboticInfoTimeout() { if (!wibotic_info_) { RCLCPP_ERROR_STREAM( @@ -292,7 +294,7 @@ bool ChargingDock::IsWiboticInfoTimeout() return false; } -} // namespace husarion_ugv_docking +} // namespace panther_docking #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(husarion_ugv_docking::ChargingDock, opennav_docking_core::ChargingDock) +PLUGINLIB_EXPORT_CLASS(panther_docking::PantherChargingDock, opennav_docking_core::ChargingDock) diff --git a/husarion_ugv_docking/test/unit/test_charging_dock.cpp b/panther_docking/test/unit/test_panther_charging_dock.cpp similarity index 85% rename from husarion_ugv_docking/test/unit/test_charging_dock.cpp rename to panther_docking/test/unit/test_panther_charging_dock.cpp index 8cce464f1..4d3711e05 100644 --- a/husarion_ugv_docking/test/unit/test_charging_dock.cpp +++ b/panther_docking/test/unit/test_panther_charging_dock.cpp @@ -22,29 +22,29 @@ #include #include -#include +#include static constexpr char kBaseFrame[] = "base_link"; static constexpr char kOdomFrame[] = "odom"; -class ChargingDockWrapper : public husarion_ugv_docking::ChargingDock +class PantherChargingDockWrapper : public panther_docking::PantherChargingDock { public: void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr msg) { - husarion_ugv_docking::ChargingDock::setDockPose(msg); + panther_docking::PantherChargingDock::setDockPose(msg); } void setWiboticInfo(wibotic_msgs::msg::WiboticInfo::SharedPtr msg) { - husarion_ugv_docking::ChargingDock::setWiboticInfo(msg); + panther_docking::PantherChargingDock::setWiboticInfo(msg); } }; -class TestChargingDock : public ::testing::Test +class TestPantherChargingDock : public ::testing::Test { protected: - TestChargingDock(); + 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); @@ -52,12 +52,12 @@ class TestChargingDock : 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_; }; -TestChargingDock::TestChargingDock() +TestPantherChargingDock::TestPantherChargingDock() { node_ = std::make_shared("test_node"); tf_buffer_ = std::make_shared(node_->get_clock()); @@ -65,14 +65,14 @@ TestChargingDock::TestChargingDock() // 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 TestChargingDock::SetTransform( +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) { @@ -85,7 +85,7 @@ void TestChargingDock::SetTransform( tf_buffer_->setTransform(transform_stamped, "unittest", true); } -void TestChargingDock::ActivateWiboticInfo() +void TestPantherChargingDock::ActivateWiboticInfo() { node_->declare_parameter("dock.use_wibotic_info", true); node_->declare_parameter("dock.wibotic_info_timeout", 1.0); @@ -93,19 +93,19 @@ void TestChargingDock::ActivateWiboticInfo() dock_->activate(); } -TEST_F(TestChargingDock, FailConfigureNoNode) +TEST_F(TestPantherChargingDock, FailConfigureNoNode) { node_.reset(); ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); } -TEST_F(TestChargingDock, FailConfigureNoTfBuffer) +TEST_F(TestPantherChargingDock, FailConfigureNoTfBuffer) { tf_buffer_.reset(); ASSERT_THROW({ dock_->configure(node_, "dock", tf_buffer_); }, std::runtime_error); } -TEST_F(TestChargingDock, GetStagingPoseLocal) +TEST_F(TestPantherChargingDock, GetStagingPoseLocal) { dock_->configure(node_, "dock", tf_buffer_); dock_->activate(); @@ -128,10 +128,10 @@ TEST_F(TestChargingDock, GetStagingPoseLocal) } // TODO: @delihus fill after nav2 tests -// TEST_F(TestChargingDock, GetStagingPoseGlobal){ +// TEST_F(TestPantherChargingDock, GetStagingPoseGlobal){ // } -TEST_F(TestChargingDock, GetRefinedPose) +TEST_F(TestPantherChargingDock, GetRefinedPose) { node_->declare_parameter("dock.external_detection_timeout", 0.5); dock_->configure(node_, "dock", tf_buffer_); @@ -163,7 +163,7 @@ TEST_F(TestChargingDock, GetRefinedPose) ASSERT_FLOAT_EQ(pose.pose.position.z, 0.0); } -TEST_F(TestChargingDock, IsDocked) +TEST_F(TestPantherChargingDock, IsDocked) { node_->declare_parameter("dock.external_detection_timeout", 0.5); dock_->configure(node_, "dock", tf_buffer_); @@ -197,13 +197,13 @@ TEST_F(TestChargingDock, IsDocked) ASSERT_TRUE(dock_->isDocked()); } -TEST_F(TestChargingDock, IsChargingNoWiboticInfo) +TEST_F(TestPantherChargingDock, IsChargingNoWiboticInfo) { ActivateWiboticInfo(); ASSERT_THROW({ dock_->isCharging(); }, opennav_docking_core::FailedToCharge); } -TEST_F(TestChargingDock, IsChargingTimeout) +TEST_F(TestPantherChargingDock, IsChargingTimeout) { ActivateWiboticInfo(); @@ -213,7 +213,7 @@ TEST_F(TestChargingDock, IsChargingTimeout) ASSERT_FALSE(dock_->isCharging()); } -TEST_F(TestChargingDock, IsChargingCurrentZero) +TEST_F(TestPantherChargingDock, IsChargingCurrentZero) { ActivateWiboticInfo(); wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = @@ -225,7 +225,7 @@ TEST_F(TestChargingDock, IsChargingCurrentZero) ASSERT_FALSE(dock_->isCharging()); } -TEST_F(TestChargingDock, IsChargingTimeoutWithGoodCurrent) +TEST_F(TestPantherChargingDock, IsChargingTimeoutWithGoodCurrent) { ActivateWiboticInfo(); wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = @@ -236,7 +236,7 @@ TEST_F(TestChargingDock, IsChargingTimeoutWithGoodCurrent) ASSERT_FALSE(dock_->isCharging()); } -TEST_F(TestChargingDock, IsChargingGoodCurrentWithoutTimeout) +TEST_F(TestPantherChargingDock, IsChargingGoodCurrentWithoutTimeout) { ActivateWiboticInfo(); wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info = diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index 6613d8842..aa55478a1 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("husarion_ugv_docking"), + FindPackageShare("panther_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 32c7ff1fb..2d5720d3e 100644 --- a/panther_gazebo/launch/spawn_charging_docks.launch.py +++ b/panther_gazebo/launch/spawn_charging_docks.launch.py @@ -99,11 +99,7 @@ def generate_launch_description(): declare_docking_server_config_path_arg = DeclareLaunchArgument( "docking_server_config_path", default_value=PathJoinSubstitution( - [ - FindPackageShare("husarion_ugv_docking"), - "config", - "husarion_ugv_docking_server.yaml", - ] + [FindPackageShare("panther_docking"), "config", "panther_docking_server.yaml"] ), description=("Path to docking server configuration file."), ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index 69d4673de..184e07c1c 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 e345f35d4..042e221b0 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"/>