Welcome to ROSMASTER R2 repository

2、Fun gameplay with radar obstacle avoidance

1、Program function description

After the program starts, the car will move forward, and when there is an obstacle within the detection range, it will adjust its attitude, avoid the obstacle, and then continue to walk forward. If the controller node is activated, the R2 key of the controller can pause/enable this function.

2、Program code reference path

Raspberry Pi PI5 master needs to enter the docker container first, Orin motherboard does not need to enter,

the location of the source code of this function is located at,

x
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/laser_Avoidance_a1_R2.py
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/laser_Avoidance_4ROS_R2.py

The A1 radar has the same architecture as the S2 radar and can be shared.

3、The program starts

3.1、start the command

After entering the docker container, according to the actual model and radar model, the terminal input,

xxxxxxxxxx
#Start the trolley chassis
ros2 run yahboomcar_bringup Ackman_driver_R2
#Start the A1 radar
ros2 launch sllidar_ros2 sllidar_launch.py
#Start the 4ros radar
ros2 launch ydlidar_ros2_driver ydlidar_launch.py
#Start the radar obstacle avoidance program R2 model, A1 radar
ros2 run yahboomcar_laser laser_Avoidance_a1_R2
#Start the radar obstacle avoidance program R2 model, 4ROS radar
ros2 run yahboomcar_laser laser_Avoidance_4ROS_R2
#Start the handle, if needed
ros2 run yahboomcar_ctrl yahboom_joy_R2
ros2 run joy joy_node

3.2、View the topic communication node diagram

docker terminal input,

xxxxxxxxxx
ros2 run rqt_graph rqt_graph 

It is also possible to set the size of parameters, terminal input, and terminal input through the dynamic parameter adjuster,

xxxxxxxxxx
ros2 run rqt_reconfigure rqt_reconfigure

The meaning of each parameter is as follows,

The above parameters are adjustable, except for Switc, the other four need to be set when they need to be decimal, after modification, click on the blank space to write.

4、Core source code analysis

Taking the X3 model, the source code of the A1 radar as an example, mainly looking at the callback function of the radar, here explains how to obtain the obstacle distance information of each angle.

​x
def registerScan(self, scan_data):
    if not isinstance(scan_data, LaserScan): return
    ranges = np.array(scan_data.ranges)
    self.Right_warning = 0
    self.Left_warning = 0
    self.front_warning = 0

    for i in range(len(ranges)):
        angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
        #The angle of radar information is a radian system, and here it is converted into an angle for calculation
        if 160 > angle > 180 - self.LaserAngle:#The angle is based on the structure of the radar to set the judgment range
            if ranges[i] < self.ResponseDist*1.5: 
        #range[i] is the result of radar scanning, which in this case refers to distance information
                self.Right_warning += 1
        if - 160 < angle < self.LaserAngle - 180:
            if ranges[i] < self.ResponseDist*1.5:
                self.Left_warning += 1
        if abs(angle) > 160:
            if ranges[i] <= self.ResponseDist*1.5:
                self.front_warning += 1
        if self.Joy_active or self.Switch == True:
            if self.Moving == True:
                self.pub_vel.publish(Twist())
                self.Moving = not self.Moving
                    return
        self.Moving = True