Obstacle Avoidance and AMCL Localization

Overview

Combine reactive obstacle avoidance with probabilistic localization. You will implement a simple lidar-based avoidance loop and then initialize Adaptive Monte Carlo Localization (AMCL) on a pre-built map to maintain pose estimates while navigating.

Prerequisites

• Completed IMU Processing with a 1D Kalman Filter
• 2D occupancy grid map (PGM/YAML pair) for your test track
• Lidar publishing /scan at ≥10 Hz
• The roboracer publishing velocity on the topic /cmd_vel

Learning Objectives

• Students can implement a complete autonomous behavior and use a standard algorithm to know where their car is.
• Generate steering commands that keep the vehicle clear of nearby obstacles
• Configure move_base-free AMCL for lightweight localization
• Validate particle filter convergence against ground truth markers

1. Implement Reactive Obstacle Avoidance

1. Create a ROS 2 node obstacle_avoidance_node.py:

   import math
   import rclpy
   from rclpy.node import Node
   from sensor_msgs.msg import LaserScan
   from geometry_msgs.msg import Twist
   
   class ObstacleAvoid(Node):
       def __init__(self):
           super().__init__('obstacle_avoid')
           self.declare_parameter('min_range', 0.5)
           self.declare_parameter('forward_speed', 0.5)
           self.declare_parameter('turn_gain', 1.5)
           self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
           self.sub = self.create_subscription(LaserScan, '/scan', self.scan_cb, 10)
   
       def scan_cb(self, scan: LaserScan):
           min_r = self.get_parameter('min_range').value
           forward = self.get_parameter('forward_speed').value
           turn_gain = self.get_parameter('turn_gain').value
           ranges = scan.ranges
           left = min(ranges[len(ranges)//2:])
           right = min(ranges[:len(ranges)//2])
           cmd = Twist()
           cmd.linear.x = forward if min(left, right) > min_r else 0.0
           cmd.angular.z = turn_gain * (right - left)
           self.pub.publish(cmd)
   
   rclpy.init()
   node = ObstacleAvoid()
   rclpy.spin(node)
   

2. Adjust min_range to maintain a comfortable buffer.
3. Test in simulation first (ros2 launch fri_sim simple_world.launch.py).

2. Prepare the Map and TF Tree

1. Copy your map files into fri_navigation/maps/track.yaml and track.pgm.
2. Ensure TF broadcasts exist for map → odom → base_link. Confirm with ros2 run tf2_tools view_frames.
3. Set the initial pose guess in map_server launch file.

3. Launch AMCL

1. Configure amcl.yaml:

   use_map_topic: false
   min_particles: 200
   max_particles: 2000
   laser_z_hit: 0.5
   laser_z_rand: 0.2
   update_min_a: 0.2
   update_min_d: 0.1
   

2. Launch stack:

   ros2 launch fri_navigation localization.launch.py map:=track.yaml
   

3. Monitor /amcl_pose and /particlecloud in Foxglove.

4. Integrate Avoidance with Localization

1. Run obstacle_avoidance_node.py concurrently with AMCL.
2. Watch the vehicle maintain pose while reacting to obstacles.
3. Log /cmd_vel, /amcl_pose, and /scan for tuning.

5. Evaluate Performance

• Drive a loop and compare /amcl_pose to ground truth markers using rviz2 or manual measurements.
• Check localization covariance; it should shrink once particles converge.
• Increase particle counts if the map has dense obstacles; reduce if CPU is constrained.

6. Troubleshooting

• Divergence suggests TF mismatches or inaccurate sensor model; re-check frame IDs.
• If the robot stalls, widen min_range or add smoothing via a PID controller.
• Use ros2 topic hz to ensure /scan rates stay consistent.

Wrap-Up

Commit the avoidance node and AMCL configuration files. Record localization errors for baseline metrics.