Merge branch 'docking_server' into develop

Author: Rabbit025879

Navigation2/nav2_controller/src/controller_server.cpp

@@ -533,7 +533,7 @@ void ControllerServer::computeAndPublishVelocity()
 void ControllerServer::updateGlobalPath()
 {
   if (action_server_->is_preempt_requested()) {
-    RCLCPP_INFO(get_logger(), "Passing new path to controller.");
+    RCLCPP_DEBUG(get_logger(), "Passing new path to controller.");
     auto goal = action_server_->accept_pending_goal();
     std::string current_controller;
     if (findControllerId(goal->controller_id, current_controller)) {

custom_bts/custom_nav_thru_poses/behavior_trees/navigate_through_poses_w_controller_selector.xml

@@ -10,7 +10,7 @@
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
             <ReactiveSequence> 
-              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.01"/>
+              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.25"/>
               <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
             </ReactiveSequence>
             <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>

navigation2_run/launch/navigation_launch.py

@@ -47,7 +47,8 @@ def generate_launch_description():
                        'behavior_server',
                        'bt_navigator',
                        'waypoint_follower',
-                       'velocity_smoother']
+                       'velocity_smoother',
+                       'docking_server']
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
     # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
@@ -203,6 +204,16 @@ def generate_launch_description():
                 parameters=[{'use_sim_time': use_sim_time},
                             {'autostart': autostart},
                             {'node_names': lifecycle_nodes}]),
+            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)
         ]
     )
 
@@ -260,6 +271,12 @@ def generate_launch_description():
                 parameters=[{'use_sim_time': use_sim_time,
                              'autostart': autostart,
                              'node_names': lifecycle_nodes}]),
+            ComposableNode(
+                package='opennav_docking',
+                plugin='opennav_docking::DockingServer',
+                name='docking_server',
+                parameters=[configured_params],
+                remappings=remappings)
         ],
     )
 

navigation2_run/params/nav2_params.yaml

@@ -90,7 +90,7 @@ controller_server:
     progress_checker_plugin: "progress_checker"
     current_goal_checker: "percise_goal_checker"
     goal_checker_plugins: ["percise_goal_checker"]
-    controller_plugins: ["FollowPath", "DockMission", "MPPI"]
+    controller_plugins: ["FollowPath", "DockMission", "Graceful", "MPPI"]
 
     # Progress checker parameters
     progress_checker:
@@ -128,6 +128,25 @@ controller_server:
       use_fixed_curvature_lookahead: false
       curvature_lookahead_dist: 0.25
 
+    Graceful:
+      plugin: nav2_graceful_controller::GracefulController
+      transform_tolerance: 0.1
+      min_lookahead: 0.25
+      max_lookahead: 1.0
+      initial_rotation: true
+      initial_rotation_threshold: 0.75
+      prefer_final_rotation: true
+      allow_backward: false
+      k_phi: 2.0
+      k_delta: 1.0
+      beta: 0.4
+      lambda: 2.0
+      v_linear_min: 0.1
+      v_linear_max: 0.5
+      v_angular_max: 5.0
+      v_angular_min_in_place: 0.25
+      slowdown_radius: 1.5
+
     MPPI:
       plugin: "nav2_mppi_controller::MPPIController"
       time_steps: 56
@@ -306,8 +325,8 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 7.0
-        inflation_radius: 0.1
+        cost_scaling_factor: 5.0
+        inflation_radius: 0.15
       rival_layer:
         plugin: "custom_path_costmap_plugin::RivalLayer"
         enabled: True
@@ -363,7 +382,6 @@ planner_server:
 smoother_server:
   ros__parameters:
     use_sim_time: False
-    # smoother_plugins: [""]
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -421,11 +439,62 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.5, 0.5, 0.75]
-    min_velocity: [-0.5, -0.5, -0.75]
+    max_velocity: [1.0, 1.0, 0.75]
+    min_velocity: [-1.0, -1.0, -0.75]
     max_accel: [1.0, 1.0, 3.0]
     max_decel: [-1.0, -1.0, -3.0]  
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
-    velocity_timeout: 1.0
+    velocity_timeout: 0.1 # must be less than velocity_smoother_wait in docking_server
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 20.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "map"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.02
+    velocity_smoother_wait: 0.12
+
+    # Types of docks
+    dock_plugins: ['mission_dock']
+    mission_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.01
+      staging_x_offset: 0.35
+      staging_y_offset: 0.0
+      staging_yaw_offset: 0.0
+      use_battery_status: false
+      use_stall_detection: false
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: 0.0
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: 0.0
+      external_detection_rotation_pitch: 0.0
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    docks: ['home_dock', 'flex_dock1', 'flex_dock2']
+    home_dock:
+      type: 'mission_dock'
+      frame: map
+      pose: [0.0, 0.0, 0.0]
+    flex_dock1:
+      type: 'mission_dock'
+      frame: map
+      pose: [2.2, 1.4, 0.0]
+    flex_dock2:
+      type: 'mission_dock'
+      frame: map
+      pose: [1.5, 1.0, 0.0]
+    
+    # dock_database: /my/path/to/dock_database.yaml
+
+    controller:
+      max_linear_vel: 0.5