Merge pull request #321 from QUT-Motorsport/dev/track-day

Dev/track day

src/control/controllers/controllers/node_vel_to_ackermann.py

@@ -29,7 +29,7 @@ def cmd_callback(self, twist_msg: Twist):
  steering = 0.0
  else:
  radius = vel / twist_msg.angular.z
- atan(self.wheelbase / radius) * (180 / pi) * self.Kp ## MAKE THIS A PARAM
+ steering = atan(self.wheelbase / radius) * (180 / pi) * self.Kp ## MAKE THIS A PARAM
 
  msg = AckermannDriveStamped()
  # make time for msg id

src/navigation/map_creation/config/cone_placement.yaml

@@ -2,6 +2,6 @@ cone_placement_node:
  ros__parameters:
  view_x: 25.0
  view_y: 25.0
- radius: 1.9
- min_detections: 7
+ radius: 2.0
+ min_detections: 9
  use_sim_time: False

src/navigation/nav_bringup/config/nav2_params.yaml

@@ -205,7 +205,7 @@ controller_server:
  # max_lookahead_dist: 5.0 # The maximum lookahead distance threshold when using velocity scaled lookahead distances.
  # lookahead_time: 2.0 # The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain.
  # rotate_to_heading_angular_vel: 1.8 # If rotate to heading is used, this is the angular velocity to use.
- transform_tolerance: 0.6 # The TF transform tolerance.
+ transform_tolerance: 0.4 # The TF transform tolerance.
  use_velocity_scaled_lookahead_dist: false # Whether to use the velocity scaled lookahead distances or constant lookahead_distance.
  min_approach_linear_velocity: 3.0 # The minimum velocity threshold to apply when approaching the goal.
  approach_velocity_scaling_dist: 1.0 # Integrated distance from end of transformed path at which to start applying velocity scaling.
@@ -247,8 +247,8 @@ controller_server:
  # cost_scaling_gain: 1.0 # A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within cost_scaling_dist.
  # inflation_cost_scaling_factor: 0.5 # The value of cost_scaling_factor set for the inflation layer in the local costmap.
  # The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values
- regulated_linear_scaling_min_radius: 5.0 # The turning radius for which the regulation features are triggered.
- regulated_linear_scaling_min_speed: 2.5 # The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature.
+ regulated_linear_scaling_min_radius: 7.0 # The turning radius for which the regulation features are triggered.
+ regulated_linear_scaling_min_speed: 3.0 # The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature.
  use_fixed_curvature_lookahead: true # Enable fixed lookahead for curvature detection. Useful for systems with long lookahead.
  curvature_lookahead_dist: 2.0 # Distance to lookahead to determine curvature for velocity regulation purposes if use_fixed_curvature_lookahead is enabled.
  use_rotate_to_heading: false # Whether to enable rotating to rough heading and goal orientation when using holonomic planners.

src/navigation/nav_bringup/config/slam_toolbox_params.yaml

@@ -23,51 +23,68 @@ 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.1 # Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode
- stack_size_to_use: 40000000 # The number of bytes to reset the stack size to, to enable serialization/deserialization of files
+ 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
 
- # Matcher Parameters
+ # 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
- scan_buffer_maximum_scan_distance: 50.0 # Maximum distance of a scan from the pose before removing the scan from the buffer
+ # 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: 1.0 # The threshold link matching algorithm response for fine resolution to pass
+ link_match_minimum_response_fine: 0.2 # The threshold link matching algorithm response for fine resolution to pass
  link_scan_maximum_distance: 10.0 # Maximum distance between linked scans to be valid
+
  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_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: 1.5 # The threshold response of the loop closure algorithm in coarse search to pass to refine
- loop_match_minimum_response_fine: 0.5 # The threshold response of the loop closure algorithm in fine 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_search_space_dimension: 2.5 # Search grid size to do scan correlation over
- correlation_search_space_resolution: 0.3 # Search grid resolution to do scan correlation over
- correlation_search_space_smear_deviation: 0.15 # Amount of multimodal smearing to smooth out responses
+ # Correlation Parameters - Correlation Parameters
+ 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.
+ correlation_search_space_smear_deviation: 0.05 # Amount of multimodal smearing to smooth out responses
+ # Needs to be between space_resolution/2 and space_dimension. Smaller=more responsive corrections, but more jittery
 
- loop_search_space_dimension: 8.0 # Size of the search grid over the loop closure algorithm
- loop_search_space_resolution: 1.0 # Amount of multimodal smearing to smooth out responses
- loop_search_space_smear_deviation: 0.9 # A penalty to apply to a matched scan as it differs from the odometric pose
+ # Correlation Parameters - Loop Closure Parameters
+ # Keep similar to correlation parameters, same reasons why
+ 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
 
