Update Nav2 to work better, and update paths

Author: Bilal Gill

src/images/box.png src/hangar_sim/images/box.pngsrc/images/box.png renamed to src/hangar_sim/images/box.png

@@ -10,7 +10,7 @@
         ID="CreatePoseVectorFromDirectory"
         _collapsed="true"
         pose_vector="{pose_vector}"
-        directory_path="/home/henry/user_ws/src/poses"
+        directory_path="/home/studio-user/user_ws/src/hangar_sim/poses"
       />
       <Action
         ID="CreateStampedPose"

src/images/box_2.png src/hangar_sim/images/box_2.pngsrc/images/box_2.png renamed to src/hangar_sim/images/box_2.png

@@ -20,13 +20,13 @@
         />
         <Action
           ID="SaveImageToFile"
-          file_path="/home/henry/user_ws/src/wrist_camera_images"
+          file_path="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
           file_prefix="box"
           image="{wrist_camera_image}"
         />
         <Action
           ID="SaveImageToFile"
-          file_path="/home/henry/user_ws/src/scene_camera_images"
+          file_path="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
           file_prefix="box"
           image="{scene_camera_image}"
         />

src/hangar_sim/objectives/count_boxes.xml

@@ -11,7 +11,7 @@
         ID="GetFilenamesFromDirectory"
         file_type=".png"
         result="{image_filename_vector}"
-        directory_path="/home/henry/user_ws/src/wrist_camera_images"
+        directory_path="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
       />
       <Action ID="Script" code="inc:=0" />
       <Decorator

src/hangar_sim/objectives/count_boxes_subtree.xml

@@ -125,8 +125,8 @@ controller_server:
     # Progress checker parameters
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.1
-      movement_time_allowance: 25.0
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
     # Goal checker parameters
     #precise_goal_checker:
     #  plugin: "nav2_controller::SimpleGoalChecker"
@@ -136,85 +136,102 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.5
+      xy_goal_tolerance: 0.25
       yaw_goal_tolerance: 0.25
-    # DWB parameters
     FollowPath:
-      plugin: "nav2_rotation_shim_controller::RotationShimController"
-      primary_controller: "dwb_core::DWBLocalPlanner"
-      # primary_controller: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-      angular_dist_threshold: 0.1
-      forward_sampling_distance: 0.5
-      rotate_to_heading_angular_vel: 5.0
-      max_angular_accel: 10.0
-      simulate_ahead_time: 1.0
-      rotate_to_goal_heading: true
-
-      # desired_linear_vel: 2.5
-      # lookahead_dist: 0.6
-      # min_lookahead_dist: 0.3
-      # max_lookahead_dist: 0.9
-      # lookahead_time: 1.5
-      # rotate_to_heading_angular_vel: 5.0
-      # transform_tolerance: 0.1
-      # use_velocity_scaled_lookahead_dist: false
-      # min_approach_linear_velocity: 0.05
-      # approach_velocity_scaling_dist: 1.0
-      # use_collision_detection: true
-      # max_allowed_time_to_collision_up_to_carrot: 1.0
-      # use_regulated_linear_velocity_scaling: true
-      # use_cost_regulated_linear_velocity_scaling: false
-      # regulated_linear_scaling_min_radius: 0.9
-      # regulated_linear_scaling_min_speed: 0.25
-      # use_rotate_to_heading: true
-      # rotate_to_heading_min_angle: 0.785
-      # max_angular_accel: 10.0
-      # max_robot_pose_search_dist: 10.0
-      # use_interpolation: true
-      # cost_scaling_dist: 0.3
-      # cost_scaling_gain: 1.0
-      # inflation_cost_scaling_factor: 1.5
-
-
-      debug_trajectory_details: false
-      min_vel_x: -1.0
-      min_vel_y: -1.0
-      max_vel_x: 2.5
-      max_vel_y: 1.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 2.5
-      min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      acc_lim_x: 2.5
-      acc_lim_y: 2.5
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      # ObstaclesCritic:
+      #   enabled: true
+      #   cost_power: 1
+      #   repulsion_weight: 1.5
+      #   critical_weight: 20.0
+      #   consider_footprint: false
+      #   collision_cost: 10000.0
+      #   collision_margin_distance: 0.1
+      #   near_goal_distance: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+      PathAlignCritic:
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 3
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+      # VelocityDeadbandCritic:
+      #   enabled: true
+      #   cost_power: 1
+      #   cost_weight: 35.0
+      #   deadband_velocities: [0.05, 0.05, 0.05]
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -384,10 +401,10 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [2.5, 2.5, 10.0]
-    min_velocity: [-2.5, -2.5, -10.0]
-    max_accel: [2.5, 2.5, 100.0]
-    max_decel: [-2.5, -2.5, -100.0]
+    max_velocity: [0.26, 0.0, 1.0]
+    min_velocity: [-0.26, 0.0, -1.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]