Skip to content

GitLab

Commit b64579f1 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

update to handle sensor

parent 228db3e3
Hide whitespace changes
Inline Side-by-side
Showing with 98 additions and 42 deletions
hv_bridge/CMakeLists.txt
View file @ b64579f1
......@@ -76,6 +76,7 @@ generate_messages(
DEPENDENCIES
geometry_msgs
std_msgs
nav_msgs
)
################################################
......
hv_bridge/includes/Sensors/hub.py
View file @ b64579f1
......@@ -27,6 +27,7 @@ class SensorHub():
"bound.rightRear.signalB":0,
"bound.leftRear.signalB":0,
"vis.sick.localScanData":0,
"vis.sick.globalScanData":0,
"vis.forwardObstacleDist":0,
"vis.forwardObstacle.x":0,
"vis.forwardObstacle.y":0,
......@@ -75,7 +76,8 @@ 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.localScanData",3275,self.mp)
self.lidar = LidarCamera("vis.sick.globalScanData",2866,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
......@@ -110,6 +112,7 @@ class SensorHub():
self.odometeryGyro = Odometer(("loc.rawOdometryWGyro.x","loc.rawOdometryWGyro.y","loc.rawOdometryWGyro.h", "loc.rawOdometryWGyro.v", "loc.rawOdometryWGyro.w"),(2463,2464,2465,2466,2467),self.mp)
#probe id 1100: manip.botGotPot type bool length 1
#probe id 3151: vis.PIGSensor.isPotInGripper type bool length 1
self.gripper = Gripper("manip.botGotPot",1100,self.mp)
def start(self, ros, hostname):
......@@ -141,7 +144,7 @@ class SensorHub():
self.rightRear.init(ros, hostname)
# start the lidar camera
# self.lidar.init(ros, hostname)
self.lidar.init(ros, hostname)
self.forwardObstacleDist.init(ros, hostname)
self.forwardObstacleX.init(ros, hostname)
self.forwardObstacleY.init(ros, hostname)
......@@ -178,7 +181,7 @@ class SensorHub():
self.rightRear.run(self.queue, current_time)
# run the lidar camera
# self.lidar.run(self.queue)
self.lidar.run(self.queue, current_time)
self.forwardObstacleDist.run(self.queue)
self.forwardObstacleX.run(self.queue)
self.forwardObstacleY.run(self.queue)
......
hv_bridge/includes/Sensors/lidar.py
View file @ b64579f1
#!/usr/bin/env python
from std_msgs.msg import Float64
from std_msgs.msg import Header
from std_msgs.msg import Int16MultiArray
from nav_msgs.msg import PointCloud2
from nav_msgs.msg import PointField
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import math
import time
import ros
import sys, os
import numpy as np
import traceback
SCRIPTS_PATH = os.path.abspath(os.path.join(sys.path[0] ,"../../../resources"))
sys.path.append(SCRIPTS_PATH)
from getter import Getter
......@@ -18,7 +23,7 @@ class LidarCamera(Getter):
def init(self, ros, hostname):
self.hostname = hostname
# create publisher
self.pub = ros.Publisher('/'+hostname+'/lidar/'+resource_name(self.name),Int16MultiArray,queue_size=1)
self.pub = ros.Publisher('/'+hostname+'/lidar/'+resource_name(self.name),PointCloud2,queue_size=1)
# pass to parent to enble probes
#self.mp.write_probe(self.codes,1)
Getter.init(self)
......@@ -27,45 +32,88 @@ class LidarCamera(Getter):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
data = q[self.name]
if (type(data) != str):
raise Exception("Data not in correct format!")
# get the data from the message
data = data.split('v=')[-1]
if (data == ''):
raise Exception("lidar data is empty")
# create x and y arrays from the base16 string of data
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])
data = q[self.name]
# get the data from the message
data = data.split('v=')[-1]
if (data == ''):
return
# create x and y arrays from the base16 string of data
strarr = data.split(':')
x=[]
y=[]
(lambda a : (x.append(int(a[0],16)),y.append(int(a[1],16)), fx(a[2:])) if a.size > 1 else False)(strarr)
# create pointcloud2 message
dataout = PointCloud2()
# add timestamp based on ros time
msg.header.stamp = current_time
# add frame
msg.header.frame_id = self.hostname
# format message
N = len(x)
xy = np.array(np.hstack([x,y]), dtype=np.float32)
msg.height = 1
msg.width = N
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
]
msg.is_bigendian = False
msg.point_step = 8
msg.row_step = msg.point_step * N
msg.is_dense = True;
msg.data = xy.tostring()
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
# publish data
Getter.run(self, dataout)
#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()
# publish data
Getter.run(self, msg)
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
# create pointcloud2 message
msg = PointCloud2()
msg.header.stamp = current_time
msg.header.frame_id = self.hostname
# publish data
Getter.run(self, msg)
# enable_probes((3275, 2876,2868,2867,2866,2930))
# enabling 3275 will collect data from laser sensor
......
hv_bridge/includes/Services/pick_target.py
View file @ b64579f1
......@@ -16,6 +16,7 @@ SET_Y_LOC_CODE = 2439
SET_H_LOC_CODE = 2440
PSEUDO_START_CODE = 1125
BOT_GOT_POT_CODE = 1100
POT_IN_GRIPPER = 3151
class PickTargetService():
......
hv_bridge/package.xml
View file @ b64579f1
......@@ -52,6 +52,7 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
......@@ -59,11 +60,13 @@
<build_export_depend>rospy</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>tf</exec_depend>
......
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