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")) ...@@ -6,7 +6,7 @@ SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0] ,"../../../resources"))
sys.path.append(SCRIPTS_PATH) sys.path.append(SCRIPTS_PATH)
from ir import IrCamera from ir import IrCamera
from lidar import LidarCamera, LidarCameraNearestObstacle from lidar import LidarSensor, LidarNO, SickLineSensor
from odometry import Odometer from odometry import Odometer
from gripper import Gripper from gripper import Gripper
...@@ -27,6 +27,9 @@ class SensorHub(): ...@@ -27,6 +27,9 @@ class SensorHub():
"bound.rightRear.signalB":0, "bound.rightRear.signalB":0,
"bound.leftRear.signalB":0, "bound.leftRear.signalB":0,
"vis.sick.localScanData":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.sick.globalScanData":0,
"vis.forwardObstacleDist":0, "vis.forwardObstacleDist":0,
"vis.forwardObstacle.x":0, "vis.forwardObstacle.x":0,
...@@ -76,16 +79,20 @@ class SensorHub(): ...@@ -76,16 +79,20 @@ class SensorHub():
# probe id 2866: vis.sick.globalScanData type tlv length 4372 # probe id 2866: vis.sick.globalScanData type tlv length 4372
# probe id 2867: vis.sick.localScanData 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 # 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 # initialize the nearest obstacle detection of the preprocessed laser data
# codes give: x, y, and distance # codes give: x, y, and distance
# probe id 2928: vis.forwardObstacle.x type float length 4 # probe id 2928: vis.forwardObstacle.x type float length 4
# probe id 2929: vis.forwardObstacle.y type float length 4 # probe id 2929: vis.forwardObstacle.y type float length 4
# probe id 2930: vis.forwardObstacleDist type float length 4 # probe id 2930: vis.forwardObstacleDist type float length 4
self.forwardObstacleDist = LidarCameraNearestObstacle("vis.forwardObstacleDist",2930,self.mp) self.forwardObs = LidarNO(("vis.forwardObstacle.x","vis.forwardObstacle.y","vis.forwardObstacleDist"),(2928,2929,2930),self.mp)
self.forwardObstacleX = LidarCameraNearestObstacle("vis.forwardObstacle.x",2928,self.mp)
self.forwardObstacleY = LidarCameraNearestObstacle("vis.forwardObstacle.y",2929,self.mp)
# initialize odometery # initialize odometery
# probe id 2438: loc.actual.x type float length 4 # probe id 2438: loc.actual.x type float length 4
...@@ -145,9 +152,9 @@ class SensorHub(): ...@@ -145,9 +152,9 @@ class SensorHub():
# start the lidar camera # start the lidar camera
self.lidar.init(ros, hostname) self.lidar.init(ros, hostname)
self.forwardObstacleDist.init(ros, hostname) self.sick.init(ros, hostname)
self.forwardObstacleX.init(ros, hostname) self.forwardObs.init(ros, hostname)
self.forwardObstacleY.init(ros, hostname)
# start the odometery # start the odometery
#self.odometeryX.init(ros, hostname) #self.odometeryX.init(ros, hostname)
...@@ -182,9 +189,8 @@ class SensorHub(): ...@@ -182,9 +189,8 @@ class SensorHub():
# run the lidar camera # run the lidar camera
self.lidar.run(self.queue, current_time) self.lidar.run(self.queue, current_time)
self.forwardObstacleDist.run(self.queue) self.sick.run(self.queue, current_time)
self.forwardObstacleX.run(self.queue) self.forwardObs.run(self.queue, current_time)
self.forwardObstacleY.run(self.queue)
# run the odometry # run the odometry
#self.odometeryX.run(self.queue) #self.odometeryX.run(self.queue)
......
...@@ -4,6 +4,7 @@ from std_msgs.msg import Header ...@@ -4,6 +4,7 @@ from std_msgs.msg import Header
from std_msgs.msg import Int16MultiArray from std_msgs.msg import Int16MultiArray
from sensor_msgs.msg import PointField from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PointStamped
import sensor_msgs.point_cloud2 as pc2 import sensor_msgs.point_cloud2 as pc2
import math import math
import time import time
...@@ -16,7 +17,7 @@ sys.path.append(SCRIPTS_PATH) ...@@ -16,7 +17,7 @@ sys.path.append(SCRIPTS_PATH)
from getter import Getter from getter import Getter
from utils import resource_name from utils import resource_name
class LidarCamera(Getter): class LidarSensor(Getter):
### ###
# Lidar sensor class extending the getter, for handling the publishing of data # Lidar sensor class extending the getter, for handling the publishing of data
### ###
...@@ -46,56 +47,32 @@ class LidarCamera(Getter): ...@@ -46,56 +47,32 @@ class LidarCamera(Getter):
strarr = data.split(':') strarr = data.split(':')
dat = [] dat = []
obj = [] 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)): for i in range(len(strarr)):
if(strarr[i]!=''): if(strarr[i]!=''):
dat.append(int(strarr[i],16)) 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() j=0.0
#header.stamp = current_time dth = 270./len(dat)
#header.frame_id = self.hostname elem_arr = []
for i in range(0,len(dat),1):
#msg = pc2.create_cloud_xyz32(header,dat) #th = np.radians(float(dat[i+1]))
th = j
N = x.size r = float(dat[i])
xyz = np.array(np.hstack([x,y,z]), dtype=np.float32) #x = float(dat[i])
msg.height = 1 x = r*math.cos(th)
msg.width = N #x = i
print(xyz) #y = float(dat[i])
y = r*math.sin(th)
msg.fields = [ z = 0.05
PointField('x', 0, PointField.FLOAT32, 1), elem_arr.append([x,y,z])
PointField('y', 4, PointField.FLOAT32, 1), j = j + np.radians(dth)
PointField('z', 8, PointField.FLOAT32, 1),
] header = Header()
#msg.is_bigendian = False header.stamp = current_time
msg.point_step = 12 header.frame_id = self.hostname
msg.row_step = msg.point_step * N
msg.is_dense = True; msg = pc2.create_cloud_xyz32(header,np.array(elem_arr))
msg.data = xyz.tostring()
# publish data # publish data
Getter.run(self, msg) Getter.run(self, msg)
...@@ -119,27 +96,96 @@ class LidarCamera(Getter): ...@@ -119,27 +96,96 @@ class LidarCamera(Getter):
# enabling 3275 will collect data from laser sensor # 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 # Lidar sensor class extending the getter, for handling the publishing of data
### ###
def init(self, ros, hostname): def init(self, ros, hostname):
self.hostname = hostname
# create publisher # 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 # 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 ## handles the publishing of data, should be run at an acceptable update interval
# get data from queue # get data from queue
data = q[self.name] try:
labels = self.name
# TODO: format data for publisher if len(labels) != 3:
#print("nearest obs data ", data) raise Exception("not enough labels")
# publish data
Getter.run(self, data)
# enable_probes((3275, 2876,2868,2867,2866,2930)) x = float(q[labels[0]])
# enabling 3275 will collect data from laser sensor 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(): ...@@ -126,7 +126,23 @@ class Mindprobe():
def start(self): def start(self):
# NOTE: is fake start required # NOTE: is fake start required
#self.enable_probes(1125) # enables the virtual start probe #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) time.sleep(2)
self.init_probes() # enable all the queued probes 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