Patrol Gameplay

1. Course Content

• Run the sample program, set the desired patrol route in the dynamic parameter adjuster, and then click start. The car will move according to the set patrol route. At the same time, the radar on the car will scan for obstacles within the set radar angle and obstacle detection distance. If an obstacle is found, it will stop and the buzzer will sound. If there are no obstacles, it will continue to patrol.

2. Start the Agent

Note: If it is already running, you do not need to start it again.

Enter the command in the vehicle's terminal:

start_agent

The terminal will print the following information, indicating a successful connection:

3. Run the Example

3.1 Start the Program

For Raspberry Pi 5 users, if the ROS container is not started, you need to start it first,

xxxxxxxxxx
bringup_m3

Open a terminal inside the container,

xxxxxxxxxx
exec_m3

• Run the patrol program node:

xxxxxxxxxx
ros2 launch m3_bringup patrol.launch.py

• Click the patrol node in the left options bar of the rqt_reconfigure panel. If there are no nodes on the left when you first start, click the Refresh button below.

[!TIP]

rqt_reconfigure panel parameter description:

• odom_frame: Name of the odometry coordinate frame.
• base_frame: Name of the base coordinate frame.
• circle_adjust: When the patrol route is a circle, this value can be used as a coefficient to adjust the size of the circle. See the code for details.
• Switch: Gameplay switch.
• Command: Patrol route, with the following options: [LengthTest]-straight line patrol, [Circle]-circle patrol, [Square]-square patrol, [Triangle]-triangle patrol.
• Set_loop: Switch to re-patrol. When set, it will continue to patrol in a loop according to the set route.
• ResponseDist: Obstacle detection distance.
• LaserAngle: Radar detection angle.
• Linear: Linear velocity magnitude.
• Angular: Angular velocity magnitude.
• Length: Distance for straight-line movement.
• RotationTolerance: Allowable tolerance for rotation error.
• RotationScaling: Proportional coefficient for rotation.

• [Command] is the set patrol route. Here, we take the straight-line patrol route as an example. The available patrol routes are described below.
• After setting the route in [Command], click Switch to start patrolling. The terminal will print the following information:

• If there is an obstacle on the patrol path, the robot will stop and display obstacles.

4. Source Code Analysis

Source code path:

xxxxxxxxxx
~/yahboomM3_ws/code_ws/src/m3_common_demo/m3_common_demo/patrol/patrol.py

4.1 View Node Relationship Graph

Open a terminal and enter the command:

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

In the node relationship diagram above:

• patrol is the key node for implementing the patrol function. This node subscribes to the coordinate transformation between odometry odom and base_footprint, and subscribes to /scan1 lidar data to determine if there are obstacles ahead.
• YB_Node: The chassis node, which publishes lidar data /scan, raw odometry data /odom_raw, and subscribes to the /cmd_vel topic to control the robot chassis movement via inverse kinematics.
• ekf_filter_node: An extended Kalman filter used to fuse raw odometry and filtered IMU data, publishing the fused /odom odometry data.
• imu_filter: The IMU filter node, which filters the raw IMU data /imu/data_raw and publishes the filtered /imu/data data.

4.2 Program Flowchart

4.3 Key Program

Below is an explanation of the core parts of the program:

Movement State Acquisition: Listens to the TF transform between odom and base_footprint to calculate the current XY coordinates and rotation angle.

Program Implementation: The get_position and get_odom_angle methods in the YahboomCarPatrol class.

​x
def get_position(self):
    try:
        now = rclpy.time.Time()
        trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)
        return trans
    except (LookupException, ConnectivityException, ExtrapolationException):
        self.get_logger().info('transform not ready')
        raise
        return
 
def get_odom_angle(self):
     try:
        now = rclpy.time.Time()
        rot = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)   
        #print("oring_rot: ",rot.transform.rotation) 
        cacl_rot = PyKDL.Rotation.Quaternion(rot.transform.rotation.x, rot.transform.rotation.y, rot.transform.rotation.z, rot.transform.rotation.w)
        #print("cacl_rot: ",cacl_rot)
        angle_rot = cacl_rot.GetRPY()[2]
​
​
     except (LookupException, ConnectivityException, ExtrapolationException):
        self.get_logger().info('transform not ready')
        return
​
     return angle_rot    

Movement Control: The basic actions for the robot chassis movement. All patrol routes are combinations of straight lines and rotations.

Program Implementation: The advancing and Spin methods in the YahboomCarPatrol class.

xxxxxxxxxx
def advancing(self,target_distance):
    self.position.x = self.get_position().transform.translation.x
    self.position.y = self.get_position().transform.translation.y
    move_cmd = Twist()
    self.distance = sqrt(pow((self.position.x - self.x_start), 2) +
                        pow((self.position.y - self.y_start), 2))
    self.distance *= self.LineScaling
    self.get_logger().info(f"distance: {self.distance}")         
    self.error = self.distance - target_distance
    move_cmd.linear.x = self.Linear
    if abs(self.error) < self.LineTolerance : 
        self.get_logger().info("stop")             
        self.distance = 0.0
        self.pub_cmdVel.publish(Twist())
        self.x_start = self.position.x;
        self.y_start = self.position.y;
        self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
        all_new_parameters = [self.Switch]
        self.set_parameters(all_new_parameters)
        return True
    else:
        if self.Joy_active or self.front_warning > 10:
            if self.moving == True:
                self.pub_cmdVel.publish(Twist())
                self.moving = False
                self.get_logger().info("obstacles")                     
        else:
            #print("Go")
            self.pub_cmdVel.publish(move_cmd)
        self.moving = True
        return False
​
    
def Spin(self,angle):
    self.target_angle = radians(angle)
    self.odom_angle = self.get_odom_angle()
    self.delta_angle = self.RotationScaling * self.normalize_angle(self.odom_angle - self.last_angle)
    self.turn_angle += self.delta_angle
    # print("turn_angle: ",self.turn_angle)
    self.get_logger().info(f"turn_angle: {self.turn_angle}")         
    self.error = self.target_angle - self.turn_angle
    self.get_logger().info(f"error: {self.error}")         
    self.last_angle = self.odom_angle
    move_cmd = Twist()
    if abs(self.error) < self.RotationTolerance or self.Switch==False :
        self.pub_cmdVel.publish(Twist())
        self.turn_angle = 0.0
        '''self.Switch  = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
        all_new_parameters = [self.Switch]
        self.set_parameters(all_new_parameters)'''
        return True
    if self.Joy_active or self.front_warning > 10:
        if self.moving == True:
            self.pub_cmdVel.publish(Twist())
            self.moving = False
            self.get_logger().info("obstacles")
    else:
        if self.Command == "Square" or self.Command == "Triangle": 
            #move_cmd.linear.x = 0.2
            move_cmd.angular.z = copysign(self.Angular, self.error)
        elif self.Command == "Circle":
            length = self.Linear * self.circle_adjust / self.Length
            #print("length: ",length)
            move_cmd.linear.x = self.Linear
            move_cmd.angular.z = copysign(length, self.error)
            #print("angular: ",move_cmd.angular.z)
            '''move_cmd.linear.x = 0.2
            move_cmd.angular.z = copysign(2, self.error)'''
        self.pub_cmdVel.publish(move_cmd)
    self.moving = True