Last tunings before track - will be able to run multiple laps

src/navigation/nav_bringup/config/slam_toolbox_params.yaml

@@ -23,42 +23,42 @@ slam_toolbox_node:
  debug_logging: false # Change logger to debug
  throttle_scans: 1 # Number of scans to throttle in synchronous mode
  transform_publish_period: 0.05 # The map to odom transform publish period. 0 will not publish transforms
- map_update_interval: 2.0 # Interval to update the 2D occupancy map for other applications / visualization
+ map_update_interval: 0.1 # Interval to update the 2D occupancy map for other applications / visualization
  enable_interactive_mode: false # Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode
  position_covariance_scale: 2.0 # This can be used to tune the influence of the pose position in a downstream localization filter
  yaw_covariance_scale: 1.5 # Pose yaw
  resolution: 0.1 # Resolution of the 2D occupancy map to generate
  max_laser_range: 25.0 # Maximum laser range to use for 2D occupancy map rastering
  minimum_time_interval: 0.1 # The minimum duration of time between scans to be processed in synchronous mode
  transform_timeout: 0.5 # TF timeout for looking up transforms
- tf_buffer_duration: 0.2 # Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode
+ tf_buffer_duration: 1.0 # Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode
  stack_size_to_use: 500000000 # 500MB # The number of bytes to reset the stack size to, to enable serialization/deserialization of files
 
  # General Parameters
  use_scan_matching: true # Whether to use scan matching to refine odometric pose
  use_scan_barycenter: false # Whether to use the barycenter or scan pose
 
  minimum_travel_distance: 0.1 # Minimum distance of travel before processing a new scan
- minimum_travel_heading: 0.1 # Minimum changing in heading to justify an update
+ minimum_travel_heading: 0.05 # Minimum changing in heading to justify an update
 
  scan_buffer_size: 1000 # The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode
  # Approx 500 in pit track, 800 in skidpad track 
  scan_buffer_maximum_scan_distance: 100.0 # Maximum distance of a scan from the pose before removing the scan from the buffer
  # Skidpad track approx 50m diam
 
  link_match_minimum_response_fine: 0.2 # The threshold link matching algorithm response for fine resolution to pass
- link_scan_maximum_distance: 5.0 # Maximum distance between linked scans to be valid
+ link_scan_maximum_distance: 10.0 # Maximum distance between linked scans to be valid
 
- loop_search_maximum_distance: 5.0 # Maximum threshold of distance for scans to be considered for loop closure
+ loop_search_maximum_distance: 10.0 # Maximum threshold of distance for scans to be considered for loop closure
  do_loop_closing: true # Whether to do loop closure
 
- loop_match_minimum_chain_size: 30 # The minimum chain length of scans to look for loop closure
- loop_match_maximum_variance_coarse: 2.0 # The threshold variance in coarse search to pass to refine
- loop_match_minimum_response_coarse: 0.6 # The threshold response of the loop closure algorithm in coarse search to pass to refine
- loop_match_minimum_response_fine: 0.3 # The threshold response of the loop closure algorithm in fine search to pass to refine
+ loop_match_minimum_chain_size: 50 # The minimum chain length of scans to look for loop closure
+ loop_match_maximum_variance_coarse: 5.0 # The threshold variance in coarse search to pass to refine
+ loop_match_minimum_response_coarse: 0.3 # The threshold response of the loop closure algorithm in coarse search to pass to refine
+ loop_match_minimum_response_fine: 0.2 # The threshold response of the loop closure algorithm in fine search to pass to refine
 
  # Correlation Parameters - Correlation Parameters
- correlation_search_space_dimension: 40.0 # Search grid size to do scan correlation over
+ correlation_search_space_dimension: 20.0 # Search grid size to do scan correlation over
  # Needs to be a multiple of space_resolution. Increasing improves scan matching (not sure if this is in meters or cells)
  correlation_search_space_resolution: 0.1 # Search grid resolution to do scan correlation over
  # Closer to map resolution (line 30) is better. > 1 goes crazy. 1 = odom tracking only. 
@@ -67,24 +67,24 @@ slam_toolbox_node:
 
  # Correlation Parameters - Loop Closure Parameters
  # Keep similar to correlation parameters, same reasons why
- loop_search_space_dimension: 80.0 # Size of the search grid over the loop closure algorithm
- loop_search_space_resolution: 0.2 # Search grid resolution to do loop closure over
+ loop_search_space_dimension: 20.0 # Size of the search grid over the loop closure algorithm
+ loop_search_space_resolution: 0.1 # Search grid resolution to do loop closure over
  loop_search_space_smear_deviation: 0.1 # Amount of multimodal smearing to smooth out responses
 
  # Scan Matcher Parameters
  distance_variance_penalty: 0.3 # A penalty to apply to a matched scan as it differs from the odometric pose
  # Increasing improves scan matching 
- angle_variance_penalty: 0.2 # A penalty to apply to a matched scan as it differs from the odometric pose
+ angle_variance_penalty: 0.3 # A penalty to apply to a matched scan as it differs from the odometric pose
  # Increasing improves scan matching
 
- fine_search_angle_offset: 0.00700 # Range of angles to test for fine scan matching
+ fine_search_angle_offset: 0.01400 # Range of angles to test for fine scan matching
  # Increasing improves scan matching, allows more angles to be tested
- coarse_search_angle_offset: 0.700 # Range of angles to test for coarse scan matching
+ coarse_search_angle_offset: 1.400 # Range of angles to test for coarse scan matching
  # Increasing improves scan matching
  coarse_angle_resolution: 0.0700 # Resolution of angles over the Offset range to test in scan matching
  # Increasing improves scan matching
- minimum_angle_penalty: 0.3 # Smallest penalty an angle can have to ensure the size doesn't blow up
+ minimum_angle_penalty: 1.3 # Smallest penalty an angle can have to ensure the size doesn't blow up
  # Decreasing improves scan matching, lets more scans be considered
- minimum_distance_penalty: 0.3 # Smallest penalty a scan can have to ensure the size doesn't blow up
+ minimum_distance_penalty: 0.9 # Smallest penalty a scan can have to ensure the size doesn't blow up
  # Decreasing improves scan matching, lets more scans be considered
  use_response_expansion: true # Whether to automatically increase the search grid size if no viable match is found

src/navigation/nav_bringup/launch/slam_toolbox.launch.py

@@ -31,7 +31,6 @@ def generate_launch_description():
  os.path.join(pkg_share, "config/slam_toolbox_params.yaml"),
  ],
  remappings=[
- ("/pose", "/slam/car_pose"),
  ("/slam_toolbox/graph_visualization", "/slam/graph_visualisation"),
  ],
  )

src/operations/mission_controller/mission_controller/node_trackdrive_handler.py

@@ -163,7 +163,7 @@ def timer_callback(self):
 
  self.laps += 1
  self.lap_trig_pub.publish(UInt8(data=self.laps - 1))
- self.get_logger().info(f"Lap {self.laps} completed")
+ self.get_logger().info(f"Lap {self.laps-1} completed")
 
  # we have finished lap "1"
  if self.laps > 1:

tools/play_bag.sh

@@ -22,7 +22,7 @@ REMAPPINGS=(
 TOPICS=(
  /imu/odometry # needed for RL
  /vehicle/wheel_twist # needed for RL
- # /velodyne_points
+ # /lidar/objects
  /lidar/converted_2D_scan
  /lidar/cone_detection
  # /debug_markers/lidar_markers