4. Lidar Lesson

4.1 Lidar Introduction

4.1.1 Preface

Lidar is a remote sensing technology that uses laser beams to detect target positions, velocities, and other characteristics. It has the advantages of high measurement resolution, strong penetration ability, strong anti-interference ability, and strong anti-stealth ability.

According to the measurement principle, Lidar can be divided into three types: triangulation Lidar, pulse Lidar, and coherent Lidar. The Lidar equipped with MentorPi belongs to the pulse Lidar.

4.1.2 Lidar Principle

A Lidar system consists of a laser emission system, a scanning system, a laser reception system, and a signal processing system.

First, the laser emission system emits a detection signal (laser beam) towards the target, while the scanning system is responsible for scanning the plane to obtain the plane map information.

Next, the laser reception system receives the laser reflected from the target object and generates a reception signal.

Finally, the signal processing system processes the reception signal to obtain the characteristics of the target surface, physical properties, and other features, such as target location, height, and speed. Then, complete the construction of the object model.

4.1.3 Lidar Parameter

The specifications of the Lidar used by MentorPi are as follows:

Electrical and Mechanical Parameter

Parameter	Unit	Minimum value	Typical value	Maximum value	Note
Input voltage	V	4.5V	5V	5V
PWM control frequency KHz	20	30	50	Square signal
PWM high level	V	3	3	5
PWM low level	V	-0.3	0	0.5
PWM duty ratio	%	0	40	100	40% duty cycle with scanning frequency of 10Hz
Start-up current	mA	-	300	-
Working current	mA	-	180	-
Size	mm	54 * 46.29 * 34.8 (length * width * height)
Wight	g	-	47	-	Connection cables are not included
Communication interface	-	UART@230400
UART high level	V	2.9	3.3	3.5
UART low level	V	-0.3	0	0.4
Drive motor	-	BLDC	BLDC
Working temperature	℃	-10	25	40
Storage temperature	℃	-30	25	70

Optical Parameter

Parameter	Unit	Minimum value	Typical value	Maximum value	Note
Laser wavelength	nm	895	905	915	Infrared wavelength
Laser power	W	-	25	-	Peak power of laser diode; the actual power used is much lower than this value
Laser pulse width	ns	-	1	-
Laser safety class	-	IEC-60825 Class 1
Pitch angle	°	0	0.5	2

Performance Parameter

Parameter	Unit	Minimum value	Typical value	Maximum value	Note
Measuring range	m	0.02	-	12	70% target reflection coefficient
Scanning frequency	Hz	5	10	13	External PWM speed control
Ranging frequency	Hz	-	4500	-	Fixed frequency
Ranging accuracy	mm	-	-	-	When the ranging is less than 0.3m, there is output data. The trend of distance measurement data changes consistent with the trend of actual distance changes.
Ranging accuracy	mm	- 45	-	45	When the measuring range is between 300mm and 12000mm, the average of 100 measurements (70% diffuse surface).
Ranging standard deviation	mm	-	10	-	When the measuring range is between 300mm and 12000mm
Measurement resolution	mm	-	15	-
Angle error	°	-	-	2
Angle resolution	°	-	1	-
Anti-environmental light interference	KLux	-	-	30
Service life of the device	h	10000	-	-

Interface Instruction and Communication Protocol

LD19 uses ZH1.5T-4P 1.5mm connector to connect with external systems for power supply and data reception. The specific interface definition and parameter are shown in the table below:

No.	Signal	Type	Description	Minimum value	Typical value	Maximum value
1	Tx	Output	Lidar data output	0V	3.3V	3.5V
2	PWM	Input	Motor control signal	0V	-	3.3V
3	GND	Power supply	Power supply negative terminal	-	0V	-
4	P5V	Power supply	Power supply positive terminal	4.5V	5V	5.5V

Serial Specification

You can connect the Lidar to external systems through its physical interface. By following the communication protocol of the system, you can obtain real-time point cloud data, device information and device status from the laser scanning, and can also set the device working mode.

Baud Rate	Data Length	Stop Bit	Parity Bit	Flow Control
230400	8 Bits	1	None	None

4.2 Lidar Working and Ranging Principle

4.2.1 Lidar Ranging

Two common methods are employed by Lidar to determine the distance to a target: triangulation and Time of Flight (TOF).

