Skip to content

GitLab

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

add input arguments to main class

parent 41dd7906
Hide whitespace changes
Inline Side-by-side
Showing with 205 additions and 112 deletions
hv_bridge/libs/Resources/__init__.py
View file @ b302efa2
......@@ -4,6 +4,12 @@
# include all the utility scripts
###
from client import Client
from reader import Reader
from writer import Writer
from config import Serv
from utils import *
from getter import *
from setter import *
......
hv_bridge/libs/Resources/client.py
View file @ b302efa2
......@@ -100,7 +100,6 @@ class Client:
print "Client Disconnet Failed"
raise e
## ----- Probe Methods ----- ##
def discover_probes(self):
......@@ -205,7 +204,7 @@ class Client:
return data
except:
raise RuntimeError("struct unpack failed")
def start_probes(self):
# start capturing information from a probe
self.capturing = True
......@@ -215,7 +214,7 @@ class Client:
# stop capturing information from a probe
self.send_message(make_tlv(MP_TLV_STOP_PROBES))
self.capturing = False
def write_probe(self, probe, value):
# write a vale to a probe
probe_id = self.lookup_probe_id(probe)
......
hv_bridge/libs/Resources/config.py
View file @ b302efa2
import enum.Enum
class Serv(Enum):
DRIVE_CMD = 1
GRIPPER_CMD = 2
ARM_CMD = 3
ODOM_CMD = 4
ODOM_LSTN = 5
IR_LSTN = 6
LIDAR_LSTN = 7
GRIPPER_LSTN = 8
NEAREST_OBS = 9
ODOM_GYRO_LSTN = 10
FOLLOW_SRV = 11
PICK_SRV = 12
AVOID_SRV = 13
hv_bridge/libs/Resources/listener.py
View file @ b302efa2
......@@ -6,7 +6,7 @@ class listen(threading.Thread):
self.mp = mp
self.callback = None
self.setDaemon(True)
def addMsgCallback(callbackFcn):
# adds a function that is called whenever a message is recived by the queue
self.callback = callbackFcn
......@@ -17,7 +17,9 @@ class listen(threading.Thread):
while True:
# Wait for incoming data from the RCP or for the listener
# pipe to close, indicating that we are
(r, w, x) = select.select((self.client.s, self.client.listener_pipe[0]), (), ())
(r, w, x) = select.select(
(self.client.s, self.client.listener_pipe[0]), (), ()
)
if self.client.listener_pipe[0] in r:
# listener pipe signaled the thread to exit
os.close(self.client.listener_pipe[0])
......@@ -29,7 +31,7 @@ class listen(threading.Thread):
try:
if self.callback == None:
raise NameError("No Callback has been defined")
self.callback(t,v)
self.callback(t, v)
except NameError:
pass
# time.sleep(0.1)
......
hv_bridge/libs/Resources/reader.py
View file @ b302efa2
class Reader():
class Reader:
# reader class for getting data from the robot and passing to ROS
def __init__(self, name, p):
# constructor for settings class
......@@ -11,14 +11,14 @@ class Reader():
def run(self, data):
## Publish data
self.pub.publish(data)
def add(name, val):
# add data piece
try:
self.queue[name] = val
except Exception as e:
raise e
def needs(name):
# check if name is used by this class
return True if name in self.probes else False
......
hv_bridge/libs/Resources/writer.py
View file @ b302efa2
class Reader():
class Writer:
# reader class for getting data from the robot and passing to ROS
def __init__(self, p):
# constructor for settings class
......
hv_bridge/libs/Sensors/hub.py
View file @ b302efa2
......@@ -101,11 +101,11 @@ class SensorHub:
# 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(
# 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
......@@ -200,7 +200,7 @@ class SensorHub:
# start the lidar camera
self.lidar.init(ros, hostname)
#self.sick.init(ros, hostname)
# self.sick.init(ros, hostname)
self.forwardObs.init(ros, hostname)
# start the odometery
......@@ -236,7 +236,7 @@ class SensorHub:
# run the lidar camera
self.lidar.run(self.queue, current_time)
#self.sick.run(self.queue, current_time)
# self.sick.run(self.queue, current_time)
self.forwardObs.run(self.queue, current_time)
# run the odometry
......
hv_bridge/libs/Sensors/lidar.py
View file @ b302efa2
......@@ -44,7 +44,7 @@ class LidarSensor(Getter):
try:
data = q[self.name]
#print data
# print data
if type(data) != str:
raise Exception("Data not in correct format!")
# get the data from the message
......@@ -58,25 +58,27 @@ class LidarSensor(Getter):
for elem in strarr:
if elem != "":
datStr += chr(int(elem, 16))
dat = [struct.unpack("<f", datStr[i : i + 4]) for i in range(0, len(datStr), 4)]
dat = [
struct.unpack("<f", datStr[i : i + 4]) for i in range(0, len(datStr), 4)
]
#elem_arr = np.zeros((361,3),dtype=float)
# elem_arr = np.zeros((361,3),dtype=float)
elem_arr = []
j = 0
for i in range(0, len(dat), 3):
if len(dat[i:]) < 3:
break
#if j > 360:
# if j > 360:
# break
elem_arr.append([dat[i],dat[i+1],0.05])
elem_arr.append([dat[i], dat[i + 1], 0.05])
z = 0.0
#print "x = %f, y = %f" % (x,y)
#elem_arr[j,0] = dat[i]
#elem_arr[j,1] = dat[i+1]
#j = j + 1
# print "x = %f, y = %f" % (x,y)
# elem_arr[j,0] = dat[i]
# elem_arr[j,1] = dat[i+1]
# j = j + 1
elem_arr = np.array(elem_arr)
header = Header()
header.stamp = current_time
header.frame_id = self.hostname
......@@ -89,7 +91,7 @@ class LidarSensor(Getter):
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
print (err)
# create pointcloud2 message
msg = PointCloud2()
msg.header.stamp = current_time
......@@ -153,7 +155,7 @@ class LidarNO(Getter):
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
print (err)
class SickLineSensor(Getter):
......@@ -217,4 +219,4 @@ class SickLineSensor(Getter):
except Exception as e:
# well this is sad
err = traceback.format_exc()
print(err)
print (err)
hv_bridge/scripts/hv_client.py
View file @ b302efa2
......@@ -58,6 +58,16 @@ class HVClient(Client):
if ODOM_GYRO_LSTN in services:
# enable odometry with gryro correction listener
self.lstnr.append(OdometryGyroListener())
if FOLLOW_SRV in services:
# enable odometry with gryro correction listener
self.srvs.append(FollowService())
if PICK_SRV in services:
# enable odometry with gryro correction listener
self.svrs.append(PickTargetService())
if AVOID_SRV in services:
# enable odometry with gryro correction listener
self.srvs.append(AvoidService())
Client(self)
except Exception as e:
......@@ -107,7 +117,7 @@ class HVClient(Client):
Client.enable_probes(self,probes)
def write(self, msgs):
def write(self):
# gather all messages from controll class instances and write to the robot
try:
get_msg_arr = lambda a,b : get_msg_arr(a+b[0],b[1:]) if len(b) > 1 else a + b[0])
......
hv_bridge/scripts/main_node.py
View file @ b302efa2
#!/usr/bin/env python
import rospy
import argparse
import traceback
import sys, os, time
# add local python scripts to the path so that they can be iported
......@@ -22,106 +24,161 @@ if __name__ == "__main__":
try:
# Init the connection with the ROS system
rospy.init_node("hai_main_node", anonymous=True)
# TODO: add rospy.on_shutdown(callback)
# Init the argument parser
parser = argparse.ArgumentParser(
description="ROS Connection Application for comunication and control of the HV Robots"
)
# Add the arguments to handle setting various sensor and controller connections
parser.add_argument(
"hostname",
metavar="hostname",
type=str,
default="hai-1095.local",
nargs="?",
help="The name of the robot, generally of the form hai-####.local (default: hai-1095.local",
)
parser.add_argument(
"-a",
"--all",
action="store_true",
dest="all",
help="Use all the sensors and controllers",
)
parser.add_argument(
"--all-controllers",
action="store_true",
dest="controller_flag",
help="Enable all controller classes",
)
parser.add_argument(
"-c",
"--controllers",
type=int,
nargs="+",
dest="controllers",
choices=[1, 2, 3],
help="Set the options for which contollers to use (choices: 1 - arm, 2 - gripper, 3 - drive)",
)
parser.add_argument(
"--all-sensors",
action="store_true",
dest="sensor_flag",
help="Enable all sensor classes",
)
parser.add_argument(
"-s",
"--sensors",
type=int,
nargs="+",
dest="sensors",
choices=[1, 2, 3, 4, 5, 6],
help="Set the options for which sensors to use (choices: 1 - lidar, 2 - gripper, 3 - ir, 4 - odometry, 5 - odometry w/ gyro, 6 - nearest object)",
)
parser.add_argument(
"--services",
type=str,
nargs="*",
dest="services",
choices=[None, "all", "follow", "pick", "avoid"],
default="all",
help="Set the services to make available",
)
# if present, set the hostname based on the argument inputed by the user
if len(sys.argv) < 2:
host_name = "hai-1095.local"
args = parser.parse_args()
# set the hostname based on the argument inputed by the user (default: hai-1095.local)
host_name = args.hostname
print(args)
services = []
if args.all:
# connect all sensors and controllers
print(args.all)
services = [
DRIVE_CMD,
GRIPPER_CMD,
ARM_CMD,
ODOM_LSTN,
IR_LSTN,
LIDAR_LSTN,
GRIPPER_LSTN,
NEAREST_OBS,
ODOM_GYRO_LSTN,
]
else:
host_name = sys.argv[1]
print("connected to {}").format(host_name)
# Create the mindprobe instance
mp = Mindprobe()
mp.init(host_name)
# wait while the mp connection is fully established
while len(mp.return_probes()) < 3000:
time.sleep(1)
### Create all the Controllers ###
drive = DriveController(mp)
arm = ArmController(mp)
gripper = GripperController(mp)
### -------------------------- ###
### Create Sensor Hub ###
hub = SensorHub(mp)
### ----------------- ###
if args.sensor_flag:
print("using all sensors")
services += [
ODOM_LSTN,
IR_LSTN,
LIDAR_LSTN,
GRIPPER_LSTN,
NEAREST_OBS,
ODOM_GYRO_LSTN,
]
elif args.sensors != None:
print(args.sensors)
if 1 in args.sensors:
services.append(LIDAR_LSTN)
if 2 in args.sensors:
services.append(GRIPPER_LSTN)
if 3 in args.sensors:
services.append(IR_LSTN)
if 4 in args.sensors:
services.append(ODOM_LSTN)
if 5 in args.sensors:
services.append(ODOM_GYRO_LSTN)
if 6 in args.sensors:
services.append(NEAREST_OBS)
if args.controller_flag:
print("using all controllers")
services += [DRIVE_CMD, GRIPPER_CMD, ARM_CMD]
elif args.controllers != None:
print(args.controllers)
if 3 in args.sensors:
services.append(DRIVE_CMD)
if 2 in args.controllers:
services.append(GRIPPER_CMD)
if 1 in args.controllers:
services.append(ARM_CMD)
if "all" in args.services:
print("services!")
services += [FOLLOW_SRV, PICK_SRV, AVOID_SRV]
elif args.services != []:
if "follow" in args.services:
services.append(FOLLOW_SRV)
if "pick" in args.services:
services.append(PICK_SRV)
if "avoid" in args.services:
services.append(AVOID_SRV)
print("connecting to {}").format(host_name)
hv_bot = HVClient(services)
# start the connection to the robot
hv_bot.start()
time.sleep(1)
### Drive Controller ###
drive.run(rospy, resource_name(host_name))
print("DRIVE control running")
### --------------- ###
### Arm Controller ###
arm.run(rospy, resource_name(host_name))
print("ARM control running")
### --------------- ###
### Gripper Controller ###
gripper.run(rospy, resource_name(host_name))
print("GRIPPER control running")
### --------------- ###
### Sensor Hub ###
hub.start(rospy, resource_name(host_name))
print("sensor HUB running")
### --------------- ###
# handle closing everything nicely when ros shutsdown
rospy.on_shutdown(lambda: hv_bot.stop())
# enable the listener services
hv_bot.enable_services()
time.sleep(1)
### Start Mindprobe Connection ###
# enables probes and starts mode to allow listening
mp.start()
### -------------------------- ###
### Start Services ###
follow = FollowService(
mp, rospy
) # allows the follow me behavior to be toggled on.
pick = PickTargetService(
mp, rospy
) # allows the setting an x-y coord to collect a pot from.
avoid = AvoidService(
mp, rospy
) # allows the avoid collision behavior to be toggled on.
### -------------- ###
time.sleep(3)
# handle closing everything nicely when ros shutsdown
rospy.on_shutdown(
lambda: follow.stop() & pick.stop() & avoid.stop() & mp.disconnect()
)
# set rate based on the mindprobe refresh rate.
# default to 100. (mp runs at 200)
hz = 100
if mp.hz:
hz = mp.hz
# timer_count_1 = 0
# timer_count_2 = 0
# Start the ROS main loop
rate = rospy.Rate(hz)
print("starting publishing data from " + host_name + " at " + str(hz) + "hz")
while not rospy.is_shutdown():
# if(timer_count_1 > 10):
# # run the sensor data collection
# hub.listen()
# timer_count_1 = 0
# if(timer_count_2 > 100):
# hub.scan()
# timer_count_2 = 0
# if(timer_count_2 == 1):
# mp.write_probe(3859,1)
# timer_count_1 = timer_count_1 + 1
# timer_count_2 = timer_count_2 + 1
hub.listen(rospy.get_rostime())
# write any messages from ROS to the robot
hv_bot.write()
rate.sleep()
......@@ -130,7 +187,8 @@ if __name__ == "__main__":
pass
except Exception as e:
# general error handling
print(e)
err = traceback.format_exc()
print(err)
finally:
# NOTE: this may not actually be called in most cases, and should be handled in the on shutdown function
# mp.disconnect()
......
hv_launch/lauch/teleop.launch
View file @ b302efa2
<launch>
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--controllers" />
<node name="main_node.py" pkg="hv_bridge" type="bridge" args="--all-controllers" />
<node name="teleop_node.py" pkg="hv_teleop" type="talker" />
</launch>
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