Steering decision

1. Course Content

Note: This program runs on a sandbox map; you will need to adjust the program for your personal map.

1. Learn the actions the car takes after recognizing a right turn.
2. Learn the newly revealed key source code.

2. Right-click to access source code

Right-click source code path:

xxxxxxxxxx
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/yolo_detect.py

xxxxxxxxxx
/home/jetson/yahboomcar_ros2_ws/yahboomcar_ws/src/auto_drive/auto_drive/auto_drive.py

3. Program Startup

3.1 Startup Command

• Start camera + chassis + road signs and lane lines

xxxxxxxxxx
ros2 launch auto_drive auto_drive_bringup.launch.py

After the program runs, it will display the YOLO recognition screen. The car will run in the center of the lane line and perform corresponding actions based on the road signs it recognizes during the journey. (If the terminal doesn't report any errors after starting the program, but the robot doesn't move, you can run the following program to check the parameters.)

• Start the dynamic parameter configuration node

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

If the robot doesn't move, check two parameters: Is START_AutoDrive checked? Is conbine_nav false?

☑️ After modifying the parameters, click on the blank space in the GUI to enter the parameter values. Note that this will only take effect for the current startup; for permanent effects, you need to modify the parameters in the source code.

3.2 Recognizing the Car's Movement After a Right Turn

Note: Because the right turn uses a fixed action, the right turn and stop A road signs need to be placed in fixed positions.

The program will print the following when it recognizes a right turn:

The car will perform a fixed action to make a steering decision. Upon seeing a right turn, it will stop for one second, then proceed for five seconds before turning right.

The steering decision processing function is located in the auto_drive.py file. Below is the function that is entered after a right turn is detected. Users can modify it themselves to see how the car moves after recognizing a road sign.

xxxxxxxxxx
def turn(self,direction):
        '''Turn'''
        self.get_logger().info(f'Turn action preparing: {direction}, waiting 2 seconds...')
        
        self.turn_action_active = True  # Set steering action indicator
        self.get_logger().info(f'Turn action started: {direction}')
        self.__set_current_speed(linear_x=0.0, angular_z=0.0 )
        time.sleep(1)
​
        if self.R == 0:#Turning in place
            if direction == 'left':
                self.get_logger().info('turn left')
                self.__set_current_speed(angular_z=self.angular_speed )
            elif direction == 'right':
                self.get_logger().info('turn right')
                self.__set_current_speed(angular_z=-self.angular_speed  )
        else:#Turning with radius
            if direction == 'left':
                self.get_logger().info('turn left')
                self.__set_current_speed(linear_x=self.drive_speed, angular_z=self.drive_speed / self.R )
            elif direction == 'right':
                self.get_logger().info('turn right')
                self.__set_current_speed(linear_x=self.drive_speed, angular_z=-0.03)
                time.sleep(5)
                self.__set_current_speed(linear_x=self.drive_speed, angular_z=-0.8)
                time.sleep(self.duration)
        self.turn_action_active = False  # Clear the steering action sign

4. Source Code Analysis

4.1 auto_drive.py

YOLO signpost message reception:

xxxxxxxxxx
def sign_callback(self, msg:DetectionObject):
    '''Subscribe to YOLO recognition results'''
    class_name = msg.class_name
    object_distance = self._get_distance(msg.ymin)  # Calculate distance
    object = Object(msg.class_name, msg.confidence, object_distance)

Multiple filtering mechanisms: mainly to prevent continuously recognizing road signs and triggering actions; the action will only be unlocked when different road signs are recognized.

xxxxxxxxxx
# 1. Check if any other actions are being performed.
if self.action_runing:
    return
​
# 2. Check cooldown time (to prevent repeated triggering)
current_time = time.time()
if class_name in self.last_processed_time:
    time_diff = current_time - self.last_processed_time[class_name]
    if time_diff < self.cooldown_time:  # Default cooldown: 3 seconds
        return
​
# 3. Check if it is a duplicate road sign.
if class_name == self.last_sign:
    return
​
# 4. Check if the road sign is enabled.
if not self.sign_enable_list[对应的索引]:
    return

Event queue processing

xxxxxxxxxx
def handle_events(self):
    '''Handling target recognition events'''
    while True:
        if not self.event_queue.empty(): 
            object = self.event_queue.get()
            self.action_runing = True
            current_sign = object.class_name
            
            try:
                # Perform the appropriate action based on the type of road sign.
                if object.class_name == "straight" and self.sign_enable_list[10]:
                    self.go_straight()
                elif object.class_name == "turnright" and self.sign_enable_list[11]:
                    self.turn('right')
                # ... Other road sign processing
                
            finally:
                # Update status record
                self.last_processed_time[current_sign] = time.time()
                self.last_sign = current_sign
                self.action_runing = False

Specific action implementation

xxxxxxxxxx
def turn(self,direction):
        '''Turn'''
        self.get_logger().info(f'Turn action preparing: {direction}, waiting 2 seconds...')
        
        self.turn_action_active = True  # Set steering action indicator
        self.get_logger().info(f'Turn action started: {direction}')
        self.__set_current_speed(linear_x=0.0, angular_z=0.0 )
        time.sleep(1)
​
        if self.R == 0:#Turning in place
            if direction == 'left':
                self.get_logger().info('turn left')
                self.__set_current_speed(angular_z=self.angular_speed )
            elif direction == 'right':
                self.get_logger().info('turn right')
                self.__set_current_speed(angular_z=-self.angular_speed  )
        else:#Turning with radius
            if direction == 'left':
                self.get_logger().info('turn left')
                self.__set_current_speed(linear_x=self.drive_speed, angular_z=self.drive_speed / self.R )
            elif direction == 'right':
                self.get_logger().info('turn right')
                self.__set_current_speed(linear_x=self.drive_speed, angular_z=-0.03)
                time.sleep(5)
                self.__set_current_speed(linear_x=self.drive_speed, angular_z=-0.8)
                time.sleep(self.duration)
        self.turn_action_active = False  # Clear the steering action sign