def run():
plt.ion()
laser = HokuyoLX()
ax = plt.subplot(111, projection='polar')
plot = ax.plot([], [], '.')[0]
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def run():
plt.ion()
laser = HokuyoLX(tsync = False)
laser.convert_time = False
ax = plt.subplot(111, projection='polar')
plot = ax.scatter([0, 1], [0, 1], s=5, c=[IMIN, IMAX], cmap=plt.cm.Reds, lw=0)
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def run():
plt.ion()
laser = HokuyoLX(tsync=False)
laser.convert_time = False
ax = plt.subplot(111, projection='polar')
plot = ax.scatter([0, 1], [0, 1], s=10, c='b', lw=0)
text = plt.text(0, 1, '', transform=ax.transAxes)
ax.set_rmax(DMAX)
ax.grid(True)
images.append((plot, ))
plt.show()
while plt.get_fignums():
update(laser, plot, text)
laser.close()
def generateSampleDist():
lidar = HokuyoLX()
_, data = lidar.get_filtered_dist()
data = data.tolist()
fileName = input("Enter a file name: ")
with open(str(fileName) + ".txt", 'w') as pw:
for i in range(len(data)):
pw.write(str(data[i][0]) + "," + str(data[i][1]))
if i != len(data) - 1:
pw.write("\n")
pw.close()
lidar.close()
def __init__(self, lidar_on=True,small=True):
sensors_number=6
self.sensor_range = 20
self.collision_avoidance = False
if small:
self.sensors_map = {0:(0, np.pi/3),1: (np.pi*0.5, np.pi*1.5),2: (5/3.*np.pi,2*np.pi),3: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]}
#self.sensors_map= {0: (0, np.pi/3), 1: (np.pi/4, np.pi*7/12), 2: (np.pi*0.5, np.pi*1.5), 3: (17/12.*np.pi, 7/4.*np.pi), 4: (5/3.*np.pi,2*np.pi), 5: [(7/4.*np.pi,2*np.pi),(0,np.pi*1/4.)]} # can be problem with 2pi and 0
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
if small:
self.coords = Array('d',[850, 170, 3*np.pi / 2])
else:
self.coords = Array('d', [170, 170, 0])
self.localisation = Value('b', True)
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500, sense_noise=25, distance_noise=45, angle_noise=0.2, in_x=self.coords[0],
in_y=self.coords[1], in_angle=self.coords[2],input_queue=self.input_queue, out_queue=self.loc_queue)
# driver process
self.dr = driver.Driver(self.input_queue,self.fsm_queue,self.loc_queue)
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,args=(self.localisation,self.coords,self.get_raw_lidar))
logging.info(self.send_command('echo','ECHO'))
logging.info(self.send_command('setCoordinates',[self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]]))
self.p2.start()
time.sleep(0.1)
save_svg()
save_csv()
# plt.ion()
pos_x = 0
pos_y = 0
step_index = 2
color_index = 0
orientation = 0
dataset = []
plots = []
laser = HokuyoLX()
fig, ax = plt.subplots()
arrow_obj = Arrow(pos_x,
pos_y,
1000 * math.sin(orientation),
1000 * math.cos(orientation),
width=200,
color="red")
arrow = ax.add_patch(arrow_obj)
text = plt.text(0,
1,
'x: %d y: %d step = %d' % (pos_x, pos_y, STEPS[step_index]),
transform=ax.transAxes)
ax.set_xlim(-DMAX, DMAX)
ax.set_ylim(-DMAX, DMAX)
ax.grid(True)
parser.add_argument("-l", "--log", help="Print debug info ")
parser.add_argument("-m", "--map", help="Send osc msg in map mode")
args = parser.parse_args()
if args.log == 'ON':
debug_ON = True
if args.map == 'OFF':
map_ON = False
# Start the system.
osc_startup()
# Make client channels to send packets.
osc_udp_client(osc_host_ip, int(osc_host_port), "osc")
try:
laser = HokuyoLX(addr=(str(sensor_ip), int(sensor_port)),
info=False,
buf=1024,
timeout=300,
time_tolerance=1000,
convert_time=False)
print("Connect " + str(sensor_ip) + "\n")
except Exception as e:
print(e)
print("[ERR]Sensor connect failed " + time.strftime("%X") + "@" +
sensor_ip + "\n")
errFlag = True
while True:
update()
def __init__(self,
lidar_on=True,
color='yellow',
sen_noise=20,
angle_noise=0.2,
dist_noise=45,
init_coord=[170, 170, 0]):
self.color = color
# collision settings
self.collision_avoidance = True
self.sensor_range = 60
self.map = np.load('npmap.npy')
self.sensors_places = [
np.pi / 2, np.pi / 2, 0, np.pi, 3 * np.pi / 2, 3 * np.pi / 2, 0,
np.pi, 0, 0
] # DIRECTION OF SENSORS
self.sensors_map = {
0: (np.pi / 6, np.pi / 2 + np.pi / 3),
1: (np.pi / 2 - np.pi / 3, np.pi * 5 / 6),
2: (0.0, 0.0 + np.pi / 3),
3: (np.pi - np.pi / 3, np.pi + np.pi / 3),
4: (3 * np.pi / 2 - np.pi / 3, 11 * np.pi / 6),
5: (np.pi - np.pi / 6, 3 * np.pi / 2 + np.pi / 3),
6: (0.0, 0.0 + np.pi / 3),
7: (np.pi - np.pi / 3, np.pi + np.pi / 3), # 8 IR sensors
8: (2 * np.pi - np.pi / 3, 2 * np.pi),
9: (2 * np.pi - np.pi / 3, 2 * np.pi)
} # doubling values
#6: (3*np.pi/2 + np.pi/4, 2*np.pi), 7:(np.pi/2-np.pi/3, np.pi*5/6): (np.pi)# 6 ir sensors ENDED
#7:(0, np.pi/4), 8:(3*np.pi/2 + np.pi/4, 2*np.pi), 9:(np.pi - np.pi/3, np.pi + np.pi/3)} # 2 us sensors
#can be problems ith 2*np.pi and 0
self.lidar_on = lidar_on
self.collision_d = 9
self.coll_go = False
# localisation settings
self.localisation = Value('b', True)
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
#self.x = 170 # mm
#self.y = 150 # mm
#self.angle = 0.0 # pi
driver.PORT_SNR = '325936843235' # need change
# for test
x1, x2, y1, y2 = 1000, 2500, 600, 1100
dx, dy = x2 - x1, y2 - y1
angle = np.pi / 2
start = [x1, y1, angle]
#
self.coords = Array('d', rev_field(init_coord, self.color)) # 170, 170
#self.coords = Array('d',rev_field([1000, 1100, np.pi],self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue()
self.PF = pf.ParticleFilter(particles=1500,
sense_noise=sen_noise,
distance_noise=dist_noise,
angle_noise=angle_noise,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
self.dr = driver.Driver(self.input_queue,
self.fsm_queue,
self.loc_queue,
device=get_device_name())
self.p = Process(target=self.dr.run)
self.p.start()
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
self.p2.start()
self.init_manipulators()
time.sleep(0.1)
logging.info("Robot __init__ done!")
def __init__(self):
self.lidar = HokuyoLX()
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
"""
from hokuyolx import HokuyoLX # For UST10LX
laser = HokuyoLX(addr=('192.168.0.10', 10940))
timestamps, scan = laser.get_filtered_dist()
scan = scan.tolist()
keys = [0] * len(scan)
for i in range(len(scan)):
keys[i] = scan[i][0]
print keys
def __init__(self, lidar_on=True, small=True, color='yellow'):
# Cylinder Staff
self.cylinders = 0
self.cyl_prepare = [0.215, 0.143, 0.143]
self.cyl_up = [0.523, 0.64, 0.66]
self.cyl_down = [-0.79, -0.67, -0.72]
self.coll_go = False
self.useLift = False
self.collision_belts = False
##################
self.color = color
self.sensor_range = 35
self.collision_d = 9
self.coll_ind = -1
self.collision_avoidance = True
self.localisation = Value('b', True)
if small:
self.sensors_places = [
0, 0, 0, np.pi, np.pi / 2, 3 * np.pi / 2, 0, 0, 0
]
self.sensors_map = {
0: (0, np.pi / 3),
7: (7 * np.pi / 4, 2 * np.pi),
3: (np.pi * 0.7, np.pi * 1.3),
1: (5 / 3. * np.pi, 2 * np.pi),
2: (0, np.pi * 1 / 4.),
6: (7 / 4. * np.pi, 2 * np.pi),
8: (0, np.pi / 4),
4: (np.pi / 4, 3 * np.pi / 4),
5: (np.pi * 5 / 4, 7 * np.pi / 4)
}
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
# self.x = 170 # mm
# self.y = 150 # mm
# self.angle = 0.0 # pi
if small:
#850 170 3p/2
#
self.coords = Array(
'd', rev_field([170, 170, 3 * np.pi / 2], self.color))
else:
driver.PORT_SNR = '325936843235' # need change
self.coords = Array('d', rev_field([170, 170, 0], self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue() # 2000,25,25,0.1
self.PF = pf.ParticleFilter(particles=2000,
sense_noise=25,
distance_noise=25,
angle_noise=0.1,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# driver process
print "Paricle filter On"
self.dr = driver.Driver(self.input_queue, self.fsm_queue,
self.loc_queue)
print "Driver On"
self.p = Process(target=self.dr.run)
print "Process driver Init"
self.p.start()
print "Process driver Start"
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
print "Process npParticle Init"
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
if self.lidar_on:
self.p2.start()
print "Process npParticle Start"
else:
self.localisation = Value('b', False)
#time.sleep(0.1)
logging.info(self.send_command('rotate_cylinder_vertical', [146.0]))
#if self.color == "yellow":
# self.rotate_cylinder_vertical()
#else:
# self.rotate_cylinder_horizonal()
print "Robot __init__ done!"
def __init__(self, ip="192.168.1.10", port=10940):
self.lidar = HokuyoLX(addr=(ip, port))
self.kalman = KalmanFilter(adaptive=False, dt=0.025)
def __init__(self):
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
self.x = 0
self.y = 0
def write_conf():
global sensor_ip
global sensor_port
global osc_host_ip
global osc_host_port
global area_left
global area_right
global map_left
global map_right
global blob_size_threshold
global marker_angual_interval
global marker_distance_interval
global laser
global errFlag
cf = configparser.ConfigParser()
sensor_ip = sVar_sensor_ip.get()
sensor_port = sVar_sensor_port.get()
osc_host_ip = sVar_osc_host_ip.get()
osc_host_port = sVar_osc_host_port.get()
area_left = sVar_area_left.get()
area_right = sVar_area_right.get()
area_far = sVar_area_far.get()
area_near = sVar_area_near.get()
map_left = sVar_map_left.get()
map_right = sVar_map_right.get()
map_near = sVar_map_near.get()
map_far = sVar_map_far.get()
marker_angual_interval = sVar_marker_angual_interval.get()
marker_distance_interval = sVar_marker_distance_interval.get()
blob_size_threshold = sVar_blob_size_threshold.get()
cf.read("config.conf")
cf.set("SENSOR", "sensor_ip", sensor_ip)
cf.set("SENSOR", "sensor_port", str(sensor_port))
cf.set("OSC", "OSC_host_ip", osc_host_ip)
cf.set("OSC", "OSC_host_port", str(osc_host_port))
cf.set("AREA", "area_left", str(area_left))
cf.set("AREA", "area_right", str(area_right))
cf.set("AREA", "area_near", str(area_near))
cf.set("AREA", "area_far", str(area_far))
cf.set("AREA", "map_near", str(map_near))
cf.set("AREA", "map_far", str(map_far))
cf.set("AREA", "map_left", str(map_left))
cf.set("AREA", "map_right", str(map_right))
cf.set("MARKER", "marker_distance_interval", str(marker_distance_interval))
cf.set("MARKER", "marker_angual_interval", str(marker_angual_interval))
cf.set("MARKER", "blob_size_threshold", str(blob_size_threshold))
# TO DO
#osc_terminate()
#osc_startup()
#osc_udp_client(str(osc_host_ip), int(osc_host_port), "osc")
try:
laser.close()
laser = HokuyoLX(addr=(str(sensor_ip), int(sensor_port)),
info=False,
buf=1024,
time_tolerance=1000,
convert_time=False)
msg_text.insert(1.0,
"[MSG]Sensor connected " + time.strftime("%X") + "\n")
msg_text.tag_remove("RED", "1.0")
errFlag = False
except Exception as e:
msg_text.insert(
1.0, "[ERR]Sensor connect failed " + time.strftime("%X") + "\n")
msg_text.tag_add("RED", "1.0")
errFlag = True
with open('config.conf', 'w') as configfile:
cf.write(configfile)
read_conf()
def __init__():
self.lidar = HokuyoLX()
def wall_position(debug=False):
# if debugging is active then create a tkinter client and display
# the laser scan and calculations
if debug:
top = tk.Tk()
# Create label to show the image on
label = tk.Label(top)
label.pack()
# Create a figure to plot the results on
f = Figure(figsize=(5, 4), dpi=100)
# Create the plot
ax = f.add_subplot(111)
ax.plot([])
# Add the plot to the client
canvas = FigureCanvasTkAgg(f, master=top)
canvas.get_tk_widget().pack()
canvas.draw()
# If not debugging create a dummy ax and canvas
else:
ax = None
canvas = None
# Create the hokuyo laser object
laser = HokuyoLX()
# Create the publisher
pub = rospy.Publisher('wall_position', Float32MultiArray, queue_size=1)
ang_pub = rospy.Publisher('ang_pub', PoseStamped, queue_size=1)
wall_pub = rospy.Publisher('wall_pub', PoseStamped, queue_size=1)
pos_pub = rospy.Publisher('pos_pub', PointStamped, queue_size=1)
est_pub = rospy.Publisher('est_pub', PoseStamped, queue_size=1)
# Create the node
rospy.init_node('wall', anonymous=True)
# Set the rate. Not sure what this should be
rate = rospy.Rate(50)
# Keep the loop alive
while not rospy.is_shutdown():
# Obtain a laser measurement
x, y, angle, ax, canvas, x_est, y_est, yaw_est, valid = main_laser(
laser, ax, canvas, debug)
# Convert it to the ros array
msg = Float32MultiArray()
msg.data = [x, y, (angle), valid]
# Publish the data
pub.publish(msg)
# Est msg
est_msg = PoseStamped()
est_msg.header.stamp = rospy.Time.now()
est_msg.header.frame_id = "drone"
est_msg.pose.position.x = y_est * 0.001
est_msg.pose.position.y = x_est * 0.001
est_msg.pose.position.z = 0
q = tf.transformations.quaternion_from_euler(0, 0, (yaw_est) *
3.14159 / 180.0)
est_msg.pose.orientation.x = q[0]
est_msg.pose.orientation.y = q[1]
est_msg.pose.orientation.z = q[2]
est_msg.pose.orientation.w = q[3]
est_pub.publish(est_msg)
# Ang msg
ang_msg = PoseStamped()
ang_msg.header.stamp = rospy.Time.now()
ang_msg.header.frame_id = "drone"
ang_msg.pose.position.x = 0
ang_msg.pose.position.y = 0
ang_msg.pose.position.z = 0
q = tf.transformations.quaternion_from_euler(0, 0, (-angle) * 3.14159 /
180.0)
ang_msg.pose.orientation.x = q[0]
ang_msg.pose.orientation.y = q[1]
ang_msg.pose.orientation.z = q[2]
ang_msg.pose.orientation.w = q[3]
ang_pub.publish(ang_msg)
# Wall msg
wall_msg = PoseStamped()
wall_msg.header.stamp = rospy.Time.now()
wall_msg.header.frame_id = "drone"
wall_msg.pose.position.x = y * 0.001
wall_msg.pose.position.y = x * 0.001
wall_msg.pose.position.z = 0
q = tf.transformations.quaternion_from_euler(0, 0, (-angle - 90) *
3.14159 / 180.0)
wall_msg.pose.orientation.x = q[0]
wall_msg.pose.orientation.y = q[1]
wall_msg.pose.orientation.z = q[2]
wall_msg.pose.orientation.w = q[3]
wall_pub.publish(wall_msg)
# Pos msg
pos_msg = PointStamped()
pos_msg.header.stamp = rospy.Time.now()
pos_msg.header.frame_id = "drone"
pos_msg.point.x = y * 0.001
pos_msg.point.y = x * 0.001
pos_msg.point.z = 0
pos_pub.publish(pos_msg)
# If debug is active then update the client
if debug:
try:
top.update()
# Break if the tk client is no longer there
except tk.TclError:
break
# Keep the rate of the loop
rate.sleep()
def __init__(self,
lidar_on=True,
small=True,
init_coord=[900, 200, np.pi / 2],
color='yellow'):
# Cylinder Staff
self.coll_go = False
##################
self.color = color
self.status = False
self.cur_state = 0 # 0-neutral,1-suck,2-throw
self.sensor_range = 35
self.collision_d = 16
self.coll_ind = -1
self.collision_avoidance = True
self.localisation = Value('b', True)
self.sensors_places = [
np.pi / 2, 0, 3 * np.pi / 2, np.pi, 3 * np.pi / 2, 0, np.pi,
np.pi / 2, 0, 0
]
self.filter_queue = [np.array([False] * len(self.sensors_places))] * 3
self.sensors_map = {
1: (0, np.pi / 4),
5: (0, np.pi / 4),
7: (np.pi / 4, 3 * np.pi / 4),
0: (np.pi / 4, 3 * np.pi / 4),
6: (3 * np.pi / 4, 5 * np.pi / 4),
3: (3 * np.pi / 4, 5 * np.pi / 4),
2: (5 * np.pi / 4, 7 * np.pi / 4),
4: (5 * np.pi / 4, 7 * np.pi / 4),
8: (7 * np.pi / 4, 2 * np.pi),
9: (7 * np.pi / 4, 2 * np.pi)
}
self.lidar_on = lidar_on
self.map = np.load('npmap.npy')
if lidar_on:
logging.debug('lidar is connected')
# add check for lidar connection
try:
self.lidar = HokuyoLX(tsync=False)
self.lidar.convert_time = False
except:
self.lidar_on = False
self.localisation = Value('b', False)
logging.warning('lidar is not connected')
# self.x = 170 # mm
# self.y = 150 # mm
# self.angle = 0.0 # pi
#850 170 3p/2
# 900 200 np.pi/2
# 400, 850, 0.
self.coords = Array('d', rev_field(init_coord, self.color))
#else:
#driver.PORT_SNR = '325936843235' # need change
#self.coords = Array('d', rev_field([170, 170, 0], self.color))
self.input_queue = Queue()
self.loc_queue = Queue()
self.fsm_queue = Queue() # 2000,25,25,0.1
self.PF = pf.ParticleFilter(particles=2000,
sense_noise=25,
distance_noise=30,
angle_noise=0.15,
in_x=self.coords[0],
in_y=self.coords[1],
in_angle=self.coords[2],
input_queue=self.input_queue,
out_queue=self.loc_queue,
color=self.color)
# coords sharing procces
self.p3 = Process(target=app.run, args=("0.0.0.0", ))
# driver process
print "Paricle filter On"
self.dr = driver.Driver(self.input_queue, self.fsm_queue,
self.loc_queue)
print "Driver On"
self.p = Process(target=self.dr.run)
print "Process driver Init"
self.p.start()
print "Process driver Start"
self.p2 = Process(target=self.PF.localisation,
args=(self.localisation, self.coords,
self.get_raw_lidar))
print "Process npParticle Init"
logging.info(self.send_command('echo', 'ECHO'))
logging.info(
self.send_command('setCoordinates', [
self.coords[0] / 1000., self.coords[1] / 1000., self.coords[2]
]))
if self.lidar_on:
self.p2.start()
print "Process npParticle Start"
else:
self.localisation = Value('b', False)
print "Robot __init__ done!"