From fa77be72509b513b0883ce6b3041f36b2f45b34b Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Sat, 8 Jun 2024 12:03:24 -0700
Subject: [PATCH] adding server to bringup

---
 nav2_bringup/launch/navigation_launch.py | 19 ++++++++++
 nav2_bringup/params/nav2_params.yaml     | 45 ++++++++++++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py
index 84d63e026c..a40b86327e 100644
--- a/nav2_bringup/launch/navigation_launch.py
+++ b/nav2_bringup/launch/navigation_launch.py
@@ -49,6 +49,7 @@ def generate_launch_description():
         'collision_monitor',
         'bt_navigator',
         'waypoint_follower',
+        'docking_server',
     ]
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
@@ -212,6 +213,17 @@ def generate_launch_description():
                 arguments=['--ros-args', '--log-level', log_level],
                 remappings=remappings,
             ),
+            Node(
+                package='opennav_docking',
+                executable='opennav_docking',
+                name='docking_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings,
+            ),
             Node(
                 package='nav2_lifecycle_manager',
                 executable='lifecycle_manager',
@@ -287,6 +299,13 @@ def generate_launch_description():
                         parameters=[configured_params],
                         remappings=remappings,
                     ),
+                    ComposableNode(
+                        package='opennav_docking',
+                        plugin='opennav_docking::DockingServer',
+                        name='docking_server',
+                        parameters=[configured_params],
+                        remappings=remappings,
+                    ),
                     ComposableNode(
                         package='nav2_lifecycle_manager',
                         plugin='nav2_lifecycle_manager::LifecycleManager',
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 3ac25d13bb..86196ae75e 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -369,3 +369,48 @@ collision_monitor:
       min_height: 0.15
       max_height: 2.0
       enabled: True
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    docks: ['home_dock']  # Input your docks here
+    home_dock:
+      type: 'simple_charging_dock'
+      frame: map
+      pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15