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

finished implementing sensor data listening

parent 9e137ca4
......@@ -88,6 +88,7 @@ IR_RIGHT_BACK_B = "bound.rightRear.signalB"
IR_LEFT_BACK_B = "bound.leftRear.signalB"
LOCAL_SCAN = "vis.sick.localScanData"
GLOBAL_SCAN = "vis.sick.globalScanData"
OBSTACLE_X = "vis.forwardObstacle.x"
OBSTACLE_Y = "vis.forwardObstacle.y"
......
......@@ -31,10 +31,14 @@ class IrListener(Reader):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
data_list = [self.queue[label] for label in self.probes]
data = sum(data_list) / len(data_list)
# publish data
Reader.publish(self, data)
try:
data_list = [self.queue[label] for label in self.probes]
data = sum(data_list) / len(data_list)
# publish data
Reader.publish(self, data)
except Exception as e:
rospy.logerr("IR Sensor publish failed")
raise e
class IrListenerRightFront(IrListener):
......
......@@ -29,7 +29,7 @@ class LidarListener(Reader):
def __init__(self, h):
self.hostname = h
Reader.__init__(self, [LOCAL_SCAN])
Reader.__init__(self, [GLOBAL_SCAN])
self.pub = rospy.Publisher(
"/" + self.hostname + "/lidar/" + resource_name(Reader.get_probes(self)),
PointCloud2,
......@@ -40,7 +40,7 @@ class LidarListener(Reader):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
try:
data = self.queue[LOCAL_SCAN]
data = self.queue[GLOBAL_SCAN]
# print data
if type(data) != str:
......@@ -53,12 +53,20 @@ class LidarListener(Reader):
# create x and y arrays from the base16 string of data
strarr = data.split(":")
datStr = ""
# convert hex numbers to char string
for elem in strarr:
if elem != "":
datStr += chr(int(elem, 16))
# parse char string as floats
dat = [
struct.unpack("<f", datStr[i : i + 4]) for i in range(0, len(datStr), 4)
]
# fix all nan or inf values
dat = list(np.nan_to_num(np.array(dat)))
# parse float array to pointcloud xyz matrix
elem_arr = []
j = 0
for i in range(0, len(dat), 3):
......@@ -76,10 +84,12 @@ class LidarListener(Reader):
# publish data
Reader.publish(self, msg)
except TypeError:
except TypeError as e:
pass
#rospy.logwarn(e)
except Exception as e:
# well this is sad
rospy.logerr("error publishing lidar data")
raise e
......@@ -119,4 +129,5 @@ class LidarNObsListener(Reader):
except Exception as e:
# well this is sad
rospy.logerr("error publishing nearest object data")
raise e
......@@ -41,20 +41,24 @@ class Odometer(Reader):
def publish(self):
## handles the publishing of data, should be run at an acceptable update interval
# get data from queue
odom = Odometry()
odom.header.stamp = rospy.get_rostime()
odom.header.frame_id = self.hostname
try:
odom = Odometry()
odom.header.stamp = rospy.get_rostime()
odom.header.frame_id = self.hostname
[X, Y, H, V, W] = list(map(self.queue.get, Reader.get_probes(self)))
[X, Y, H, V, W] = list(map(self.queue.get, Reader.get_probes(self)))
heading = tf.transformations.quaternion_from_euler(0.0, 0.0, H)
odom.pose.pose = Pose(
Point(X, Y, 0.0),
Quaternion(heading[0], heading[1], heading[2], heading[3]),
)
odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W))
# publish data
Reader.publish(self, odom)
heading = tf.transformations.quaternion_from_euler(0.0, 0.0, H)
odom.pose.pose = Pose(
Point(X, Y, 0.0),
Quaternion(heading[0], heading[1], heading[2], heading[3]),
)
odom.twist.twist = Twist(Vector3(V, 0, 0), Vector3(0, 0, W))
# publish data
Reader.publish(self, odom)
except Exception as e:
rospy.logerror("publishing odometry data failed")
raise e
class OdometryListener(Odometer):
......
#!/usr/bin/env python
# import rospy
import rospy
from hv_bridge.srv import mp_toggle, mp_toggleResponse
import sys, os
RESOURCE_PATH = os.path.abspath(os.path.join(sys.path[0], "../../"))
sys.path.append(RESOURCE_PATH)
from libs.Resources import Writer
from libs.Resources.utils import *
from libs.Resources.config import *
# probe id 3601: behaviors.scriptAvoidCollision.isEnabled type bool length 1
# probe id 3602: behaviors.script.run type bool length 1
......@@ -9,11 +17,11 @@ AVOID_CODE = 3601
PSEUDO_START_CODE = 1125
class AvoidService:
def __init__(self, mp, ros):
class AvoidService(Writer):
def __init__(self):
# contstruct basics for
Writer.__init__(self, [AVOID])
self.serv = ros.Service("AvoidCollision", mp_toggle, self.toggle_avoid)
self.serv = rospy.Service("AvoidCollision", mp_toggle, self.toggle_avoid)
def stop(self):
try:
......
......@@ -8,6 +8,7 @@ sys.path.append(RESOURCE_PATH)
from libs.Resources import Writer
from libs.Resources.utils import *
from libs.Resources.config import *
# probe id 3842: behaviors.followMe.run type bool length 1
......
......@@ -9,6 +9,7 @@ sys.path.append(RESOURCE_PATH)
from libs.Resources import Writer
from libs.Resources.utils import *
from libs.Resources.config import *
# probe id 3486: behaviors.spacing.pick.pickTarget.x type float length 4
# probe id 3487: behaviors.spacing.pick.pickTarget.y type float length 4
......@@ -21,10 +22,10 @@ SET_H_LOC_CODE = 2440
class PickTargetService(Writer):
def __init__(self, mp, ros):
def __init__(self):
# contstruct basics for
Writer.__init__(self, [BOT_GOT_POT, SET_PICK_X, SET_PICK_Y])
self.serv_set = ros.Service(
self.serv_set = rospy.Service(
"SetPickTarget", mp_set_target, self.set_pick_target
)
......@@ -44,10 +45,10 @@ class PickTargetService(Writer):
class TogglePickTargetService(Writer):
def __init__(self, mp, ros):
def __init__(self):
# contstruct basics for
Writer.__init__(self, [RUN_SPACING])
self.serv_run = ros.Service("RunPick", mp_toggle, self.toggle_pick)
self.serv_run = rospy.Service("RunPick", mp_toggle, self.toggle_pick)
def stop(self):
try:
......
......@@ -8,6 +8,7 @@ import time
import os
import sys
import subprocess
import traceback
# add local python scripts to the path so that they can be iported
sys.path.append(os.path.abspath(os.path.join(sys.path[0], "../")))
......@@ -75,7 +76,7 @@ class HVClient(Client):
pass
if PICK_SRV in services:
# enable odometry with gryro correction listener
self.svrs.append(PickTargetService())
self.srvs.append(PickTargetService())
pass
if AVOID_SRV in services:
# enable odometry with gryro correction listener
......@@ -103,8 +104,10 @@ class HVClient(Client):
self.enable_services()
time.sleep(1)
# Client.start_probes(self)
Client.start_probes(self)
except Exception as e:
err = traceback.format_exc()
rospy.logerr(e)
raise e
def stop(self):
......@@ -157,12 +160,11 @@ class HVClient(Client):
# collect all subscription messages to write to robot
msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr))
# collect all services messages to write to robot
msgs += get_msg_arr([], map((lambda a: a.get_msgs()), self.servs))
msgs += get_msg_arr([], map((lambda a: a.get_msgs()), self.srvs))
v = ""
if len(msgs) == 0:
return
for (probe, value) in msgs:
print (probe, value)
probe_id = Client.lookup_probe_id(self, probe)
probe_type = Client.lookup_probe_type(self, probe_id)
......@@ -194,17 +196,18 @@ class HVClient(Client):
def read_probe_msg(self, data):
# pass data to the correct child class
try:
print ("CALLED!!! the read msg\n\n\n")
while len(item):
while len(data):
(t, l, v, data) = next_tlv(data)
(probe_name, probe_type) = Client.lookup_probe(self, t)
if any([lst.needs(probe_name) for lst in self.lstnr]):
val = Client.decode_probe_data(self, probe_type, v)
print ("New Message[name: " + probe_name + " val: " + val + "]")
#print ("New Message[name: " + str(probe_name) + " val: " + str(val) + "]")
[serv.add(probe_name, val) for serv in self.lstnr]
else:
rospy.logwarn("Probe info not needed: %s" % probe_name)
# msg can be used for debugging. otherwise this block is probably not needed
#rospy.loginfo("Probe info not needed: %s" % probe_name)
pass
except Exception as e:
raise e
......@@ -220,7 +223,7 @@ class HVClient(Client):
def end_ros_services(self):
# stop all ROS services
try:
for ros_srv in self.servs:
for ros_srv in self.srvs:
ros_srv.stop()
except Exception as e:
raise e
......@@ -98,7 +98,6 @@ if __name__ == "__main__":
services = []
if args.all:
# connect all sensors and controllers
print(args.all)
services = [
DRIVE_CMD,
GRIPPER_CMD,
......@@ -182,7 +181,7 @@ if __name__ == "__main__":
)
while not rospy.is_shutdown():
# write any messages from ROS to the robot
# hv_bot.capture(1 / 200)
#hv_bot.capture(1 / 200)
hv_bot.write()
hv_bot.publish()
......
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