Commit 378496fe authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

progress with handling laser scanner

parent b64579f1
......@@ -6,7 +6,7 @@ SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0] ,"../../../resources"))
sys.path.append(SCRIPTS_PATH)
from ir import IrCamera
from lidar import LidarCamera, LidarCameraNearestObstacle
from lidar import LidarSensor, LidarNO, SickLineSensor
from odometry import Odometer
from gripper import Gripper
......@@ -27,6 +27,9 @@ class SensorHub():
"bound.rightRear.signalB":0,
"bound.leftRear.signalB":0,
"vis.sick.localScanData":0,
"vis.sick.localScanData_EVERY_TICK":0,
"vis.lineSensor.line.now.pt.x":0,
"vis.lineSensor.line.now.pt.y":0,
"vis.sick.globalScanData":0,
"vis.forwardObstacleDist":0,
"vis.forwardObstacle.x":0,
......@@ -76,16 +79,20 @@ class SensorHub():
# probe id 2866: vis.sick.globalScanData type tlv length 4372
# probe id 2867: vis.sick.localScanData type tlv length 4372
# probe id 2868: vis.sick.localScanData_EVERY_TICK type tlv length 4372
self.lidar = LidarCamera("vis.sick.globalScanData",2866,self.mp)
# probe id 3223: vis.lineSensor.fitScanData.glob type tlv length 4372
self.lidar = LidarSensor("vis.sick.localScanData_EVERY_TICK",2868,self.mp)
# probe id 3198: vis.lineSensor.line.now.pt.x type float length 4
# probe id 3199: vis.lineSensor.line.now.pt.y type float length 4
# probe id 3182: vis.lineSensor.cand.local.pt.x type float length 4
# probe id 3183: vis.lineSensor.cand.local.pt.y type float length 4
self.sick = SickLineSensor(("vis.lineSensor.line.now.pt.x","vis.lineSensor.line.now.pt.y"),(3198,3199),self.mp)
# initialize the nearest obstacle detection of the preprocessed laser data
# codes give: x, y, and distance
# probe id 2928: vis.forwardObstacle.x type float length 4
# probe id 2929: vis.forwardObstacle.y type float length 4
# probe id 2930: vis.forwardObstacleDist type float length 4
self.forwardObstacleDist = LidarCameraNearestObstacle("vis.forwardObstacleDist",2930,self.mp)
self.forwardObstacleX = LidarCameraNearestObstacle("vis.forwardObstacle.x",2928,self.mp)
self.forwardObstacleY = LidarCameraNearestObstacle("vis.forwardObstacle.y",2929,self.mp)
self.forwardObs = LidarNO(("vis.forwardObstacle.x","vis.forwardObstacle.y","vis.forwardObstacleDist"),(2928,2929,2930),self.mp)
# initialize odometery
# probe id 2438: loc.actual.x type float length 4
......@@ -145,9 +152,9 @@ class SensorHub():
# start the lidar camera
self.lidar.init(ros, hostname)
self.forwardObstacleDist.init(ros, hostname)
self.forwardObstacleX.init(ros, hostname)
self.forwardObstacleY.init(ros, hostname)
self.sick.init(ros, hostname)
self.forwardObs.init(ros, hostname)
# start the odometery
#self.odometeryX.init(ros, hostname)
......@@ -182,9 +189,8 @@ class SensorHub():
# run the lidar camera
self.lidar.run(self.queue, current_time)
self.forwardObstacleDist.run(self.queue)
self.forwardObstacleX.run(self.queue)
self.forwardObstacleY.run(self.queue)
self.sick.run(self.queue, current_time)
self.forwardObs.run(self.queue, current_time)
# run the odometry
#self.odometeryX.run(self.queue)
......
......@@ -4,6 +4,7 @@ from std_msgs.msg import Header
from std_msgs.msg import Int16MultiArray
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PointStamped
import sensor_msgs.point_cloud2 as pc2
import math
import time
......@@ -16,7 +17,7 @@ sys.path.append(SCRIPTS_PATH)
from getter import Getter
from utils import resource_name
class LidarCamera(Getter):
class LidarSensor(Getter):
###
# Lidar sensor class extending the getter, for handling the publishing of data
###
......@@ -46,56 +47,32 @@ class LidarCamera(Getter):
strarr = data.split(':')
dat = []
obj = []
#f = lambda a : (x.append(int(a[0],16)),y.append(int(a[1],16)), f(a[2:])) if len(a[2:]) > 1 else (x.append(int(a[0],16)),y.append(int(a[1],16)))
#res = f(strarr)
for i in range(len(strarr)):
if(strarr[i]!=''):
dat.append(int(strarr[i],16))
#y.append(int(strarr[i+1],16))
#obj.append([int(strarr[i],16), int(strarr[i+1],16), int(strarr[i+2],16)])
x=[]
y=[]
z=[]
for i in range(0,len(dat),4):
x.append(float(dat[i]) + float(dat[i+1])/100.)
y.append(float(dat[i+2]) + float(dat[i+3])/100.)
z.append([0.05])
x = np.array(x)
y = np.array(y)
z = np.array(z)
# create pointcloud2 message
msg = PointCloud2()
# add timestamp based on ros time
msg.header.stamp = current_time
# add frame
msg.header.frame_id = self.hostname
# format message
#header = Header()
#header.stamp = current_time
#header.frame_id = self.hostname
#msg = pc2.create_cloud_xyz32(header,dat)
N = x.size
xyz = np.array(np.hstack([x,y,z]), dtype=np.float32)
msg.height = 1
msg.width = N
print(xyz)
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
]
#msg.is_bigendian = False
msg.point_step = 12
msg.row_step = msg.point_step * N
msg.is_dense = True;
msg.data = xyz.tostring()
j=0.0
dth = 270./len(dat)
elem_arr = []
for i in range(0,len(dat),1):
#th = np.radians(float(dat[i+1]))
th = j
r = float(dat[i])
#x = float(dat[i])
x = r*math.cos(th)
#x = i
#y = float(dat[i])
y = r*math.sin(th)
z = 0.05
elem_arr.append([x,y,z])
j = j + np.radians(dth)
header = Header()
header.stamp = current_time
header.frame_id = self.hostname
msg = pc2.create_cloud_xyz32(header,np.array(elem_arr))
# publish data
Getter.run(self, msg)
......@@ -119,27 +96,96 @@ class LidarCamera(Getter):
# enabling 3275 will collect data from laser sensor
class LidarCameraNearestObstacle(Getter):
class LidarNO(Getter):
###
# Lidar sensor class extending the getter, for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
self.pub = ros.Publisher('/'+hostname+'/lidar/'+resource_name(self.name),Float64,queue_size=1)
if type(self.name) != tuple:
self.pub = ros.Publisher('/'+hostname+'/lidar/'+resource_name(self.name),PointCloud2,queue_size=1)
else:
self.pub = ros.Publisher('/'+hostname+'/lidar/'+resource_name(os.path.commonprefix(self.name)),PointStamped,queue_size=1)
# pass to parent to enble probes
# self.mp.write_probe(self.codes,1)
# Getter.init(self)
Getter.init(self)
def run(self, q):
def run(self, q, current_time):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data = q[self.name]
# TODO: format data for publisher
#print("nearest obs data ", data)
# publish data
Getter.run(self, data)
try:
labels = self.name
if len(labels) != 3:
raise Exception("not enough labels")
# enable_probes((3275, 2876,2868,2867,2866,2930))
# enabling 3275 will collect data from laser sensor
x = float(q[labels[0]])
y = float(q[labels[1]])
z=0.
dist = float(q[labels[2]])
msg = PointStamped()
msg.header.stamp = current_time
msg.header.frame_id = self.hostname
msg.point.x = x
msg.point.y = y
msg.point.z = z
# publish data
Getter.run(self, msg)
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
class SickLineSensor(Getter):
###
# Lidar sensor class extending the getter, for handling the publishing of data
###
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
if type(self.name) != tuple:
self.pub = ros.Publisher('/'+hostname+'/sick/'+resource_name(self.name),PointCloud2,queue_size=1)
else:
self.pub = ros.Publisher('/'+hostname+'/sick/'+resource_name(os.path.commonprefix(self.name)),PointCloud2,queue_size=1)
# pass to parent to enble probes
Getter.init(self)
self.dat = []
def run(self, q, current_time):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
x=0
y=0
labels = self.name
if(len(labels)!=2):
raise Exception("Not enough labels!")
x = float(q[labels[0]])
y = float(q[labels[1]])
self.dat.append([x,y,0.05])
if(len(self.dat) > 270):
N = len(self.dat) - 270
self.dat = self.dat[N:]
xyz = np.array(self.dat)
header = Header()
header.stamp = current_time
header.frame_id = self.hostname
msg = pc2.create_cloud_xyz32(header,xyz)
# publish data
Getter.run(self, msg)
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
......@@ -126,7 +126,23 @@ class Mindprobe():
def start(self):
# NOTE: is fake start required
#self.enable_probes(1125) # enables the virtual start probe
self.write_probe(1125,1) # sets the start to TRUE. this 'starts' the robot.
FAKE_START = 1125
STANDBY_MODE = 2877
ARM_ENABLED = 1082
GRIPPER_ENABLED = 1083
DRIVE_ENABLED = 580
DRIVE_COMMAND = 1220
ROB_SENSOR = 3100
LINE_SENSOR = 3171
I2C_SENSOR = 3895
BEEP = 1120
self.write_probe(BEEP,0)
self.write_probe(FAKE_START,1)
self.write_probe(STANDBY_MODE,0)
#self.write_probes([(STANDBY_MODE,1),(DRIVE_ENABLED,1),(DRIVE_COMMAND,1),(ROB_SENSOR,1),(LINE_SENSOR,1),(I2C_SENSOR,1),(ARM_ENABLED,1),(GRIPPER_ENABLED,1)]) # sets the start to TRUE. this 'starts' the robot.
time.sleep(2)
self.init_probes() # enable all the queued probes
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment