'working' nav2 for physical robot

[ArendJan]

No description provided.

[ArendJan]

Doesn't need to be merged right away, maybe we need to change some launch file names

[Rezenders]

@ArendJan I totally missed this PR, sorry.
Is it still relevant? Should I review it and merge it? Are these parameters better for the real mirte?

[ArendJan]

It's okay to merge, but according to @SuperJappie08 it's not great, some things like local/global planner

[ArendJan]

it needs some improvements for it to be good, but I'm not knowledgeable in nav2 enough for that

[Rezenders]

This is the diff between the param file in this repo and the param file from the 'best' MDP team with the modifications from @SuperJappie08 @fghzamani @EGAlberts

It seems that the main differences are the laser range max/min values, the robot_model_type (diff -> omni), some changes to the critics, the addition of an obstacle_layer plugin to the local_costmap, an increase in the min velocity, and a decrease in the max velocity. Thoughts anyone?

diff --git a/krr_mirte_nav2_param.yaml b/mirte_nav2_params.yaml
index bee4144..640595c 100644
--- a/krr_mirte_nav2_param.yaml
+++ b/mirte_nav2_params.yaml
@@ -13,30 +13,34 @@ amcl:
     global_frame_id: "map"
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
+    laser_max_range: 15.0
+    laser_min_range: 0.2
     laser_model_type: "likelihood_field"
-    max_beams: 60
+    max_beams: 30
     max_particles: 2000
     min_particles: 500
     odom_frame_id: "odom"
     pf_err: 0.05
     pf_z: 0.99
-    recovery_alpha_fast: 0.0
-    recovery_alpha_slow: 0.0
+    recovery_alpha_fast: 0.2
+    recovery_alpha_slow: 0.001
     resample_interval: 1
-    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    # robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    robot_model_type: "nav2_amcl::OmniMotionModel"
+
     save_pose_rate: 0.5
-    sigma_hit: 0.2
+    sigma_hit: 0.05
     tf_broadcast: true
     transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
-    z_max: 0.05
+    update_min_a: 0.02
+    update_min_d: 0.01
+    z_hit: 0.95
+    z_max: 0.03
     z_rand: 0.5
     z_short: 0.05
     scan_topic: scan
+    set_initial_pose: True
+    initial_pose: {x: 0.0, y: 0.0, z: 0.0, yaw: 0.0}
 
 bt_navigator:
   ros__parameters:
@@ -45,6 +49,7 @@ bt_navigator:
     odom_topic: odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    default_nav_to_pose_bt_xml: "<tree_path>"
     wait_for_service_timeout: 1000
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
@@ -70,7 +75,7 @@ controller_server:
     controller_frequency: 20.0
     costmap_update_timeout: 0.30
     min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
+    min_y_velocity_threshold: 0.001
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
     progress_checker_plugins: ["progress_checker"]
@@ -81,12 +86,12 @@ controller_server:
     # Progress checker parameters
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
+      required_movement_radius: 0.1
       movement_time_allowance: 10.0
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.4
+      xy_goal_tolerance: 0.1
       yaw_goal_tolerance: 6.2
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
@@ -109,8 +114,8 @@ controller_server:
       transform_tolerance: 0.1
       temperature: 0.3
       gamma: 0.015
-      motion_model: "DiffDrive"
-      visualize: true
+      motion_model: "Omni"
+      visualize: true #Turn off for final demo maybe.
       regenerate_noises: true
       TrajectoryVisualizer:
         trajectory_step: 5
@@ -132,9 +137,9 @@ controller_server:
         threshold_to_consider: 1.4
       GoalAngleCritic:
         enabled: true
-        cost_power: 1
+        cost_power: 2
         cost_weight: 3.0
-        threshold_to_consider: 0.5
+        threshold_to_consider: 1.5 #Original is 0.5, increase to 1m so that further away, it will consider the angle
       PreferForwardCritic:
         enabled: true
         cost_power: 1
@@ -142,12 +147,12 @@ controller_server:
         threshold_to_consider: 0.5
       CostCritic:
         enabled: true
-        cost_power: 1
-        cost_weight: 3.81
+        cost_power: 3 #was 1 increased to 2 to better avoid obstacles
+        cost_weight: 4.0 #was 3.81, increased to try and better avoid obstacles
         critical_cost: 300.0
         consider_footprint: true
         collision_cost: 1000000.0
-        near_goal_distance: 1.0
+        near_goal_distance: 0.5 # was 1.0, decreased to reduce more flexibility when near to goal.
         trajectory_point_step: 2
       PathAlignCritic:
         enabled: true
@@ -181,19 +186,30 @@ local_costmap:
   local_costmap:
     ros__parameters:
       update_frequency: 5.0
-      publish_frequency: 2.0
-      global_frame: odom
+      publish_frequency: 2.0 #5hz for both
+      global_frame: odom 
       robot_base_frame: base_link
+      use_sim_time: False
       rolling_window: true
-      width: 3
-      height: 3
+      width: 2
+      height: 2
       resolution: 0.05
-      robot_radius: 0.22
-      plugins: ["voxel_layer", "inflation_layer"]
+      robot_radius: 0.2
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan # different, has a slash
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.5
+        cost_scaling_factor: 0.5
+        inflation_radius: 0.20
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -205,26 +221,71 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          # A relative topic will be appended to the parent of the local_costmap namespace.
-          # For example:
-          #   * User chosen namespace is `tb4`.
-          #   * User chosen topic is `scan`.
-          #   * Topic will be remapped to `/tb4/scan` without `local_costmap`.
-          #   * Use global topic `/scan` if you do not wish the node namespace to apply
-          topic: scan
+          topic: /scan #has a slash
           max_obstacle_height: 2.0
           clearing: True
           marking: True
           data_type: "LaserScan"
           raytrace_max_range: 3.0
           raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
+          obstacle_max_range: 7.5
           obstacle_min_range: 0.0
       static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
       always_send_full_costmap: True
 
 global_costmap:
   global_costmap:
     ros__parameters:
@@ -232,7 +293,7 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      robot_radius: 0.22
+      robot_radius: 0.3
       resolution: 0.05
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
@@ -241,13 +302,6 @@ global_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          # A relative topic will be appended to the parent of the global_costmap namespace.
-          # For example:
-          #   * User chosen namespace is `tb4`.
-          #   * User chosen topic is `scan`.
-          #   * Topic will be remapped to `/tb4/scan` without `global_costmap`.
-          #   * Use global topic `/scan` if you do not wish the node namespace to apply
-          topic: scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -262,15 +316,15 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.5
+        inflation_radius: 0.20
       always_send_full_costmap: True
 
-# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
-# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
-# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
-# map_server:
-#   ros__parameters:
-#     yaml_filename: ""
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    # Overridden in launch by the "map" launch configuration or provided defaul>
+    # To use in yaml, remove the default "map" value in the tb3_simulation_laun>
+    yaml_filename: ""
 
 map_saver:
   ros__parameters:
@@ -313,12 +367,12 @@ behavior_server:
       plugin: "nav2_behaviors/BackUp"
       acceleration_limit: 2.5
       deceleration_limit: -2.5
-      minimum_speed: 0.10
+      minimum_speed: 1.0
     drive_on_heading:
       plugin: "nav2_behaviors/DriveOnHeading"
       acceleration_limit: 2.5
       deceleration_limit: -2.5
-      minimum_speed: 0.10
+      minimum_speed: 1.0
     wait:
       plugin: "nav2_behaviors/Wait"
     assisted_teleop:
@@ -348,10 +402,10 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.5, 0.0, 1.0]
-    min_velocity: [-0.5, 0.0, -1.0]
-    max_accel: [2.5, 0.0, 3.2]
-    max_decel: [-2.5, 0.0, -3.2]
+    max_velocity: [1.0, 1.0, 1.0]
+    min_velocity: [-1.0, -1.0, -1.0]
+    max_accel: [2.5, 2.0, 3.2]
+    max_decel: [-2.5, -2.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]

[gavanderhoorn]

the robot_model_type (diff -> omni)

minor change ..

---

Edit: all the topic name updates that introduce a / should probably not be merged in.

[Rezenders]

This is the diff between the file in this PR and the one from https://github.com/SuperJappie08/mirte-demo-ensurance/blob/development/mirte_navigation/params/mirte_nav2_params.yaml

diff --git a/mirte_nav2_params_arend.yaml b/mirte_nav2_params.yaml
index 15c3331..640595c 100644
--- a/mirte_nav2_params_arend.yaml
+++ b/mirte_nav2_params.yaml
@@ -13,32 +13,34 @@ amcl:
     global_frame_id: "map"
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
+    laser_max_range: 15.0
+    laser_min_range: 0.2
     laser_model_type: "likelihood_field"
-    max_beams: 60
+    max_beams: 30
     max_particles: 2000
     min_particles: 500
     odom_frame_id: "odom"
     pf_err: 0.05
     pf_z: 0.99
-    recovery_alpha_fast: 0.0
-    recovery_alpha_slow: 0.0
+    recovery_alpha_fast: 0.2
+    recovery_alpha_slow: 0.001
     resample_interval: 1
     # robot_model_type: "nav2_amcl::DifferentialMotionModel"
     robot_model_type: "nav2_amcl::OmniMotionModel"
 
     save_pose_rate: 0.5