In the case of TOF, as depicted in the diagram, the Lidar initially projects light onto the object. The object reflects the light directly back to the Lidar, which calculates the time it takes for the light to return. The distance between the object and the Lidar is then obtained by multiplying this time by the speed of light.

In triangulation, as illustrated in the diagram, the Lidar undergoes adjustments during manufacturing to ensure that the light does not directly strike the object. Instead, it is projected at a specific angle, a pre-set value that remains constant during operation. The distance from the object to the Lidar can be calculated by incorporating this angle into trigonometric functions.

4.2.2 Lidar Working Result

Its working result is as follows. Lidar will emit light and shine it on the object surface. When receiving the light reflected by the object, Lidar will mark the contour of the object at the position where the light is reflected.

4.3 Lidar Obstacle Avoidance

For instructions on app connection, please consult the tutorial located in “1. Getting Ready/ 1.6 APP Control”.

4.3.1 Enable Game

Initiate Lidar Game through App

(1) Connect the robot to “WonderPi” app.

(2) Click-on Lidar to navigate to the game interface.

(3) Switch on “Avoid obstacle” button to start the game.

Initiate Lidar Game Using Command

(1) Start the robot, and access the robot system desktop using VNC according to the tutorial saved in “Set Development Environment/1.VNC Installation and Connection”.

(2) Click-on to open the command line terminal.

(3) Execute the command and press Enter to deactivate the automatic start service for the app.

~/.stop_ros.sh

(4) Run the command to enable the local services for app-related game and chassis control services.

ros2 launch app lidar_node.launch.py debug:=true

(5) Open a new terminal, and execute the command. Then, press Enter to start the Lidar game.

ros2 service call /lidar_app/enter std_srvs/srv/Trigger {}

Note

The robot’s performance initiated by the app and command is identical.

(6) Enter the command and hit Enter to launch Lidar obstacle avoidance game.

ros2 service call /lidar_app/set_running interfaces/srv/SetInt64 "{data: 1}"

(7) If you want to exit the game, enter the command and press “Enter”.

ros2 service call /lidar_app/set_running interfaces/srv/SetInt64 "{data: 0}"

Note

The game will continue to run under the current Raspberry Pi power-on state if not exited. To avoid excessive use of the Raspberry Pi’s operating memory, please follow the above instructions to close the current game before executing other games.

(8) If you need to terminate the program, press short-cut “Ctrl+C” on the terminal opened in steps (4) and (5).

After experiencing the Lidar game, you can activate the app service either by using a command or restarting the robot. If the app service is not activated, related app functions will be disabled. In the case of a robot restart, the app service will start automatically.

Click and enter the command. Press enter to start the app, and wait for the buzzer to beep.

Note

please enter the command in the system path, not in the Docker container.

sudo systemctl restart start_node.service

4.3.2 Program Outcome

Let’s use a book as the object to be detected. In the function of Lidar following, the object to be detected needs to be higher than the scanning height of the Lidar. Only in this way the Lidar mounted on the MentorPi can effectively scan its position information. Then, the MentorPi moves forward. When the obstacle is detected, the MentorPi will automatically turn to avoid it.

4.3.3 Program Analysis

Launch File

The launch file is located in: /home/ubuntu/ros2_ws/src/app/launch/lidar_node.launch.py

(1) Set the storage path

Retrieve the paths for the two packages: peripherals and controller.

    if compiled == 'True':
        controller_package_path = get_package_share_directory('controller')
        peripherals_package_path = get_package_share_directory('peripherals')
    else:
        controller_package_path = '/home/ubuntu/ros2_ws/src/driver/controller'
        peripherals_package_path = '/home/ubuntu/ros2_ws/src/peripherals'

(2) Initiate other Launch files

    lidar_controller_node = GroupAction([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(peripherals_package_path, 'launch/lidar.launch.py')),
            condition=IfCondition(debug),
            ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(controller_package_path, 'launch/controller.launch.py')),
            condition=IfCondition(debug),
            ),

lidar.launch.py Lidar launch

controller.launch.py Motion control launch

(3) Initiate Node

        Node(
            package='app',
            executable='lidar_controller',
            output='screen',
            ),
    ])

Enable Lidar game node.

Source Code File

The source code of the program is saved in: /home/ubuntu/ros2_ws/src/app/app/lidar_controller.py

