Merge pull request #409 from husarion/ros2-docking-components

Add Wibotic componenets to Panther

panther_controller/launch/controller.launch.py

@@ -219,8 +219,6 @@ def generate_launch_description():
             "controller_manager",
             "--controller-manager-timeout",
             "10",
-            "--namespace",
-            namespace,
         ],
         namespace=namespace,
         emulate_tty=True,
@@ -235,8 +233,6 @@ def generate_launch_description():
             "controller_manager",
             "--controller-manager-timeout",
             "10",
-            "--namespace",
-            namespace,
         ],
         namespace=namespace,
         emulate_tty=True,
@@ -259,8 +255,6 @@ def generate_launch_description():
             "controller_manager",
             "--controller-manager-timeout",
             "10",
-            "--namespace",
-            namespace,
         ],
         namespace=namespace,
         emulate_tty=True,

panther_description/config/docking_components.yaml

@@ -0,0 +1,20 @@
+
+components:
+  - type: WCH01
+    parent_link: lights_channel_1_link
+    xyz: -0.02 0.0 -0.0185
+    rpy: 0.0 0.0 0.0
+    device_namespace: wireless_charger
+
+  - type: WCH02
+    parent_link: world
+    xyz: 3.0 -2.0 1.0
+    rpy: -1.57  0.0 1.57
+    device_namespace: wireless_charger
+
+  - type: CAM01
+    name: camera
+    parent_link: lights_channel_1_link
+    xyz: 0.05 0.0 0.0
+    rpy: 0.0 0.0 0.0
+    device_namespace: camera

panther_docking/CMakeLists.txt

@@ -22,6 +22,13 @@ foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
 endforeach()
 
+find_package(
+  Python
+  COMPONENTS Interpreter
+  REQUIRED)
+
+execute_process(COMMAND ${Python_EXECUTABLE} -m pip install moms_apriltag)
+
 include_directories(include)
 
 pluginlib_export_plugin_description_file(opennav_docking_core plugin.xml)

panther_docking/README.md

@@ -4,7 +4,8 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 
 ## Launch Files
 
-- `docking.launch.py`: Launch a node that creates `docking_server` and run a `PantherChargingDock` plugin.
+- `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/config/panther_docking_server.yaml

@@ -19,26 +19,25 @@
       base_frame: "<robot_namespace>/base_link"
       docking_distance_threshold: 0.15
       docking_yaw_threshold: 0.15
-      staging_x_offset: -0.5
+      staging_x_offset: -0.8
       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_translation_x: 0.6
+      external_detection_translation_y: 0.0
+      external_detection_translation_z: 0.0
       external_detection_rotation_roll: 0.0
-      external_detection_rotation_pitch: 1.57
-      external_detection_rotation_yaw:  0.0
-      filter_coef: 0.1
+      external_detection_rotation_pitch: 0.0
+      external_detection_rotation_yaw: 3.14
 
       enable_charger_service_call_timeout: 1.0
 
     docks: ["main_dock"]
     main_dock:
       type: panther_charging_dock
-      frame: <robot_namespace>/main_dock
+      frame: <robot_namespace>/wibotic_station_link
       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:

panther_docking/launch/docking.launch.py

@@ -13,7 +13,8 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     EnvironmentVariable,
     LaunchConfiguration,
@@ -84,11 +85,25 @@ def generate_launch_description():
         namespace=namespace,
     )
 
+    station_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_docking"),
+                    "launch",
+                    "station.launch.py",
+                ]
+            ),
+        ),
+        launch_arguments={"namespace": namespace}.items(),
+    )
+
     return LaunchDescription(
         [
             declare_use_sim_arg,
             declare_namespace_arg,
             declare_docking_server_config_path_arg,
+            station_launch,
             docking_server_node,
             docking_server_activate_node,
         ]

panther_docking/launch/station.launch.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import imageio
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import (
+    Command,
+    EnvironmentVariable,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from moms_apriltag import TagGenerator2
+
+
+def generate_apriltag_and_get_path(tag_id):
+    tag_generator = TagGenerator2("tag36h11")
+    tag_image = tag_generator.generate(tag_id, scale=100)
+
+    path = f"/tmp/tag_{tag_id}.png"
+
+    imageio.imwrite(path, tag_image)
+    return path
+
+
+def launch_setup(context, *args, **kwargs):
+    namespace = LaunchConfiguration("namespace")
+    apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context))
+    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
+
+    apriltag_image_path = generate_apriltag_and_get_path(apriltag_id)
+
+    station_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ros_components_description"),
+                    "urdf",
+                    "wibotic_station.urdf.xacro",
+                ]
+            ),
+            " namespace:=",
+            namespace,
+            " apriltag_image_path:=",
+            apriltag_image_path,
+            " apriltag_size:=",
+            apriltag_size,
+        ]
+    )
+
+    namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"])
+
+    station_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="wibotic_station_state_publisher",
+        parameters=[
+            {"robot_description": station_description_content},
+            {"frame_prefix": namespace_ext},
+        ],
+        remappings=[("robot_description", "station_description")],
+        namespace=namespace,
+        emulate_tty=True,
+    )
+
+    return [station_state_pub_node]
+
+
+def generate_launch_description():
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes.",
+    )
+
+    declare_apriltag_id = DeclareLaunchArgument(
+        "apriltag_id",
+        default_value="1",
+        description="ID of a generated apriltag on the station",
+    )
+
+    declare_apriltag_size = DeclareLaunchArgument(
+        "apriltag_size",
+        default_value="0.15",
+        description="Size in meters of a generated apriltag on the station",
+    )
+
+    return LaunchDescription(
+        [
+            declare_namespace_arg,
+            declare_apriltag_id,
+            declare_apriltag_size,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

panther_docking/package.xml

@@ -15,16 +15,20 @@
   <author email="[email protected]">Jakub Delicat</author>
 
   <depend>geometry_msgs</depend>
-  <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_util</depend>
   <depend>opennav_docking</depend>
   <depend>panther_utils</depend>
   <depend>pluginlib</depend>
+  <depend>python3-pip</depend>
   <depend>rclcpp</depend>
+  <depend>ros_components_description</depend>
   <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
 
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>python3-imageio</exec_depend>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <test_depend>ament_cmake_gtest</test_depend>