Tested with nav2 in simulation | Added docking to multibple stations …

…| Spawning multiple stations

Signed-off-by: Jakub Delicat <[email protected]>

panther_description/config/docking_components.yaml

@@ -6,15 +6,15 @@ components:
     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: front_bumper_link
     xyz: 0.0 0.0 -0.06
     rpy: 0.0 0.0 0.0
     device_namespace: camera
+
+  - type: LDR06
+    parent_link: cover_link
+    xyz: 0.0 0.0 0.0
+    rpy: 0.0 0.0 0.0
+    device_namespace: main_lidar

panther_gazebo/config/apriltag.yaml → panther_docking/config/apriltag.yaml

@@ -18,6 +18,6 @@
 
 
     tag:
-      ids: [0]
-      frames: [<robot_namespace>/main_apriltag_link]
-      sizes: [0.06]
+      ids: [0, 1]
+      frames: [main_apriltag_link, backup_apriltag_link]
+      sizes: [0.06, 0.06]

panther_docking/config/panther_docking_server.yaml

@@ -16,17 +16,26 @@
     dock_plugins: ["panther_charging_dock"]
     panther_charging_dock:
       plugin: panther_docking::PantherChargingDock
-      external_detection_timeout: 0.1
-      docking_distance_threshold: 0.08
-      docking_yaw_threshold: 0.1
-      staging_x_offset: -0.5
+      external_detection_timeout: 0.2
+      docking_distance_threshold: 0.12
+      docking_yaw_threshold: 0.2
+      staging_x_offset: -0.8
       filter_coef: 0.1
 
-    docks: ["main"]
+    docks: ["main", "backup"]
     main:
       type: panther_charging_dock
-      frame: <robot_namespace>/main_wibotic_receiver_requested_pose_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
+      frame: <robot_namespace>/map
+      dock_frame: main_wibotic_receiver_requested_pose_link
+      pose: [1.0, 1.5, 1.57] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot
+      apriltag_id: 0
+
+    backup:
+      type: panther_charging_dock
+      frame: <robot_namespace>/map
+      dock_frame: backup_wibotic_receiver_requested_pose_link
+      pose: [-1.0, 1.5, 1.57] # position of the dock device (not the staging position), the front (X+) of the dock should point away from the robot
+      apriltag_id: 1
 
     controller:
       k_phi: 1.0

panther_docking/include/panther_docking/panther_charging_dock.hpp

@@ -140,7 +140,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    */
   void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
 
-  void updateAndPublishStagingPose();
+  void updateAndPublishStagingPose(const std::string & frame);
 
   void setDockPose(const PoseStampedMsg::SharedPtr pose);
 

panther_docking/launch/docking.launch.py

@@ -13,11 +13,14 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument  # , IncludeLaunchDescription
-from launch.conditions import IfCondition  # , UnlessCondition
-
-# from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
@@ -40,6 +43,15 @@ def generate_launch_description():
         choices=["True", "False", "true", "false"],
     )
 
+    apriltag_config_path = LaunchConfiguration("apriltag_config_path")
+    declare_apriltag_config_path_arg = DeclareLaunchArgument(
+        "apriltag_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_docking"), "config", "apriltag.yaml"]
+        ),
+        description=("Path to apriltag configuration file. Only used in simulation."),
+    )
+
     namespace = LaunchConfiguration("namespace")
     use_docking = LaunchConfiguration("use_docking")
     use_sim = LaunchConfiguration("use_sim")
@@ -102,29 +114,43 @@ def generate_launch_description():
         condition=IfCondition(use_docking),
     )
 
-    # FIXME: This launch does not work with the simulation. It can be caused by different versions of opencv
-    # station_launch = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource(
-    #         PathJoinSubstitution(
-    #             [
-    #                 FindPackageShare("panther_docking"),
-    #                 "launch",
-    #                 "station.launch.py",
-    #             ]
-    #         ),
-    #     ),
-    #     launch_arguments={"namespace": namespace}.items(),
-    #     condition=UnlessCondition(use_sim),
-    # )
+    apriltag_node = Node(
+        package="apriltag_ros",
+        executable="apriltag_node",
+        parameters=[{"use_sim_time": True}, apriltag_config_path],
+        namespace=namespace,
+        emulate_tty=True,
+        remappings={
+            "camera_info": "camera/color/camera_info",
+            "image_rect": "camera/color/image_raw",
+            "detections": "docking/april_tags",
+        }.items(),
+        condition=IfCondition(PythonExpression([use_docking, " and ", use_sim])),
+    )
+
+    station_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_docking"),
+                    "launch",
+                    "station.launch.py",
+                ]
+            ),
+        ),
+        launch_arguments={"namespace": namespace}.items(),
+    )
 
     return LaunchDescription(
         [
+            declare_apriltag_config_path_arg,
             declare_use_docking_arg,
             declare_docking_server_config_path_arg,
             declare_log_level,
-            # station_launch,
+            station_launch,
             docking_server_node,
             docking_server_activate_node,
             dock_pose_publisher,
+            apriltag_node,
         ]
     )