The process logic of the program based on the implemented effect is shown in the diagram:

(1) Initialization

class LidarController(Node):
    def __init__(self, name):
        rclpy.init()
        super().__init__(name)

        self.name = name
        self.running_mode = 0
        self.threshold = 0.6  # meters
        self.scan_angle = math.radians(360)  # radians
        self.speed = 0.2
        self.last_act = 0
        self.timestamp = 0
        self.angle_data = []
        # pid参数
        self.pid_yaw = pid.PID(1.6, 0, 0.16)
        self.pid_dist = pid.PID(1.7, 0, 0.16)
        self.lock = threading.RLock()
        self.lidar_sub = None
        self.lidar_type = os.environ.get('LIDAR_TYPE')
        self.machine_type = os.environ.get('MACHINE_TYPE')

self.name: stores the incoming name as the property of the object.

Other attributes include parameters for controlling the Lidar, such as operating mode, threshold, scanning angle, and speed.

self.lock: creates a thread lock for safe access in a multi-threaded environment.

(2) Create ROS service

        self.create_service(Trigger, '~/enter', self.enter_srv_callback)  # 进入玩法(enter the game)
        self.create_service(Trigger, '~/exit', self.exit_srv_callback)  # 退出玩法(exit the game)
        self.create_service(SetInt64, '~/set_running', self.set_running_srv_callback)  # 开启玩法(start the game)
        Heart(self, self.name + '/heartbeat', 5, lambda _: self.exit_srv_callback(request=Trigger.Request(), response=Trigger.Response()))  # 心跳包(heartbeat package)
        self.create_service(SetFloat64List, '~/set_param', self.set_parameters_srv_callback)  # 参数设置(set parameter)

Enter game: A ROS service named /enter is created with the type Trigger, and the callback function is enter_srv_callback. When this service is called, it will execute the enter_srv_callback function.

Exit game: A ROS service named /exit is created with the type Trigger, and the callback function is exit_srv_callback. When this service is called, it will execute the exit_srv_callback function.

Enable game: A ROS service named /set_running is created with the type SetInt64, and the callback function is set_running_srv_callback. When this service is called, it will execute the set_running_srv_callback function.

Set parameters: A ROS service named /set_param is created with the type SetFloat64List, and the callback function is set_parameters_srv_callback. When this service is called, it will execute the set_parameters_srv_callback function.

(3) Lidar obstacle avoidance

                if self.running_mode == 1 and self.timestamp <= time.time():
                    left_nonzero = left_range.nonzero()
                    right_nonzero = right_range.nonzero()
                    left_nonan = np.isfinite(left_range[left_nonzero])
                    right_nonan = np.isfinite(right_range[right_nonzero])
                    min_dist_left_ = left_range[left_nonzero][left_nonan]
                    min_dist_right_ = right_range[right_nonzero][right_nonan]
                    # 取左右最近的距离(Take the nearest distance left and right)
                    if len(min_dist_left_) > 1 and len(min_dist_right_) > 1:
                        min_dist_left = min_dist_left_.min()
                        min_dist_right = min_dist_right_.min()
                        if min_dist_left <= self.threshold and min_dist_right > self.threshold:  # 左侧有障碍(there is obstacle at left)
                            twist.linear.x = self.speed / 6  # 转弯时向前速度尽量小(keep the forward velocity as low as possible when turning)
                            max_angle = math.radians(90)  # 转90度(turn 90 degree)
                            w = self.speed * 6.0  # 转弯速度尽量大(keep the turning speed as high as possible)
                            twist.angular.z = -w
                            if self.last_act != 0 and self.last_act != 1:
                                twist.angular.z = w
                            self.last_act = 1
                            self.mecanum_pub.publish(twist)
                            self.timestamp = time.time() + (max_angle / w / 2)
                        elif min_dist_left <= self.threshold and min_dist_right <= self.threshold:  # 两侧都有障碍
                            twist.linear.x = self.speed / 6  # 转弯时向前速度尽量小(keep the forward velocity as low as possible when turning)
                            w = self.speed * 6.0  # 转弯速度尽量大(keep the turning speed as high as possible)
                            twist.angular.z = w
                            self.last_act = 3
                            self.mecanum_pub.publish(twist)
                            # 转180度
                            self.timestamp = time.time() + (math.radians(180) / w / 2)
                        elif min_dist_left > self.threshold and min_dist_right <= self.threshold:  # 右侧有障碍(there is obstacles at right)
                            twist.linear.x = self.speed / 6  # 转弯时向前速度尽量小(keep the forward velocity as low as possible when turning)
                            max_angle = math.radians(90)  # 转90度 turn 90 degrees
                            w = self.speed * 6.0  # 转弯速度尽量大(keep the turning speed as high as possible)
                            twist.angular.z = w
                            if self.last_act != 0 and self.last_act != 2:
                                twist.angular.z = -w
                            self.last_act = 2
                            self.mecanum_pub.publish(twist)
                            self.timestamp = time.time() + (max_angle / w / 2)
                        else:  # 没有障碍(none obstacle)