- distance_variance_penalty: 0.5 # A penalty to apply to a matched scan as it differs from the odometric pose
- angle_variance_penalty: 0.6 # A penalty to apply to a matched scan as it differs from the odometric pose
+ # 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.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.00349 # Range of angles to test for fine scan matching
- coarse_search_angle_offset: 0.349 # Range of angles to test for coarse scan matching
- coarse_angle_resolution: 0.0349 # Resolution of angles over the Offset range to test in scan matching
- minimum_angle_penalty: 0.9 # Smallest penalty an angle can have to ensure the size doesn't blow up
- minimum_distance_penalty: 0.5 # Smallest penalty a scan can have to ensure the size doesn't blow up
+ 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: 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: 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.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/navigation/planners/config/ft_planner.yaml

@@ -16,7 +16,7 @@ ft_planner_node:
  mpc_path_length: 20
  mpc_prediction_horizon: 40
 
- min_track_width: 3.5
+ min_track_width: 3.3
  max_search_range: 5
- max_search_angle: 50
+ max_search_angle: 45
  matches_should_be_monotonic: True

src/operations/mission_controller/mission_controller/node_ebs_test_handler.py

@@ -99,6 +99,8 @@ def state_callback(self, msg: State):
  and self.odom_received
  ):
 
+ self.mission_started = True
+
  command = [
  "stdbuf",
  "-o",

src/operations/mission_controller/mission_controller/node_trackdrive_handler.py

@@ -144,7 +144,7 @@ def timer_callback(self):
 
  # check if we are within the bounds of the start line width (approx 2m)
  # and we are also within the distance of the finish line
- if abs(track_to_base.transform.translation.x) < 2 and abs(track_to_base.transform.translation.y) < 2:
+ if abs(track_to_base.transform.translation.x) < 2 and abs(track_to_base.transform.translation.y) < 3:
  self.in_box = True
  self.get_logger().info(f"In the starting box", throttle_duration_sec=1)
 
@@ -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

@@ -3,52 +3,31 @@
 source install/setup.bash
 
 # use clock from bag as sim time and start paused
-BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_05_21-05_25_56_mcap --clock -p"
-# BAG_CMD="ros2 bag play /mnt/e/rosbag2_2024_05_15-05_27_12 --clock -p"
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_11-05_28_15/ --clock -p"
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_18-05_47_36/ --clock -p" # pushed aroud
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_25-03_24_19/ --clock -p" # lidar on roll hoop test
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_25-05_04_46/ --clock -p" # half lap
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_07_25-05_18_42/ --clock -p" # other half of lap
+BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_09_04-05_14_43/ --clock -p" # 2.5 laps
+# BAG_CMD="ros2 bag play -s mcap /mnt/e/rosbag2_2024_09_24-06_08_59/ --clock -p" # 1 lap, loop close crashing
+
+REMAPPINGS=(
+ # /tf:=/tf_old
+ # /tf_static:=/tf_static_old
+ # /slam/occupancy_grid:=/slam/occupancy_grid_old
+ # /slam/car_pose:=/slam/car_pose_old
+ # /odom/filtered:=/odom/filtered_old
+)
 
-# list of topics we want to play
 TOPICS=(
- # Lidar and boundary mapping tuning
-
- # /lidar/cone_detection
- # /slam/global_map
- # /scan
- # /velodyne_points
- # /zed2i/zed_node/left_raw/image_raw_color
- # /zed2i/zed_node/left_raw/camera_info
- # /zed2i/zed_node/right_raw/image_raw_color
- # /zed2i/zed_node/right_raw/camera_info
- # /tf
- # /tf_static
- # /system/as_status
- # /zed2i/zed_node/odom
- # /odometry/sbg_ekf
- # /imu/odometry
-
- # 3D lidar slam testing
-
- # /zed2i/zed_node/odom
- # /velodyne_points
- # /odometry/sbg_ekf
- # /tf_static
- # /imu/data
- # /scan
-
- # VD data
-
- # /vehicle/steering_angle
- # /slam/car_pose
- # /imu/data
-
- # RL and SLAM toolbox
- /velodyne_points
+ /imu/odometry # needed for RL
+ /vehicle/wheel_twist # needed for RL
+ # /lidar/objects
+ /lidar/converted_2D_scan
  /lidar/cone_detection
- /vision/cone_detection
- # /scan
- /imu/odometry
- /imu/data
-
-
+ # /debug_markers/lidar_markers
+ /system/as_status # needed for lap = 0 in mapping
+ # /imu/nav_sat_fix # for visual
 )
 
 # remap these topics if you are running programs which output to the same topic names