function.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding:utf-8 -*-
00003 """
00004 Copyright (c) 2017 Xu Zhihao (Howe).  All rights reserved.
00005 
00006 This program is free software; you can redistribute it and/or modify
00007 """
00008 from reference import *
00009 import time
00010 import rospy
00011 import list_ports_linux
00012 
00013 ranges_default = []
00014 for i in range(360):
00015     ranges_default.append(float('inf'))
00016 
00017 def rplidar_points(port, raw_data):
00018     while port.inWaiting() < response_device_point_format.sizeof():
00019         time.sleep(0.001)
00020     _str = port.read(response_device_point_format.sizeof())
00021     raw_data.append(_str)
00022 
00023 def stop_device(port):
00024     cmd = stop
00025     send_command(port, cmd)
00026     port.setDTR(1)
00027     # port.close()
00028 
00029 def send_command(port, com):
00030     rospy.loginfo('sending commands')
00031     command = com
00032     cmd = command_format.build(Container(sync_byte=sync_byte, cmd_flag=command))
00033     port.write(cmd)
00034 
00035 def header_check(port):
00036     rospy.loginfo('evaluating header')
00037     stamp = time.time()
00038     time_out = 1
00039     while time.time() < stamp + time_out:
00040         if port.inWaiting() < response_header_format.sizeof():
00041             time.sleep(0.01)
00042         else:
00043             _str = port.read(response_header_format.sizeof())
00044             response_str = response_header_format.parse(_str)
00045             if response_str.sync_byte1 != sync_byte1 or response_str.sync_byte2 != sync_byte2:
00046                 rospy.logerr('unexpect response header')
00047             return response_str.response_type
00048     rospy.loginfo("time out")
00049     return None
00050 
00051 def device_health(port):
00052     cmd = get_device_health
00053     send_command(port, cmd)
00054     if header_check(port) == devhealth:
00055         _str = port.readline(response_device_health_format.sizeof())
00056         response_str = response_device_health_format.parse(_str)
00057         return response_str
00058     else:
00059         rospy.logwarn('command for devhealth error or return value error')
00060         return None
00061 
00062 def driver_reset(port):
00063     cmd = reset
00064     send_command(port, cmd)
00065     port.setDTR(1)
00066     time.sleep(0.01)
00067 
00068 def OutputCoordinate(raw):
00069     response = response_device_point_format.parse(raw)
00070     inten = response.quality.quality
00071     angular = (response.angle_q6 >> angle_shift) / 64.0
00072     angle = round(angular)
00073     if response.distance_q2 != 0:
00074         dis = response.distance_q2 / 4.0 / 1000.0
00075     else:
00076         dis = float('inf')
00077     return [angle, dis, inten]
00078 
00079 def range_matrix():
00080     global ranges_default
00081     return [i for i in ranges_default]
00082 
00083 def range_matrix():
00084     global ranges_default
00085     return [i for i in ranges_default]
00086 
00087 def intensive_matrix():
00088     global ranges_default
00089     return [0 for i in ranges_default]
00090 
00091 def port_finder(trigger, port_name):
00092     ports = list(list_ports_linux.comports())
00093     for port in ports:
00094         if port_name in port[1]:
00095             trigger = True
00096             rospy.logwarn('find rplidar connect on port: %s' % port[0])
00097             return [port, trigger]
00098     return [None, trigger]
00099 
00100 def fusion(origion_PolorCoordinate, current_PolorCoordinate):
00101     return round((origion_PolorCoordinate[0]*origion_PolorCoordinate[1] + current_PolorCoordinate[1]*current_PolorCoordinate[2])/float(current_PolorCoordinate[2] + origion_PolorCoordinate[1]), 4)


rplidar_python
Author(s): DinnerHowe
autogenerated on Thu Jun 6 2019 21:53:33