The code detects the minimum distances on the left and right sides of the robot, stored in variables min_dist_left and min_dist_right, respectively. Based on the relationship between the minimum distances and a threshold value self.threshold, the code determines the presence of obstacles on either side. If there is an obstacle on the left side, the robot will turn right by 90 degrees. If there are obstacles on both sides, the robot will turn around by 180 degrees. If there is an obstacle on the right side, the robot will turn left by 90 degrees. If there is no obstacle, the robot will continue moving forward.

To avoid repetitive turns, the code keeps track of the last action performed by the robot in the variable self.last_act.

4.4 Lidar Following

For instructions on app connection, please consult the tutorial located in “1.Getting Ready/1.6 APP Control”.

4.4.1 Enable Game

Initiate Lidar Game through App

(1) Connect the robot to “WondePi” app.

(2) Click-on Lidar to navigate to the game interface.

(3) Switch on “Lidar following” button to start the game.

4.1.2 Initiate Lidar Game Using Command

(1) Start the robot, and access the robot system desktop using VNC according to the tutorial saved in “Set Development Environment/2.1 VNC Installation and Connection”.

(2) Click-on to open the command line terminal.

(3) Execute the command and press Enter to deactivate the automatic start service for the app.

~/.stop_ros.sh

(4) Run the command to enable the local services for app-related game and chassis control services.

ros2 launch app lidar_node.launch.py debug:=true

(5) Open a new terminal, and execute the command. Then, press Enter to start the Lidar game.

ros2 service call /lidar_app/enter std_srvs/srv/Trigger {}

Note

The robot’s performance initiated by the app and command is identical.

(6) Enter the command and hit Enter to launch Lidar following game.

ros2 service call /lidar_app/set_running interfaces/srv/SetInt64 "{data: 2}"

(7) If you want to exit the game, enter the command and press “Enter”.

ros2 service call /lidar_app/set_running interfaces/srv/SetInt64 "{data: 0}"

Note

The game will continue to run under the current Raspberry Pi power-on state if not exited. To avoid excessive use of the Raspberry Pi’s operating memory, please follow the above instructions to close the current game before executing other games.

(8) If you need to terminate the program, press short-cut “Ctrl+C” on the terminal opened in steps 4) and 5).

After experiencing the Lidar game, you can activate the app service either by using a command or restarting the robot. If the app service is not activated, related app functions will be disabled. In the case of a robot restart, the app service will start automatically.

Click and enter the command. Press enter to start the app, and wait for the buzzer to beep.

sudo systemctl restart start_node.service

4.4.2 Program Outcome

Let’s use a book as the object to be detected. When using the Lidar following function, it’s important to ensure that the object to be detected is higher than the scanning height of the Lidar mounted on the MentorPi. This will enable the Lidar to effectively scan the position information of the object. Once the position information is obtained, the MentorPi will adjust its position to maintain a distance of around 0.35m between itself and the obstacle.

4.4.3 Program Analysis

Launch File

The launch file is located in: /home/ubuntu/ros2_ws/src/app/launch/lidar_node.launch.py

(1) Set the storage path

Retrieve the paths for the two packages: peripherals and controller.

    if compiled == 'True':
        controller_package_path = get_package_share_directory('controller')
        peripherals_package_path = get_package_share_directory('peripherals')
    else:
        controller_package_path = '/home/ubuntu/ros2_ws/src/driver/controller'
        peripherals_package_path = '/home/ubuntu/ros2_ws/src/peripherals'