-    sigma_hit: 0.2
+    sigma_hit: 0.05
     tf_broadcast: true
     transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
-    z_max: 0.05
+    update_min_a: 0.02
+    update_min_d: 0.01
+    z_hit: 0.95
+    z_max: 0.03
     z_rand: 0.5
     z_short: 0.05
     scan_topic: scan
+    set_initial_pose: True
+    initial_pose: {x: 0.0, y: 0.0, z: 0.0, yaw: 0.0}
 
 bt_navigator:
   ros__parameters:
@@ -47,6 +49,7 @@ bt_navigator:
     odom_topic: odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    default_nav_to_pose_bt_xml: "<tree_path>"
     wait_for_service_timeout: 1000
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
@@ -112,7 +115,7 @@ controller_server:
       temperature: 0.3
       gamma: 0.015
       motion_model: "Omni"
-      visualize: true
+      visualize: true #Turn off for final demo maybe.
       regenerate_noises: true
       TrajectoryVisualizer:
         trajectory_step: 5
@@ -134,9 +137,9 @@ controller_server:
         threshold_to_consider: 1.4
       GoalAngleCritic:
         enabled: true
-        cost_power: 1
+        cost_power: 2
         cost_weight: 3.0
-        threshold_to_consider: 0.5
+        threshold_to_consider: 1.5 #Original is 0.5, increase to 1m so that further away, it will consider the angle
       PreferForwardCritic:
         enabled: true
         cost_power: 1
@@ -144,12 +147,12 @@ controller_server:
         threshold_to_consider: 0.5
       CostCritic:
         enabled: true
-        cost_power: 1
-        cost_weight: 3.81
+        cost_power: 3 #was 1 increased to 2 to better avoid obstacles
+        cost_weight: 4.0 #was 3.81, increased to try and better avoid obstacles
         critical_cost: 300.0
         consider_footprint: true
         collision_cost: 1000000.0
-        near_goal_distance: 1.0
+        near_goal_distance: 0.5 # was 1.0, decreased to reduce more flexibility when near to goal.
         trajectory_point_step: 2
       PathAlignCritic:
         enabled: true
@@ -183,19 +186,30 @@ local_costmap:
   local_costmap:
     ros__parameters:
       update_frequency: 5.0
-      publish_frequency: 2.0
-      global_frame: odom
+      publish_frequency: 2.0 #5hz for both
+      global_frame: odom 
       robot_base_frame: base_link
+      use_sim_time: False
       rolling_window: true
-      width: 3
-      height: 3
+      width: 2
+      height: 2
       resolution: 0.05
-      robot_radius: 0.22
-      plugins: ["voxel_layer", "inflation_layer"]
+      robot_radius: 0.2
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan # different, has a slash
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.5
+        cost_scaling_factor: 0.5
+        inflation_radius: 0.20
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -207,26 +221,71 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          # A relative topic will be appended to the parent of the local_costmap namespace.
-          # For example:
-          #   * User chosen namespace is `tb4`.
-          #   * User chosen topic is `scan`.
-          #   * Topic will be remapped to `/tb4/scan` without `local_costmap`.
-          #   * Use global topic `/scan` if you do not wish the node namespace to apply
-          topic: scan
+          topic: /scan #has a slash
           max_obstacle_height: 2.0
           clearing: True
           marking: True
           data_type: "LaserScan"
           raytrace_max_range: 3.0
           raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
+          obstacle_max_range: 7.5
           obstacle_min_range: 0.0
       static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
       always_send_full_costmap: True
 
 global_costmap:
   global_costmap:
     ros__parameters:
@@ -234,7 +293,7 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      robot_radius: 0.22
+      robot_radius: 0.3
       resolution: 0.05
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
@@ -243,13 +302,6 @@ global_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          # A relative topic will be appended to the parent of the global_costmap namespace.
-          # For example:
-          #   * User chosen namespace is `tb4`.
-          #   * User chosen topic is `scan`.
-          #   * Topic will be remapped to `/tb4/scan` without `global_costmap`.
-          #   * Use global topic `/scan` if you do not wish the node namespace to apply
-          topic: scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -264,15 +316,15 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.5
+        inflation_radius: 0.20
       always_send_full_costmap: True
 
-# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
-# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
-# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
-# map_server:
-#   ros__parameters:
-#     yaml_filename: ""
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    # Overridden in launch by the "map" launch configuration or provided defaul>
+    # To use in yaml, remove the default "map" value in the tb3_simulation_laun>
+    yaml_filename: ""