ROS2 Docking - panther_docking to husarion_ugv_docking

panther_docking/CMakeLists.txt → 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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
-                               $<INSTALL_INTERFACE:include>)
+  charging_dock PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+                       $<INSTALL_INTERFACE:include>)
 
-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()

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.
+- `<dock_name>.type` [*string*, default: **charging_dock**]: It checks if this dock with name `dock_name` is a type of  `charging_dock`.
+- `<dock_name>.frame` [*string*, default: **main_wibotic_transmitter_link** ]: Then look for transformation between `fixed_frame` and `<dock_name>.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.
+- `<dock_name>.apriltag_id` [*int*, default: **0**]: AprilTag ID of a dock.
+- `<dock_name>.dock_frame` [*string*, default: **main_wibotic_transmitter_link**]: A frame id to compare with fixed frame if docked.
+- `<dock_name>.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.

panther_docking/config/panther_docking_server.yaml → 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: <robot_namespace>/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: <robot_namespace>/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.

panther_docking/include/panther_docking/panther_charging_dock.hpp → 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 <memory>
 #include <string>
@@ -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<PantherChargingDock>;
-  using UniquePtr = std::unique_ptr<PantherChargingDock>;
+  using SharedPtr = std::shared_ptr<ChargingDock>;
+  using UniquePtr = std::unique_ptr<ChargingDock>;
   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_

panther_docking/include/panther_docking/dock_pose_publisher_node.hpp → 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 <memory>
 #include <string>
@@ -26,7 +26,7 @@
 #include <rclcpp_lifecycle/state.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-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<tf2_ros::Buffer> 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_

panther_docking/launch/docking.launch.py → 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",
                 ]

panther_docking/package.xml → husarion_ugv_docking/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>panther_docking</name>
+  <name>husarion_ugv_docking</name>
   <version>2.1.0</version>
   <description>Integration Panther with Wibotic charger and Opennav Docking framework</description>
 
@@ -41,6 +41,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
-    <panther_docking plugin="${prefix}/plugin.xml"/>
+    <husarion_ugv_docking plugin="${prefix}/plugin.xml"/>
   </export>
 </package>

panther_docking/plugin.xml → husarion_ugv_docking/plugin.xml

@@ -1,6 +1,6 @@
 <class_libraries>
-    <library path="panther_charging_dock">
-        <class name="panther_docking::PantherChargingDock" type="panther_docking::PantherChargingDock"
+    <library path="charging_dock">
+        <class name="husarion_ugv_docking::ChargingDock" type="husarion_ugv_docking::ChargingDock"
             base_class_type="opennav_docking_core::ChargingDock">
             <description>A dock plugin interface for Panther robot</description>
         </class>