(2) Initiate other Launch files

    lidar_controller_node = GroupAction([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(peripherals_package_path, 'launch/lidar.launch.py')),
            condition=IfCondition(debug),
            ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(controller_package_path, 'launch/controller.launch.py')),
            condition=IfCondition(debug),
            ),

lidar.launch.py Lidar launch

controller.launch.py Motion control launch

(3) Initiate Node

        Node(
            package='app',
            executable='lidar_controller',
            output='screen',
            ),
    ])

Enable Lidar game node.

Source Code File

The source code of the program is saved in: /home/ubuntu/ros2_ws/src/app/app/lidar_controller.py

The process logic of the program based on the implemented effect is shown in the diagram:

(1) Initialization

class LidarController(Node):
    def __init__(self, name):
        rclpy.init()
        super().__init__(name)

        self.name = name
        self.running_mode = 0
        self.threshold = 0.6  # meters
        self.scan_angle = math.radians(360)  # radians
        self.speed = 0.2
        self.last_act = 0
        self.timestamp = 0
        self.angle_data = []
        # pid参数
        self.pid_yaw = pid.PID(1.6, 0, 0.16)
        self.pid_dist = pid.PID(1.7, 0, 0.16)
        self.lock = threading.RLock()
        self.lidar_sub = None
        self.lidar_type = os.environ.get('LIDAR_TYPE')
        self.machine_type = os.environ.get('MACHINE_TYPE')

self.name: stores the incoming name as the property of the object.

Other attributes include parameters for controlling the Lidar, such as operating mode, threshold, scanning angle, and speed.

self.lock: creates a thread lock for safe access in a multi-threaded environment.

(2) Create ROS service

        self.create_service(Trigger, '~/enter', self.enter_srv_callback)  # 进入玩法(enter the game)
        self.create_service(Trigger, '~/exit', self.exit_srv_callback)  # 退出玩法(exit the game)
        self.create_service(SetInt64, '~/set_running', self.set_running_srv_callback)  # 开启玩法(start the game)
        Heart(self, self.name + '/heartbeat', 5, lambda _: self.exit_srv_callback(request=Trigger.Request(), response=Trigger.Response()))  # 心跳包(heartbeat package)

Enter game: A ROS service named /enter is created with the type Trigger, and the callback function is enter_srv_callback. When this service is called, it will execute the enter_srv_callback function.

Exit game: A ROS service named /exit is created with the type Trigger, and the callback function is exit_srv_callback. When this service is called, it will execute the exit_srv_callback function.

Enable game: A ROS service named /set_running is created with the type SetInt64, and the callback function is set_running_srv_callback. When this service is called, it will execute the set_running_srv_callback function.

Set parameters: A ROS service named /set_param is created with the type SetFloat64List, and the callback function is set_parameters_srv_callback. When this service is called, it will execute the set_parameters_srv_callback function.

(3) Lidar following

                    # 拼合距离数据, 从右半侧逆时针到左半侧(the merged distance data from right half counterclockwise to the left half)
                    ranges = np.append(right_range[::-1], left_range)
                    self.get_logger().info(str(ranges))
                    nonzero = ranges.nonzero()
                    nonan = np.isfinite(ranges[nonzero])
                    dist_ = ranges[nonzero][nonan]
                    if len(dist_) > 0:
                        dist = dist_.min()
                        min_index = list(ranges).index(dist)
                        angle = -angle + lidar_data.angle_increment * min_index  # 计算最小值对应的角度(calculate the angle corresponding to the minimum value)
                        # self.get_logger().info(str(dist_))
                        # self.get_logger().info(str([dist, angle]))
                        if dist < self.threshold and abs(math.degrees(angle)) > 5:  # 控制左右(control the left and right)
                            if self.lidar_type != 'G4':
                                self.pid_yaw.update(-angle)
                                twist.angular.z = common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) 
                            elif self.lidar_type == 'G4':
                                self.pid_yaw.update(angle)
                                twist.angular.z = -common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) 
                        else:
                            self.pid_yaw.clear()

                        if dist < self.threshold and abs(0.2 - dist) > 0.02:  # 控制前后(control the front and back)
                            self.pid_dist.update(self.threshold / 2 - dist)
                            twist.linear.x = common.set_range(self.pid_dist.output, -self.speed, self.speed)
                        else:
                            self.pid_dist.clear()
                        if abs(twist.angular.z) < 0.008:
                            twist.angular.z = 0.0
                        if abs(twist.linear.x) < 0.05:
                            twist.linear.x = 0.0
                        self.mecanum_pub.publish(twist)

