Merge remote-tracking branch 'origin/ros2-docking' into ros2-joy-plugin

.github/workflows/pre-commit.yaml

@@ -0,0 +1,11 @@
+---
+name: Pre-Commit
+
+on:
+  push:
+
+jobs:
+  pre-commit:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
+    with:
+      ros_distro: humble

.pre-commit-config.yaml

@@ -34,7 +34,7 @@ repos:
       - id: cmake-format
 
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.6
+    rev: v18.1.8
     hooks:
       - id: clang-format
 
@@ -63,13 +63,13 @@ repos:
         args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100']
 
   - repo: https://github.com/psf/black
-    rev: 24.4.2
+    rev: 24.8.0
     hooks:
       - id: black
         args: ["--line-length=99"]
 
   - repo: https://github.com/PyCQA/flake8
-    rev: 7.1.0
+    rev: 7.1.1
     hooks:
       - id: flake8
         args:
@@ -94,7 +94,7 @@ repos:
         exclude: ^.*\/CHANGELOG\.rst/.*$
 
   - repo: https://github.com/tier4/pre-commit-hooks-ros
-    rev: v0.8.0
+    rev: v0.10.0
     hooks:
       - id: prettier-package-xml
       - id: sort-package-xml

ROS_API.md

@@ -1,12 +1,12 @@
 # ROS API
 
-> [!IMPORTANT]  
-> **Beta Release**  
->   
-> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur.  
->   
+> [!IMPORTANT]
+> **Beta Release**
+>
+> Please be advised that the software you are about to use is a Beta version of the ROS 2 Driver for Panther. It is functional, and the architecture will not change significantly. Although it has been tested by the Husarion team, some stability issues and bugs may still occur.
+>
 > We would greatly appreciate your feedback regarding the Panther ROS 2 driver. You can reach us in the following ways:
-> 
+>
 > - By email at: [[email protected]](mailto:[email protected])
 > - Via our community forum: [Husarion Community](https://community.husarion.com)
 > - By submitting an issue request on: [GitHub](https://github.com/husarion/panther_ros/issues)

panther_description/urdf/gazebo.urdf.xacro

@@ -6,12 +6,7 @@
 
     <!-- Read property -->
     <xacro:property name="battery_config" value="${xacro.load_yaml(config_file)}" />
-    <xacro:if value="${namespace == ''}">
-      <xacro:property name="ns" value="" />
-    </xacro:if>
-    <xacro:unless value="${namespace == ''}">
-        <xacro:property name="ns" value="${namespace}/" />
-    </xacro:unless>
+    <xacro:property name="ns" value='${namespace + "/" if namespace else ""}' />
 
     <!-- Sim Battery -->
     <gazebo>
@@ -68,12 +63,7 @@
   <!-- IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications -->
   <xacro:macro name="imu" params="reference_frame namespace:=''">
 
-    <xacro:if value="${namespace == ''}">
-      <xacro:property name="ns" value="" />
-    </xacro:if>
-    <xacro:unless value="${namespace == ''}">
-        <xacro:property name="ns" value="${namespace}/" />
-    </xacro:unless>
+    <xacro:property name="ns" value='${namespace + "/" if namespace else ""}' />
 
     <gazebo reference="${reference_frame}">
       <sensor name="${ns}imu" type="imu">

panther_description/urdf/panther_macro.urdf.xacro

@@ -17,12 +17,7 @@
       <xacro:property name="imu_rpy" value="0.000 -0.000 3.142" />
     </xacro:if>
 
-    <xacro:if value="${namespace == ''}">
-      <xacro:property name="ns" value="" />
-    </xacro:if>
-    <xacro:unless value="${namespace == ''}">
-        <xacro:property name="ns" value="${namespace}/" />
-    </xacro:unless>
+    <xacro:property name="ns" value='${namespace + "/" if namespace else ""}' />
 
     <xacro:property name="wheel_config" value="${xacro.load_yaml(wheel_config_file)}" />
     <xacro:property name="wheel_separation_x" value="0.44" />

panther_docking/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.8)
+project(panther_docking)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(PACKAGE_DEPENDENCIES
+    ament_cmake
+    geometry_msgs
+    opennav_docking_core
+    opennav_docking
+    panther_utils
+    pluginlib
+    rclcpp
+    sensor_msgs
+    std_srvs
+    tf2_geometry_msgs
+    tf2_ros)
+
+foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+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})
+# 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)
+
+install(TARGETS panther_charging_dock LIBRARY DESTINATION lib)
+
+install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(${PROJECT_NAME}_test_panther_charging_dock
+                  test/test_panther_charging_dock.cpp)
+  target_link_libraries(${PROJECT_NAME}_test_panther_charging_dock
+                        panther_charging_dock)
+  ament_target_dependencies(${PROJECT_NAME}_test_panther_charging_dock
+                            ${PACKAGE_DEPENDENCIES})
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(panther_charging_dock)
+
+ament_package()

