-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathWrover-Basic (REV B).py
284 lines (240 loc) · 13.5 KB
/
Wrover-Basic (REV B).py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
"""
Title: Enhanced Autonomous Navigation and Obstacle Avoidance System for
Waveshare Rovers with Advanced Sensory Integration. REV B
About:
This script is at the heart of an advanced autonomous navigation and obstacle avoidance system designed for Waveshare Rovers. It intricately
integrates LiDAR (FHL-LD19) for spatial mapping and obstacle detection, alongside sophisticated analysis of light intensity to enhance object
recognition and environmental understanding. With a Raspberry Pi 4 as the central processor, the system establishes serial communications with
the rover and LiDAR sensor, processing real-time spatial and light intensity data for intelligent maneuvering in diverse environments. The system's
core capabilities include dynamic obstacle detection, strategic navigation decision-making based on comprehensive sensory data, and
execution of refined movement commands. Enhancements cover improved maneuverability, precise obstacle engagement through light intensity
analysis, and a robust error handling framework, making the system ideal for a broad spectrum of educational, developmental, and research applications.
Features:
- Advanced obstacle detection using LiDAR and light intensity data.
- Strategic navigation decision-making with enhanced sensory integration.
- Robust serial communication with rover and LiDAR sensor.
- Light intensity analysis for improved environmental perception.
- Comprehensive error handling and system resilience.
- Modular design for ease of customization and extension.
Requirements:
- Python 3.x
- pyserial package for robust serial communication.
Disclaimer:
This software is provided 'as-is' without warranty, expressed or implied. The authors disclaim all liabilities for damages resulting from its use.
Permission is granted under the MIT License for use, modification, and distribution. Designed for educational and developmental purposes only,
the creators assume no responsibility for its use in operational environments.
License:
Released under the MIT License.
Copyright (c) <2024> <FigTroniX>
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software
without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
"""
import serial
import struct
import json
import time
# ================================================
# Configuration and Constants
# ================================================
# Rover connection settings
ROVER_SERIAL_PORT = '/dev/serial0'
ROVER_BAUD_RATE = 1000000
# Lidar connection settings
LIDAR_SERIAL_PORT = '/dev/ttyUSB0'
LIDAR_BAUD_RATE = 230400
# Define a safety distance (in millimeters)
SAFETY_DISTANCE = 950
# Enhanced Movement and Speed Definitions
# These values should be adjusted based on testing to ensure effective maneuvering
# Movement commands and safety distance definition
COMMANDS = {
"FORWARD": {"T": 1, "L": 200, "R": 200}, # Normal forward, adjust speed as needed
"REVERSE": {"T": 1, "L": -225, "R": -225}, # Fast reverse, adjust speed as needed
"TURN_LEFT": {"T": 1, "L": -255, "R": 255}, # Sharp left turn, negative value for left motor, positive for right
"TURN_RIGHT": {"T": 1, "L": 255, "R": -255}, # Sharp right turn, positive value for left motor, negative for right
"STOP": {"T": 0} # Emergency stop
}
# ================================================
# Utility Functions
# ================================================
def open_serial_connection(port, baud_rate):
"""Attempts to open a serial connection to a given port with specified baud rate."""
try:
return serial.Serial(port, baud_rate, timeout=1)
except serial.SerialException as e:
print(f"Failed to open serial port {port}: {e}")
return None
def send_command_to_rover(serial_conn, command):
"""Sends a JSON-formatted command to the rover over a serial connection."""
command_str = json.dumps(command) + '\n'
try:
serial_conn.write(command_str.encode())
except serial.SerialException as e:
print(f"Failed to send command to rover: {e}")
# ================================================
# LiDAR Processing
# ================================================
def CalCRC8(data):
"""Calculate the CRC8 for the given data using the CrcTable."""
# CRC8 table for LD19 LiDAR data validation, predefined as per device specifications.
CrcTable = [
0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c,
0xa9, 0xe4, 0x33, 0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8, 0xf5,
0x3a, 0x77, 0xa0, 0xed, 0x43, 0x0e, 0xd9, 0x94, 0xc8, 0x85, 0x52, 0x1f, 0xb1, 0xfc, 0x2b, 0x66,
0x88, 0xc5, 0x12, 0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99, 0xd4,
0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14, 0x59, 0xf7, 0xba, 0x6d, 0x20,
0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36, 0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89,
0x63, 0x2e, 0xf9, 0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72, 0x3f,
0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2, 0xef, 0x41, 0x0c, 0xdb, 0x96,
0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1, 0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e,
0xeb, 0xa6, 0x71, 0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa, 0xb7,
0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35, 0x78, 0xd6, 0x9b, 0x4c, 0x01,
0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17, 0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8
]
crc = 0
for byte in data:
crc = CrcTable[(crc ^ byte) & 0xFF]
return crc
def find_packet_start(serial_port):
"""Read bytes one at a time until the packet start header (0x54) is found."""
while True:
byte = serial_port.read(1)
if byte == b'\x54': # Header byte for packet start
return byte # Return the header to be included in the packet
def read_packet(serial_port):
"""Read a complete packet dynamically, based on header detection."""
packet = find_packet_start(serial_port) # Include the packet start
packet += serial_port.read(21) # Example: Read the next 21 bytes; adjust based on your packet structure
return packet
def parse_lidar_packet(packet):
"""
Parses a LiDAR data packet, extracting and processing distance and intensity information.
Adjusts for the LiDAR's orientation to ensure navigation decisions align with the rover's perspective.
"""
try:
# Unpack the packet header, version+length, and speed
header, ver_len, speed = struct.unpack_from('<BBH', packet, 0)
start_angle, = struct.unpack_from('<H', packet, 4)
end_angle, timestamp, crc_check = struct.unpack_from('<HHB', packet, len(packet) - 5)
num_points = ver_len & 0x1F # Lower five bits of ver_len indicate the number of points
closest_distance = float('inf')
closest_intensity = 0
closest_angle_relative_to_rover = None
decision = "FORWARD"
for i in range(num_points):
offset = 6 + i * 3
if offset + 3 <= len(packet) - 5:
distance, intensity = struct.unpack_from('<HB', packet, offset)
angle_increment = (end_angle - start_angle) / max((num_points - 1), 1)
angle = (start_angle + angle_increment * i) % 36000 # Angle in 0.01 degrees
# Adjust for LiDAR's orientation. Assuming a 90-degree (9000 in 0.01 degrees) offset.
# This makes forward direction of LiDAR align with rover's forward direction.
adjusted_angle = (angle + 9000) % 36000
# Process data based on adjusted angles
if 0 < distance < closest_distance:
closest_distance = distance
closest_intensity = intensity
closest_angle_relative_to_rover = adjusted_angle
# Decision making based on closest obstacle's data
if closest_distance < SAFETY_DISTANCE:
# Depending on where the closest obstacle is, make a turn or stop
if closest_intensity > 20: # High intensity implies a potentially dangerous obstacle
if 9000 <= closest_angle_relative_to_rover <= 27000:
decision = "TURN_LEFT" # Obstacle is more on the right side
else:
decision = "TURN_RIGHT" # Obstacle is more on the left side
else:
decision = "STOP" # For lower intensity, stop as a precaution
return decision
except struct.error as e:
print(f"Error parsing packet: {e}")
return "STOP"
def determine_dynamic_action(closest_angle, lidar_data):
"""
Decides whether to reverse, turn left, or turn right based on the direction with the furthest distance
from obstacles. This function assumes access to more comprehensive LiDAR data to make this decision.
Parameters:
- closest_angle: The angle of the closest detected obstacle.
- lidar_data: A collection of distances measured by the LiDAR, indexed by their angle.
Returns:
- A string indicating the chosen action ("REVERSE", "TURN_LEFT", or "TURN_RIGHT").
"""
# Sample angles for left, right, and rear directions
# These values should be adjusted based on the specific orientation and FOV of your LiDAR
left_angle = (closest_angle + 90) % 360
right_angle = (closest_angle - 90) % 360
rear_angle = (closest_angle + 180) % 360
# Retrieve the distances for left, right, and directly behind the rover
left_distance = lidar_data.get(left_angle, 0)
right_distance = lidar_data.get(right_angle, 0)
rear_distance = lidar_data.get(rear_angle, 0)
# Determine the safest direction based on available distances
if left_distance > right_distance and left_distance > rear_distance:
return "TURN_LEFT"
elif right_distance > left_distance and right_distance > rear_distance:
return "TURN_RIGHT"
else:
# If reversing has more space or if left/right distances are too close, choose to reverse
# Additional logic can be added here to handle cases where all directions are blocked
return "REVERSE"
def check_for_obstacles(lidar_conn):
packet = read_packet(lidar_conn)
if packet:
return parse_lidar_packet(packet)
return False
def lidar_processing_thread(lidar_conn, action_queue):
"""Processes LiDAR data in a separate thread, detecting obstacles and deciding movement actions."""
while True:
try:
action = check_for_obstacles(lidar_conn)
if action:
action_queue.put(action)
except Exception as e:
print(f"Error in LiDAR processing: {e}")
def rover_control_thread(rover_conn, action_queue):
"""Controls the rover based on actions decided by the LiDAR processing thread."""
while True:
try:
if not action_queue.empty():
action = action_queue.get()
#print(f"Action decided: {action}")
send_command_to_rover(rover_conn, COMMANDS[action])
#print(f"Command sent: {COMMANDS[action]}")
#time.sleep(0.1)
except Exception as e:
print(f"Error in rover control: {e}")
# ================================================
# Main Logic
# ================================================
def main():
"""Main function to initialize connections and control the rover based on LiDAR data."""
rover_conn = open_serial_connection(ROVER_SERIAL_PORT, ROVER_BAUD_RATE)
lidar_conn = open_serial_connection(LIDAR_SERIAL_PORT, LIDAR_BAUD_RATE)
if not rover_conn or not lidar_conn:
print("Unable to open serial connections. Exiting.")
return
try:
while True:
action = check_for_obstacles(lidar_conn)
if action:
#print(f"Action decided: {action}")
send_command_to_rover(rover_conn, COMMANDS[action])
#print(f"Command sent: {COMMANDS[action]}")
time.sleep(0.05)
except KeyboardInterrupt:
print("Program terminated by user.")
except Exception as e:
print(f"An unexpected error occurred: {e}")
finally:
if rover_conn:
rover_conn.close()
if lidar_conn:
lidar_conn.close()
print("Connections closed.")
if __name__ == "__main__":
main()