Merge remote-tracking branch 'origin/ros2-docking' into ros2-docking-…

…charging-dock-wibotic-msgs Signed-off-by: Jakub Delicat <[email protected]>
husarion · Nov 12, 2024 · 860b1cf · 860b1cf
2 parents 7ae2294 + 4ee2f68
commit 860b1cf
Show file tree

Hide file tree

Showing 42 changed files with 1,236 additions and 367 deletions.
diff --git a/README.md b/README.md
@@ -88,6 +88,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `components_config_path`     | Additional components configuration file. Components described in this file are dynamically included in Panther's urdf. Panther options are described in [the manual](https://husarion.com/manuals/panther/panther-options). <br/> ***string:*** [`components.yaml`](./panther_description/config/components.yaml)                                          |
 | 🤖🖥️  | `controller_config_path`     | Path to controller configuration file. A path to custom configuration can be specified here. <br/> ***string:*** [`{wheel_type}_controller.yaml`](./panther_controller/config/)                                                                                                                                                                              |
 | 🤖   | `disable_manager`            | Enable or disable manager_bt_node. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                      |
+| 🤖🖥️  | `docking_bt_project_path`    | Path to BehaviorTree project file, responsible for docking management. <br/> ***string:*** [`DockingBT.btproj`](./panther_manager/behavior_trees/DockingBT.btproj)                                                                                                                                                                                            |
 | 🤖🖥️  | `fuse_gps`                   | Include GPS for data fusion. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                            |
 | 🖥️   | `gz_bridge_config_path`      | Path to the parameter_bridge configuration file. <br/> ***string:*** [`gz_bridge.yaml`](./panther_gazebo/config/gz_bridge.yaml)                                                                                                                                                                                                                                 |
 | 🖥️   | `gz_gui`                     | Run simulation with specific GUI layout. <br/> ***string:*** [`teleop.config`](https://github.com/husarion/husarion_gz_worlds/blob/main/config/teleop.config)                                                                                                                                                                                                                |
@@ -104,6 +105,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🖥️   | `robots`                     | The list of the robots spawned in the simulation e.g. `robots:='robot1={x: 1.0, y: -2.0}; robot2={x: 1.0, y: -4.0}'` <br/> ***string:*** `''`                                                                                                                                                                                                                  |
 | 🤖🖥️  | `safety_bt_project_path`     | Path to BehaviorTree project file, responsible for safety and shutdown management. <br/> ***string:*** [`PantherSafetyBT.btproj`](./panther_manager/behavior_trees/PantherSafetyBT.btproj)                                                                                                                                                                   |
 | 🤖🖥️  | `shutdown_hosts_config_path` | Path to file with list of hosts to request shutdown. <br/> ***string:*** [`shutdown_hosts_config.yaml`](./panther_manager/config/shutdown_hosts_config.yaml)                                                                                                                                                                                                               |
+| 🤖🖥️  | `use_docking`                | Enable docking server. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |

diff --git a/panther_diagnostics/src/system_monitor_node.cpp b/panther_diagnostics/src/system_monitor_node.cpp
@@ -28,7 +28,6 @@
 #include "panther_utils/ros_utils.hpp"
 
 #include "panther_diagnostics/filesystem.hpp"
-#include "panther_diagnostics/types.hpp"
 
 namespace panther_diagnostics
 {

diff --git a/panther_docking/README.md b/panther_docking/README.md
@@ -46,7 +46,7 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 
 - `base_frame` [*string*, default: **base_link**]: A base frame id of a robot.
 - `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot.
-- `<dock_type>.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for dock pose.
+- `<dock_type>.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_type>.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_type>.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_type>.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X.

diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py
@@ -17,7 +17,6 @@
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
-    EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
     PythonExpression,
@@ -28,21 +27,6 @@
 
 
 def generate_launch_description():
-    use_sim = LaunchConfiguration("use_sim")
-    declare_use_sim_arg = DeclareLaunchArgument(
-        "use_sim",
-        default_value="False",
-        description="Whether simulation is used",
-        choices=[True, False, "True", "False", "true", "false", "1", "0"],
-    )
-
-    namespace = LaunchConfiguration("namespace")
-    declare_namespace_arg = DeclareLaunchArgument(
-        "namespace",
-        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
-        description="Add namespace to all launched nodes.",
-    )
-
     docking_server_config_path = LaunchConfiguration("docking_server_config_path")
     declare_docking_server_config_path_arg = DeclareLaunchArgument(
         "docking_server_config_path",
@@ -52,6 +36,17 @@ 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")
+
     log_level = LaunchConfiguration("log_level")
     declare_log_level = DeclareLaunchArgument(
         "log_level",
@@ -82,19 +77,22 @@ def generate_launch_description():
     docking_server_node = Node(
         package="opennav_docking",
         executable="opennav_docking",
+        namespace=namespace,
         parameters=[
             namespaced_docking_server_config,
             {"use_sim_time": use_sim},
         ],
         arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"],
-        namespace=namespace,
+        remappings=[("~/transition_event", "~/_transition_event")],
         emulate_tty=True,
+        condition=IfCondition(use_docking),
     )
 
     docking_server_activate_node = Node(
         package="nav2_lifecycle_manager",
         executable="lifecycle_manager",
         name="nav2_docking_lifecycle_manager",
+        namespace=namespace,
         parameters=[
             {
                 "autostart": True,
@@ -104,7 +102,7 @@ def generate_launch_description():
                 "use_sim_time": use_sim,
             },
         ],
-        namespace=namespace,
+        condition=IfCondition(use_docking),
     )
 
     dock_pose_publisher = Node(
@@ -144,8 +142,7 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
-            declare_use_sim_arg,
-            declare_namespace_arg,
+            declare_use_docking_arg,
             declare_docking_server_config_path_arg,
             declare_log_level,
             declare_use_wibotic_info_arg,

diff --git a/panther_docking/launch/station.launch.py b/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,
@@ -28,10 +29,11 @@
 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)
 
@@ -45,6 +47,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 +88,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 +122,7 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
+            declare_use_docking_arg,
             declare_namespace_arg,
             declare_apriltag_id,
             declare_apriltag_size,

diff --git a/panther_gazebo/config/apriltag.yaml b/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]
diff --git a/panther_gazebo/launch/apriltag.launch.py b/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,
+        ]
+    )
diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py
@@ -185,6 +185,32 @@ def generate_launch_description():
         emulate_tty=True,
     )
 
+    docking_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_docking"),
+                    "launch",
+                    "docking.launch.py",
+                ]
+            ),
+        ),
+        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,
@@ -200,5 +226,7 @@ def generate_launch_description():
             ekf_launch,
             simulate_components,
             gz_bridge,
+            docking_launch,
+            apriltag_launch,
         ]
     )
diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml
@@ -27,13 +27,15 @@
   <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>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch_ros</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">nav2_common</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_controller</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_description</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_docking</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_lights</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_localization</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_manager</exec_depend>