Added aprilros launch to simulation

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

panther_docking/launch/docking.launch.py

@@ -13,14 +13,11 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-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.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_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
@@ -36,6 +33,13 @@ def generate_launch_description():
         description=("Path to docking server configuration file."),
     )
 
+    declare_use_docking_arg = DeclareLaunchArgument(
+        "use_docking",
+        default_value="True",
+        description="Enable docking server.",
+        choices=["True", "False", "true", "false"],
+    )
+
     namespace = LaunchConfiguration("namespace")
     use_docking = LaunchConfiguration("use_docking")
     use_sim = LaunchConfiguration("use_sim")
@@ -95,28 +99,30 @@ def generate_launch_description():
         namespace=namespace,
         emulate_tty=True,
         arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"],
+        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=IfCondition(PythonExpression(["not ", use_sim, " and ", use_docking])),
-    )
+    # station_launch = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         PathJoinSubstitution(
+    #             [
+    #                 FindPackageShare("panther_docking"),
+    #                 "launch",
+    #                 "station.launch.py",
+    #             ]
+    #         ),
+    #     ),
+    #     launch_arguments={"namespace": namespace}.items(),
+    #     condition=UnlessCondition(use_sim),
+    # )
 
     return LaunchDescription(
         [
+            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,

panther_docking/launch/station.launch.py

@@ -17,6 +17,7 @@
 import imageio
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition
 from launch.substitutions import (
     Command,
     EnvironmentVariable,
@@ -45,6 +46,7 @@ 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)
 
     apriltag_image_path = generate_apriltag_and_get_path(apriltag_id)
 
@@ -85,12 +87,20 @@ def launch_setup(context, *args, **kwargs):
         remappings=[("robot_description", "station_description")],
         namespace=namespace,
         emulate_tty=True,
+        condition=IfCondition(use_docking),
     )
 
     return [station_state_pub_node]
 
 
 def generate_launch_description():
+    declare_use_docking_arg = DeclareLaunchArgument(
+        "use_docking",
+        default_value="True",
+        description="Enable docking server.",
+        choices=["True", "False", "true", "false"],
+    )
+
     declare_namespace_arg = DeclareLaunchArgument(
         "namespace",
         default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
@@ -111,6 +121,7 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
+            declare_use_docking_arg,
             declare_namespace_arg,
             declare_apriltag_id,
             declare_apriltag_size,

panther_gazebo/config/apriltag.yaml

@@ -0,0 +1,23 @@
+---
+/**:
+  ros__parameters:
+    image_transport: raw
+    family: 36h11
+    size: 0.06
+    profile: false
+
+
+    max_hamming: 0
+    detector:
+      threads: 1
+      decimate: 2.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+
+
+    tag:
+      ids: [0]
+      frames: [<robot_namespace>/main_apriltag_link]
+      sizes: [0.06]

panther_gazebo/launch/apriltag.launch.py

@@ -0,0 +1,62 @@
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from nav2_common.launch import ReplaceString
+
+
+def generate_launch_description():
+    use_docking = LaunchConfiguration("use_docking")
+    use_sim = LaunchConfiguration("use_sim")
+    namespace = LaunchConfiguration("namespace")
+
+    apriltag_config_path = LaunchConfiguration("apriltag_config_path")
+    apriltag_config_path_arg = DeclareLaunchArgument(
+        "apriltag_config_path",
+        default_value=PathJoinSubstitution(
+            [FindPackageShare("panther_gazebo"), "config", "apriltag.yaml"]
+        ),
+        description=("Path to apriltag configuration file."),
+    )
+
+    namespaced_apriltag_config_path = ReplaceString(
+        source_file=apriltag_config_path,
+        replacements={"<robot_namespace>": namespace, "//": "/"},
+    )
+
+    apriltag_node = Node(
+        package="apriltag_ros",
+        executable="apriltag_node",
+        parameters=[{"use_sim_time": use_sim}, namespaced_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(use_docking),
+    )
+
+    return LaunchDescription(
+        [
+            apriltag_config_path_arg,
+            apriltag_node,
+        ]
+    )

panther_gazebo/launch/simulate_robot.launch.py

@@ -198,6 +198,19 @@ def generate_launch_description():
         launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
     )
 
+    apriltag_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_gazebo"),
+                    "launch",
+                    "apriltag.launch.py",
+                ]
+            ),
+        ),
+        launch_arguments={"namespace": namespace, "use_sim": "True"}.items(),
+    )
+
     return LaunchDescription(
         [
             declare_battery_config_path_arg,
@@ -214,5 +227,6 @@ def generate_launch_description():
             simulate_components,
             gz_bridge,
             docking_launch,
+            apriltag_launch,
         ]
     )

panther_gazebo/package.xml

@@ -27,6 +27,7 @@
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">std_msgs</depend>
   <depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">std_srvs</depend>
 
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">apriltag_ros</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">controller_manager</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_gz_worlds</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch</exec_depend>