panther_docking/launch/station.launch.py

@@ -14,25 +14,27 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
 import imageio
+import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition
 from launch.substitutions import (
     Command,
-    EnvironmentVariable,
     FindExecutable,
     LaunchConfiguration,
     PathJoinSubstitution,
-    PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from launch_ros.substitutions import FindPackageShare
-from moms_apriltag import TagGenerator2
 
 
 def generate_apriltag_and_get_path(tag_id):
+    from moms_apriltag import TagGenerator2
+
     tag_generator = TagGenerator2("tag36h11")
     tag_image = tag_generator.generate(tag_id, scale=1000)
 
@@ -42,12 +44,7 @@ def generate_apriltag_and_get_path(tag_id):
     return path
 
 
-def launch_setup(context, *args, **kwargs):
-    namespace = LaunchConfiguration("namespace").perform(context)
-    apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context))
-    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
-    use_docking = LaunchConfiguration("use_docking").perform(context)
-
+def generate_urdf(name, apriltag_id, apriltag_size):
     apriltag_image_path = generate_apriltag_and_get_path(apriltag_id)
 
     station_description_content = Command(
@@ -62,35 +59,56 @@ def launch_setup(context, *args, **kwargs):
                 ]
             ),
             " device_namespace:=",
-            "main",
+            name,
             " apriltag_image_path:=",
             apriltag_image_path,
             " apriltag_size:=",
             apriltag_size,
         ]
     )
+    return station_description_content
 
-    namespace_ext = PythonExpression(["'", namespace, "' + '/' if '", namespace, "' else ''"])
-
-    station_description = {
-        "robot_description": ParameterValue(station_description_content, value_type=str)
-    }
-
-    station_state_pub_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        name="wibotic_station_state_publisher",
-        parameters=[
-            station_description,
-            {"frame_prefix": namespace_ext},
-        ],
-        remappings=[("robot_description", "station_description")],
-        namespace=namespace,
-        emulate_tty=True,
-        condition=IfCondition(use_docking),
-    )
 
-    return [station_state_pub_node]
+def launch_stations_descriptions(context, *args, **kwargs):
+    apriltag_id = int(LaunchConfiguration("apriltag_id").perform(context))
+    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
+    use_docking = LaunchConfiguration("use_docking").perform(context)
+
+    docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context)
+    apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
+
+    docking_server_config = None
+    if docking_server_config_path == "None":
+        return []
+
+    with open(os.path.join(docking_server_config_path)) as file:
+        docking_server_config = yaml.safe_load(file)
+
+    actions = []
+    ros_parameters = docking_server_config["/**"]["ros__parameters"]
+    docks_names = ros_parameters["docks"]
+    for dock_name in docks_names:
+        apriltag_id = ros_parameters[dock_name]["apriltag_id"]
+        station_description_content = generate_urdf(dock_name, apriltag_id, apriltag_size)
+        station_description = {
+            "robot_description": ParameterValue(station_description_content, value_type=str)
+        }
+
+        station_state_pub_node = Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            name=[dock_name, "_station_state_publisher"],
+            parameters=[
+                station_description,
+            ],
+            remappings=[("robot_description", [dock_name, "_station_description"])],
+            emulate_tty=True,
+            condition=IfCondition(use_docking),
+        )
+
+        actions.append(station_state_pub_node)
+
+    return actions
 
 
 def generate_launch_description():
@@ -101,12 +119,6 @@ def generate_launch_description():
         choices=["True", "False", "true", "false"],
     )
 
-    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="0",
@@ -122,9 +134,8 @@ def generate_launch_description():
     return LaunchDescription(
         [
             declare_use_docking_arg,
-            declare_namespace_arg,
             declare_apriltag_id,
             declare_apriltag_size,
-            OpaqueFunction(function=launch_setup),
+            OpaqueFunction(function=launch_stations_descriptions),
         ]
     )

panther_docking/package.xml

@@ -26,6 +26,7 @@
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
 
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">apriltag_ros</exec_depend>
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>python3-imageio</exec_depend>
   <exec_depend>xacro</exec_depend>