The code concatenates the distance data from the left and right sides of the robot, obtained from the Lidar, into an array “ranges” to form a circular scan. It then calculates the minimum distance value “dist” and its corresponding angle “angle” from the “ranges”.

The “dist” is checked if it is less than a threshold value. If it is and the angle deviates from the forward direction by more than 5 degrees, the robot will rotate to avoid the obstacle. The angular velocity is controlled using a PID controller. If “dist” is less than the threshold and the robot is close to a distance of 0.2m in the forward direction, the robot will stop or move forward using a PID controller for the linear velocity. The control commands are published as a Twist message to the robot to execute the desired actions.

4.5 Lidar Guarding

For instructions on app connection, please consult the tutorial located in “1. Getting Ready/ 1.6. APP Control”.

4.5.1 Enable Game

Note

The MentorPi of Ackerman version does not support Lidar guarding function.

Initiate Lidar Game through App

(1) Connect the robot to “WondePi” app.

(2) Click-on Lidar to navigate to the game interface.

(3) Switch on “Lidar guarding” button to start the game.

5.1.2 Initiate Lidar Game Using Command

(1) Start the robot, and access the robot system desktop using VNC according to the tutorial saved in “Set Development Environment/1.VNC Installation and Connection”.

(2) Click-on to open the command line terminal.

(3) Execute the command and press Enter to deactivate the automatic start service for the app.

~/.stop_ros.sh

(4) Run the command to enable the local services for app-related game and chassis control services.

ros2 launch app lidar_node.launch.py debug:=true

(5) Open a new terminal, and execute the command. Then, press Enter to start the Lidar game.

ros2 service call /lidar_app/enter std_srvs/srv/Trigger {}

Note

The robot’s performance initiated by the app and command is identical.

(6) Enter the command and hit Enter to launch Lidar guarding game.

ros2 service call /lidar_app/set_running interfaces/srv/SetInt64 "{data: 3}"

(7) If you want to exit the game, enter the command and press “Enter”.

ros2 service call /lidar_app/set_running interfaces/srv/SetInt64 "{data: 0}"

Note

The game will continue to run under the current Raspberry Pi power-on state if not exited. To avoid excessive use of the Raspberry Pi’s operating memory, please follow the above instructions to close the current game before executing other games.

(8) If you need to terminate the program, press short-cut “Ctrl+C” on the terminal opened in steps (4) and (5).

After experiencing the Lidar game, you can activate the app service either by using a command or restarting the robot. If the app service is not activated, related app functions will be disabled. In the case of a robot restart, the app service will start automatically.

Click and enter the command. Press enter to start the app, and wait for the buzzer to beep.

Note

please enter the command in the system path, not in the Docker container.

sudo systemctl restart start_node.service

4.5.2 Program Outcome

Let’s use a book as the object to be detected. When using the Lidar following function, it’s important to ensure that the object to be detected is higher than the scanning height of the Lidar mounted on the MentorPi. This will enable the Lidar to effectively scan the position information of the object. Once the position information is obtained, the MentorPi will adjust its orientation to face towards the obstacle. It means that the camera will face directly towards the obstacle.

4.5.3 Program Analysis

Launch File

The launch file is located in: /home/ubuntu/ros2_ws/src/app/launch/lidar_node.launch.py

(1) Set the storage path

Retrieve the paths for the two packages: peripherals and controller.

    if compiled == 'True':
        controller_package_path = get_package_share_directory('controller')
        peripherals_package_path = get_package_share_directory('peripherals')
    else:
        controller_package_path = '/home/ubuntu/ros2_ws/src/driver/controller'
        peripherals_package_path = '/home/ubuntu/ros2_ws/src/peripherals'

(2) Initiate other Launch files

    lidar_controller_node = GroupAction([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(peripherals_package_path, 'launch/lidar.launch.py')),
            condition=IfCondition(debug),
            ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(controller_package_path, 'launch/controller.launch.py')),
            condition=IfCondition(debug),
            ),

lidar.launch.py Lidar launch

controller.launch.py Motion control launch

(3) Initiate Node

        Node(
            package='app',
            executable='lidar_controller',
            output='screen',
            ),
    ])

