class RPLidar(object):
def __init__(self,
port,
baudrate=115200,
timeout=2,
max_size_points=360,
max_size_frames=10):
self.serial_port = None # The actual serial port
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.is_connected = False
self.is_motor_running = False
self.is_scanning = False
# Raw data
# self.raw_points = Queue()
# self.raw_frames = Queue()
# self.test = Queue()
self._raw_points = list()
# Parsed data
# self.points = Queue(maxsize=max_size_points)
# self.frames = Queue(maxsize=max_size_frames)
# self.current_frame = RPLidarFrame()
self.rplidar_monitor = None
def connect(self):
if not self.is_connected:
try:
self.serial_port = serial.Serial(port=self.port,
baudrate=self.baudrate,
stopbits=serial.STOPBITS_ONE,
parity=serial.PARITY_NONE,
timeout=self.timeout)
self.stop_motor()
self.is_connected = True
except serial.SerialException as e:
# TOOD: Handle this
print 'error conecting'
def disconnect(self):
self._stop_monitor()
self.stop_scan()
if self.is_connected:
try:
if self.rplidar_monitor is not None:
self.stop_motor()
self.serial_port.close()
self.is_connected = False
except serial.SerialException as e:
print e.message
def start_motor(self):
self.serial_port.setDTR(False)
self.is_motor_running = True
def stop_motor(self):
if self.is_motor_running:
self.serial_port.setDTR(True)
self.is_motor_running = False
def get_health(self, timeout=2):
self.serial_port.flushInput()
self._send_command(RPLIDAR_CMD_GET_DEVICE_HEALTH)
if self._read_response_header(timeout) == RPLIDAR_ANS_TYPE_DEVHEALTH:
data = self.serial_port.read(
rplidar_response_device_health_format.sizeof())
parsed_data = rplidar_response_device_health_format.parse(data)
return {
"status": parsed_data.status,
"error_code": parsed_data.error_code
}
else:
pass
#TODO: raise an exception
def get_device_info(self, timeout=2):
if not self.is_connected:
pass
# TODO: Throw an exception
self.is_scanning = False
self.serial_port.flushInput()
self._send_command(RPLIDAR_CMD_GET_DEVICE_INFO)
if self._read_response_header(timeout) == RPLIDAR_ANS_TYPE_DEVINFO:
data = self.serial_port.read(
rplidar_response_device_info_format.sizeof())
parsed_data = rplidar_response_device_info_format.parse(data)
return {
"model": parsed_data.model,
"firmware_version_major": parsed_data.firmware_major,
"firmware_version_minor": parsed_data.firmware_minor,
"hardware_version": parsed_data.hardware,
"serial_number": to_hex(parsed_data.serial_number)
}
else:
pass
# TODO: exception
def start_scan(self):
self.rplidar_monitor = RPLidarMonitor(self)
self.rplidar_monitor.start()
def stop_scan(self):
self._send_command(RPLIDAR_CMD_STOP)
self.stop_motor()
time.sleep(0.2)
def _send_command(self, command):
# Build the command from the command format
data = rplidar_cmd_format.build(
Container(sync_byte=RPLIDAR_CMD_SYNC_BYTE, cmd_flag=command))
self.serial_port.write(data)
def _read_response_header(self, timeout=2):
start = time.time()
# Keep trying until the timeout is reached
while time.time() < start + timeout:
# If the number of bytes in the input buffer is less than
# the expected bytes from the response header, then we need to wait
if self.serial_port.inWaiting(
) < rplidar_response_header_format.sizeof():
time.sleep(0.010) # Wait 10ms and try again
else:
data = self.serial_port.read(
rplidar_response_header_format.sizeof())
parsed_data = rplidar_response_header_format.parse(data)
if (parsed_data.sync_byte_1 != RPLIDAR_ANS_SYNC_BYTE1
or parsed_data.sync_byte_2 != RPLIDAR_ANS_SYNC_BYTE2):
pass
# TODO: Raise an exception
else:
return parsed_data.type
# TODO: Raise exception if we get here
def _stop_scan(self):
if not self.is_connected:
pass
# TODO: exception
if not self.is_scanning:
return
self.stop_motor()
self._send_command(RPLIDAR_CMD_STOP)
time.sleep(0.1)
def _stop_monitor(self):
if self.rplidar_monitor is not None:
self.rplidar_monitor.join()
self.rplidar_monitor = None
class RPLidar(object):
def __init__(self, port, baudrate=115200, timeout=2, max_size_points=360, max_size_frames=10):
self.serial_port = None # The actual serial port
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.is_connected = False
self.is_motor_running = False
self.is_scanning = False
# Raw data
# self.raw_points = Queue()
# self.raw_frames = Queue()
# self.test = Queue()
self._raw_points = list()
# Parsed data
# self.points = Queue(maxsize=max_size_points)
# self.frames = Queue(maxsize=max_size_frames)
# self.current_frame = RPLidarFrame()
self.rplidar_monitor = None
def connect(self):
if not self.is_connected:
try:
self.serial_port = serial.Serial(port=self.port,
baudrate=self.baudrate,
stopbits=serial.STOPBITS_ONE,
parity=serial.PARITY_NONE,
timeout=self.timeout
)
self.stop_motor()
self.is_connected = True
except serial.SerialException as e:
# TOOD: Handle this
print 'error conecting'
def disconnect(self):
self._stop_monitor()
self.stop_scan()
if self.is_connected:
try:
if self.rplidar_monitor is not None:
self.stop_motor()
self.serial_port.close()
self.is_connected = False
except serial.SerialException as e:
print e.message
def start_motor(self):
self.serial_port.setDTR(False)
self.is_motor_running = True
def stop_motor(self):
if self.is_motor_running:
self.serial_port.setDTR(True)
self.is_motor_running = False
def get_health(self, timeout=2):
self.serial_port.flushInput()
self._send_command(RPLIDAR_CMD_GET_DEVICE_HEALTH)
if self._read_response_header(timeout) == RPLIDAR_ANS_TYPE_DEVHEALTH:
data = self.serial_port.read(rplidar_response_device_health_format.sizeof())
parsed_data = rplidar_response_device_health_format.parse(data)
return {
"status" : parsed_data.status,
"error_code" : parsed_data.error_code
}
else:
pass
#TODO: raise an exception
def get_device_info(self, timeout=2):
if not self.is_connected:
pass
# TODO: Throw an exception
self.is_scanning = False
self.serial_port.flushInput()
self._send_command(RPLIDAR_CMD_GET_DEVICE_INFO)
if self._read_response_header(timeout) == RPLIDAR_ANS_TYPE_DEVINFO:
data = self.serial_port.read(rplidar_response_device_info_format.sizeof())
parsed_data = rplidar_response_device_info_format.parse(data)
return {
"model" : parsed_data.model,
"firmware_version_major" : parsed_data.firmware_major,
"firmware_version_minor" : parsed_data.firmware_minor,
"hardware_version" : parsed_data.hardware,
"serial_number" : to_hex(parsed_data.serial_number)
}
else:
pass
# TODO: exception
def start_scan(self):
self.rplidar_monitor = RPLidarMonitor(self)
self.rplidar_monitor.start()
def stop_scan(self):
self._send_command(RPLIDAR_CMD_STOP)
self.stop_motor()
time.sleep(0.2)
def _send_command(self, command):
# Build the command from the command format
data = rplidar_cmd_format.build(Container(
sync_byte=RPLIDAR_CMD_SYNC_BYTE, cmd_flag=command))
self.serial_port.write(data)
def _read_response_header(self, timeout=2):
start = time.time()
# Keep trying until the timeout is reached
while time.time() < start + timeout:
# If the number of bytes in the input buffer is less than
# the expected bytes from the response header, then we need to wait
if self.serial_port.inWaiting() < rplidar_response_header_format.sizeof():
time.sleep(0.010) # Wait 10ms and try again
else:
data = self.serial_port.read(rplidar_response_header_format.sizeof())
parsed_data = rplidar_response_header_format.parse(data)
if(parsed_data.sync_byte_1 != RPLIDAR_ANS_SYNC_BYTE1 or
parsed_data.sync_byte_2 != RPLIDAR_ANS_SYNC_BYTE2):
pass
# TODO: Raise an exception
else:
return parsed_data.type
# TODO: Raise exception if we get here
def _stop_scan(self):
if not self.is_connected:
pass
# TODO: exception
if not self.is_scanning:
return
self.stop_motor()
self._send_command(RPLIDAR_CMD_STOP)
time.sleep(0.1)
def _stop_monitor(self):
if self.rplidar_monitor is not None:
self.rplidar_monitor.join()
self.rplidar_monitor = None
def start_scan(self):
self.rplidar_monitor = RPLidarMonitor(self)
self.rplidar_monitor.start()
def start_scan(self):
self.rplidar_monitor = RPLidarMonitor(self)
self.rplidar_monitor.start()