panther_docking/README.md

@@ -0,0 +1,49 @@
+# 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 run a `PantherChargingDock` plugin.
+
+## Configuration Files
+
+- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin.
+
+## ROS Nodes
+
+- `PantherChargingDock`:  A plugin for a Panther robot what is responsible for a charger service.
+
+### PantherChargingDock
+
+#### Publishes
+
+- `docking/dock_pose` [*geometry_msgs/PoseStamped*]: An offset dock pose.
+- `docking/staging_pose` [*geometry_msgs/PoseStamped*]: An offset staging pose next to a charging station.
+
+#### Subscribers
+
+- `battery/charging_status` [*panther_msgs/ChargingStatus*]: A charging status of Panther robot.
+- `hardware/io_state` [*panther_msgs/IOState*]: States of GPIOs of Panther robot. The `PantherChargingDock` subscribes this topic to check if a robot charges.
+
+#### Service Clients
+
+- `hardware/charger_enable` [*std_srvs/SetBoot*]: This service client enables charging in a robot.
+
+#### Parameters
+
+- `~panther_version` [*double*, default: **1.21**]: A version of Panther robot.
+- `~<dock_name>.base_frame` [*string*, default: **base_link**]: A base frame id of a robot.
+- `~<dock_name>.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.
+- `~<dock_name>.external_detection_translation_x` [*double*, default: **0.0**]: A translation over an X axis between a detected frame and a dock pose.
+- `~<dock_name>.external_detection_translation_y` [*double*, default: **0.0**]: A translation over an Y axis between a detected frame and a dock pose.
+- `~<dock_name>.external_detection_translation_x` [*double*, default: **0.0**]: A translation over a Z axis between a detected frame and a dock pose.
+- `~<dock_name>.external_detection_rotation_roll` [*double*, default: **0.0**]: A rotation over an X axis between a detected frame and a dock pose.
+- `~<dock_name>.external_detection_rotation_pitch` [*double*, default: **0.0**]: A rotation over an Y axis between a detected frame and a dock pose.
+- `~<dock_name>.external_detection_rotation_yaw` [*double*, default: **0.0**]: A rotation over a Z axis between a detected frame and a dock pose.
+- `~<dock_name>.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.
+- `~<dock_name>.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.
+- `~<dock_name>.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.
+- `~<dock_name>.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X.
+- `~<dock_name>.staging_yaw_offset` [*double*, default: **0.0**]: A staging pose is defined by offsetting a yaw angle.
+- `~<dock_name>.enable_charger_service_call_timeout` [*double*, default: **0.2**]: A timeout for calling enable charging service. A robot is unable to dock if excised.

panther_docking/config/panther_docking_server.yaml

@@ -0,0 +1,49 @@
+/**:
+  ros__parameters:
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 20.0
+    undock_linear_tolerance: 0.08
+    undock_angular_tolerance: 0.08
+    max_retries: 3
+    base_frame: "<robot_namespace>/base_link"
+    fixed_frame: "<robot_namespace>/odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+
+    dock_plugins: ["panther_charging_dock"]
+    panther_charging_dock:
+      plugin: panther_docking::PantherChargingDock
+      base_frame: "<robot_namespace>/base_link"
+      docking_distance_threshold: 0.15
+      docking_yaw_threshold: 0.15
+      staging_x_offset: -0.5
+      staging_yaw_offset: 0.0
+
+      # TODO: @delihus Try to remove this parameters by using docking station description in the ros_components_description
+      # Transform between april tag frame and dock pose. An april tag Z+ faces always a camera
+      external_detection_timeout: 0.3
+      external_detection_translation_x: 0.0
+      external_detection_translation_y: -0.175 # Distance between the detection and ground
+      external_detection_translation_z: 0.8 # Distance between the detection and the front of the robot
+      external_detection_rotation_roll: 0.0
+      external_detection_rotation_pitch: 1.57
+      external_detection_rotation_yaw:  0.0
+      filter_coef: 0.1
+
+      enable_charger_service_call_timeout: 1.0
+
+    docks: ["main_dock"]
+    main_dock:
+      type: panther_charging_dock
+      frame: <robot_namespace>/main_dock
+      pose: [0.0, 0.0, 0.0] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot
+
+    controller:
+      k_phi: 1.0
+      k_delta: 2.0
+      v_linear_min: 0.05
+      v_linear_max: 0.2
+      v_angular_max: 0.3