Enable Lidar game node.

Source Code File

The source code of the program is saved in:/home/ubuntu/ros2_ws/src/app/app/lidar_controller.py

The process logic of the program based on the implemented effect is shown in the diagram:

(1) Initialization

class LidarController(Node):
    def __init__(self, name):
        rclpy.init()
        super().__init__(name)

        self.name = name
        self.running_mode = 0
        self.threshold = 0.6  # meters
        self.scan_angle = math.radians(360)  # radians
        self.speed = 0.2
        self.last_act = 0
        self.timestamp = 0
        self.angle_data = []
        # pid参数
        self.pid_yaw = pid.PID(1.6, 0, 0.16)
        self.pid_dist = pid.PID(1.7, 0, 0.16)
        self.lock = threading.RLock()
        self.lidar_sub = None
        self.lidar_type = os.environ.get('LIDAR_TYPE')
        self.machine_type = os.environ.get('MACHINE_TYPE')

self.name: stores the incoming name as the property of the object.

Other attributes include parameters for controlling the Lidar, such as operating mode, threshold, scanning angle, and speed.

self.lock: creates a thread lock for safe access in a multi-threaded environment.

(2) Create ROS service

        self.create_service(Trigger, '~/enter', self.enter_srv_callback)  # 进入玩法(enter the game)
        self.create_service(Trigger, '~/exit', self.exit_srv_callback)  # 退出玩法(exit the game)
        self.create_service(SetInt64, '~/set_running', self.set_running_srv_callback)  # 开启玩法(start the game)
        Heart(self, self.name + '/heartbeat', 5, lambda _: self.exit_srv_callback(request=Trigger.Request(), response=Trigger.Response()))  # 心跳包(heartbeat package)
        self.create_service(SetFloat64List, '~/set_param', self.set_parameters_srv_callback)  # 参数设置(set parameter)

Enter game: A ROS service named /enter is created with the type Trigger, and the callback function is enter_srv_callback. When this service is called, it will execute the enter_srv_callback function.

Exit game: A ROS service named /exit is created with the type Trigger, and the callback function is exit_srv_callback. When this service is called, it will execute the exit_srv_callback function.

Enable game: A ROS service named /set_running is created with the type SetInt64, and the callback function is set_running_srv_callback. When this service is called, it will execute the set_running_srv_callback function.

Set parameters: A ROS service named /set_param is created with the type SetFloat64List, and the callback function is set_parameters_srv_callback. When this service is called, it will execute the set_parameters_srv_callback function.

(3) Lidar guarding

                elif self.running_mode == 3:
                    # 拼合距离数据, 从右半侧逆时针到左半侧(the merged distance data from right half counterclockwise to the left half)
                    ranges = np.append(right_range[::-1], left_range)
                    nonzero = ranges.nonzero()
                    nonan = np.isfinite(ranges[nonzero])
                    dist_ = ranges[nonzero][nonan]
                    if len(dist_) > 1:
                        dist = dist_.min()
                        min_index = list(ranges).index(dist)
                        
                        angle = -angle + lidar_data.angle_increment * min_index  # 计算最小值对应的角度(calculate the angle corresponding to the minimum value)
                        if dist < self.threshold and abs(math.degrees(angle)) > 5:  # 控制左右(control the left and right)
                            if self.lidar_type != 'G4':
                                self.pid_yaw.update(-angle)
                                twist.angular.z = common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0)
                            elif self.lidar_type == 'G4':
                                self.pid_yaw.update(angle)
                                twist.angular.z = -common.set_range(self.pid_yaw.output, -self.speed * 6.0, self.speed * 6.0) 
                        else:
                            self.pid_yaw.clear()
                        if abs(twist.angular.z) < 0.008:
                            twist.angular.z = 0.0
                        self.mecanum_pub.publish(twist)

The code concatenates the distance data from the left and right sides of the robot, obtained from the Lidar, into an array ranges to form a circular scan. It then calculates the minimum distance value dist and its corresponding angle angle from the ranges array.

If dist is less than a threshold value and the angle deviates from the forward direction by more than 5 degrees, the Yaw axis PID controller will be activated. Based on the output of the PID controller, the code calculates the turning angular velocity twist.angular.z. The control commands are published as a Twist message to the robot to execute the desired actions.