Commit 3538da8b authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

more cleaning up code

parent bf7c6418
......@@ -14,12 +14,10 @@ class ArmController(Writer):
# Controller for the robot arm
# Extends the Setter class
###
def __init__(self):
def __init__(self, hostname):
# Create writer for this probe
# manip.armRequestedTargetPositionInPercent
Writer.__init__(self, ["manip.armTargetPositionInPercent"])
def run(self, hostname):
# runs a subscriber for setting the arm position.
rospy.Subscriber("/" + hostname + "/set_arm_pos", Float32, self.callback)
......
......@@ -18,12 +18,11 @@ class DriveController(Writer):
# extends the Writer parent class
# handles differential drive controll for the robot
###
def __init__(self):
def __init__(self, hostname):
Writer.__init__(
self, ["mob.driveControlWheelSpeeds.l", "mob.driveControlWheelSpeeds.r"]
)
def run(self, hostname):
# runs a subscriber for setting the wheel velocity.
rospy.Subscriber("/" + hostname + "/cmd_vel", Twist, self.callback)
def callback(self, cmd_msg):
......
......@@ -14,10 +14,12 @@ class GripperController(Writer):
# Controller for the robot gripper
# Extends the Setter class
###
def __init__(self):
def __init__(self, hostname):
# Create writer for this probe
# manip.gripperRequestedTargetPositionInPercent
Writer.__init__(self, ["manip.gripperTargetPositionInPercent"])
# intializes the mp connection and runs a subscriber for setting the gripper position.
rospy.Subscriber("/" + hostname + "/set_gripper_pos", Float32, self.callback)
def callback(self, cmd_msg):
# callback function - extends the setter.callback functionality
......@@ -28,7 +30,3 @@ class GripperController(Writer):
elif pos < 0:
pos = 0
Writer.callback(self, pos)
def run(self, rospy, hostname):
# intializes the mp connection and runs a subscriber for setting the gripper position.
rospy.Subscriber("/" + hostname + "/set_gripper_pos", Float32, self.callback)
......@@ -11,4 +11,4 @@ from writer import *
from config import *
from utils import *
from listener import *
from listener import *
......@@ -172,6 +172,24 @@ class Client:
except Exception as e:
raise e
def lookup_probe_type(self, probe_id):
# get the probe type
try:
if type(probe_id) != int:
raise ValueError("probe id must be an int")
return self.probe_defs[probe_id][1]
except Exception as e:
raise e
def lookup_probe(self, t):
# get the probe info
try:
if type(t) != int:
raise ValueError("probe id must be an int")
return (self.probe_defs[t][0], self.probe_defs[t][1])
except Exception as e:
raise e
def enable_probes(self, probes):
# enables one or more probes.
# allow for singleton argument
......
......@@ -22,20 +22,20 @@ MP_TLV_MISSED_DEBUG_MESSAGES = 15
MP_TLV_WRITE_PROBES = 16
type_table = {
1: ("uint8", "<B"), # 1-byte unsigned integer
2: ("int8", "<b"), # 1-byte signed integer
3: ("uint16", "<H"), # 2-byte unsigned integer
4: ("int16", "<h"), # 2-byte signed integer
5: ("uint32", "<I"), # 4-byte unsigned integer
6: ("int32", "<i"), # 4-byte signed integer
7: ("uint64", "<Q"), # 8-byte unsigned integer
8: ("int64", "<q"), # 8-byte signed integer
9: ("float", "<f"), # 4-byte float (IEEE 754)
10: ("double", "<d"), # 8-byte float (IEEE 754)
11: ("tlv", ""), # Variable-length TLV data
12: ("bool", "<B"), # 1-byte boolean integer (1 or 0)
13: ("string", ""), # variable-size null-terminated char string
}
1: ("uint8", "<B"), # 1-byte unsigned integer
2: ("int8", "<b"), # 1-byte signed integer
3: ("uint16", "<H"), # 2-byte unsigned integer
4: ("int16", "<h"), # 2-byte signed integer
5: ("uint32", "<I"), # 4-byte unsigned integer
6: ("int32", "<i"), # 4-byte signed integer
7: ("uint64", "<Q"), # 8-byte unsigned integer
8: ("int64", "<q"), # 8-byte signed integer
9: ("float", "<f"), # 4-byte float (IEEE 754)
10: ("double", "<d"), # 8-byte float (IEEE 754)
11: ("tlv", ""), # Variable-length TLV data
12: ("bool", "<B"), # 1-byte boolean integer (1 or 0)
13: ("string", ""), # variable-size null-terminated char string
}
# Declare function types
......@@ -63,18 +63,18 @@ AVOID_SRV = HV_Services.AVOID_SRV
# Probe names
START = "ui.fakeStartButtonNoSpacing" #1125
LIDAR_STANDBY = "vis.sick.useStandByMode" #2877
ARM_ENABLED = "manip.armEnableRequest" #1082
GRIPPER_ENABLED = "manip.gripperEnableRequest" #1083
DRIVE_ENABLED = "drive.enableRequest" #580
DRIVE_COMMAND = "mob.driveCommand.enable" #1220
ROB_SENSOR = "vis.robotSensor.enabled" #3100
LINE_SENSOR = "vis.lineSensor.enabled" #3171
START = "ui.fakeStartButtonNoSpacing" # 1125
LIDAR_STANDBY = "vis.sick.useStandByMode" # 2877
ARM_ENABLED = "manip.armEnableRequest" # 1082
GRIPPER_ENABLED = "manip.gripperEnableRequest" # 1083
DRIVE_ENABLED = "drive.enableRequest" # 580
DRIVE_COMMAND = "mob.driveCommand.enable" # 1220
ROB_SENSOR = "vis.robotSensor.enabled" # 3100
LINE_SENSOR = "vis.lineSensor.enabled" # 3171
ESTOP = "ui.fakeEstop"
FLAG_PULL = "estop.fakeFlagPulled"
BEEP = "ui.enableBeeping" #1120
BEEP = "ui.enableBeeping" # 1120
HAS_POT = "manip.botGotPot"
......
......@@ -10,6 +10,7 @@ import sys
import subprocess
import rospy
class listen(threading.Thread):
# listener class for getting data from robot
def __init__(self, q, client):
......
......@@ -30,7 +30,8 @@ def twist_to_vel(twist):
def resource_name(name):
# formats a hostname to be legaly used in a ros pubisher/subscriber name
return ((name.replace("-", "")).replace(".", "_")).lower()
###
# functions that handle the formatting, reading and witing of tlv messages
# tlv is the structure of message that mp uses to communicate
......
......@@ -9,7 +9,13 @@ class Writer:
def get_msgs(self):
# collect the messages to send
return self.msgs
m = self.msgs
self.msgs = []
return m
def clear_msgs(self):
# clear msg array
self.msgs = []
def callback(self, msg):
# subscriber callback function
......@@ -21,7 +27,7 @@ class Writer:
# each message coresponds to a probe (1 to 1)
if len(msg) != len(self.probes):
raise Exception("message size missmatch")
self.msgs = zip(self.probes)
self.msgs = zip(self.probes, msg)
else:
# publish the same message to each of the probes
self.msgs = [(c, msg) for c in self.probes]
......@@ -36,13 +36,13 @@ class HVClient(Client):
name = resource_name(hostname)
if DRIVE_CMD in services:
# enable drive controller
self.cmdr.append(DriveController())
self.cmdr.append(DriveController(name))
if GRIPPER_CMD in services:
# enable gripper controller
self.cmdr.append(GripperController())
self.cmdr.append(GripperController(name))
if ARM_CMD in services:
# enable arm controller
self.cmdr.append(ArmController())
self.cmdr.append(ArmController(name))
if ODOM_LSTN in services:
# enable default odometry listener
......@@ -112,10 +112,10 @@ class HVClient(Client):
# sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true
# TODO: switch codes for enums
Client.write_probes(self,[(ESTOP, 1), (START, 0), (FLAG_PULL, 1)])
Client.write_probes(self, [(ESTOP, 1), (START, 0), (FLAG_PULL, 1)])
print "virtual e-stop triggered"
time.sleep(1)
Client.write_probes(self,[(ESTOP, 0), (FLAG_PULL, 0)])
Client.write_probes(self, [(ESTOP, 0), (FLAG_PULL, 0)])
print str(self.hostname) + " has stopped all running processes"
except Exception as e:
raise e
......@@ -142,25 +142,29 @@ class HVClient(Client):
)
msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr))
v = ""
if len(msgs) == 1:
msgs = ((msgs),)
if len(msgs) == 0:
return
print msgs
print len(msgs)
for (probe, value) in msgs:
probe_id = Client.lookup_probe_id(self,probe)
probe_type = self.probe_defs[probe_id][1]
probe_id = Client.lookup_probe_id(self, probe)
probe_type = Client.lookup_probe_type(self, probe_id)
v += make_tlv(probe_id, Client.encode_probe_value(self,probe_type, value))
Client.send_message(self,make_tlv(MP_TLV_WRITE_PROBES, v))
v += make_tlv(
probe_id, Client.encode_probe_value(self, probe_type, value)
)
Client.send_message(self, make_tlv(MP_TLV_WRITE_PROBES, v))
except Exception as e:
raise e
rospy.logwarn(e)
# raise e
def read(self, t, v):
# pass data to the correct child class
try:
name = self.probe_defs[t][0]
type = self.probe_defs[t][1]
val = Client.decode_probe_data(self, type, v)
(pname, ptype) = Client.lookup_probe(self, t)
val = Client.decode_probe_data(self, ptype, v)
[serv.add(name, val) for serv in self.lstnr]
[serv.add(pname, val) for serv in self.lstnr]
except Exception as e:
raise e
......
......@@ -87,7 +87,7 @@ if __name__ == "__main__":
"sys",
type=str,
nargs="*",
help="System included variables from launch file (not set bu user)"
help="System included variables from launch file (not set bu user)",
)
args = parser.parse_args()
......@@ -177,7 +177,7 @@ if __name__ == "__main__":
# default to 100. (mp runs at 200)
hz = 100
if hv_bot.hz:
hz = hv_bot.hz/2
hz = hv_bot.hz / 2
# Start the ROS main loop
rate = rospy.Rate(hz)
......
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