Commit 9e137ca4 authored by Nicholas Shindler's avatar Nicholas Shindler
Browse files

add services to hv_client class

parent 17fa7e54
[flake8] [flake8]
use-flake8-tabs=true use-flake8-tabs=true
max-line-length=88 max-line-length=88
tab-width=2 tab-width=4
blank-lines-indent="never" blank-lines-indent="never"
# ignore = ET127
...@@ -103,3 +103,10 @@ ODOM_GYRO_Y = "loc.rawOdometryWGyro.y" ...@@ -103,3 +103,10 @@ ODOM_GYRO_Y = "loc.rawOdometryWGyro.y"
ODOM_GYRO_H = "loc.rawOdometryWGyro.h" ODOM_GYRO_H = "loc.rawOdometryWGyro.h"
ODOM_GYRO_V = "loc.rawOdometryWGyro.v" ODOM_GYRO_V = "loc.rawOdometryWGyro.v"
ODOM_GYRO_W = "loc.rawOdometryWGyro.w" ODOM_GYRO_W = "loc.rawOdometryWGyro.w"
FOLLOW_ME = "behaviors.followMe.run"
BOT_GOT_POT = "manip.botGotPot"
SET_PICK_X = "behaviors.spacing.pick.pickTarget.x"
SET_PICK_Y = "behaviors.spacing.pick.pickTarget.y"
RUN_SPACING = "behaviors.spacing.runCollection"
AVOID = "behaviors.scriptAvoidCollision.isEnabled"
...@@ -12,7 +12,7 @@ PSEUDO_START_CODE = 1125 ...@@ -12,7 +12,7 @@ PSEUDO_START_CODE = 1125
class AvoidService: class AvoidService:
def __init__(self, mp, ros): def __init__(self, mp, ros):
# contstruct basics for # contstruct basics for
self.mp = mp Writer.__init__(self, [AVOID])
self.serv = ros.Service("AvoidCollision", mp_toggle, self.toggle_avoid) self.serv = ros.Service("AvoidCollision", mp_toggle, self.toggle_avoid)
def stop(self): def stop(self):
...@@ -26,11 +26,5 @@ class AvoidService: ...@@ -26,11 +26,5 @@ class AvoidService:
def toggle_avoid(self, req): def toggle_avoid(self, req):
# toggle follow person on/off # toggle follow person on/off
if req.val == 1: Writer.callback(self, 1 if req.val == 1 else 0)
# self.mp.write_probe(PSEUDO_START_CODE,0)
self.mp.write_probe(AVOID_CODE, 1)
else:
self.mp.write_probe(AVOID_CODE, 0)
# self.mp.write_probe(PSEUDO_START_CODE,1)
return "success: value set to " + ("true" if req.val == 1 else "false") return "success: value set to " + ("true" if req.val == 1 else "false")
#!/usr/bin/env python #!/usr/bin/env python
# import rospy import rospy
from hv_bridge.srv import mp_toggle, mp_toggleResponse from hv_bridge.srv import mp_toggle, mp_toggleResponse
import sys, os
# probe id 3842: behaviors.followMe.run type bool length 1 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 *
FOLLOW_ME_CODE = 3842 # probe id 3842: behaviors.followMe.run type bool length 1
PSEUDO_START_CODE = 1125
class FollowService: class FollowService(Writer):
def __init__(self, mp, ros): def __init__(self):
# contstruct basics for # contstruct basics for
self.mp = mp Writer.__init__(self, [FOLLOW_ME])
self.serv = ros.Service("FollowMe", mp_toggle, self.toggle_follow) self.serv = rospy.Service("FollowMe", mp_toggle, self.toggle_follow)
def stop(self): def stop(self):
try: try:
...@@ -25,11 +29,5 @@ class FollowService: ...@@ -25,11 +29,5 @@ class FollowService:
def toggle_follow(self, req): def toggle_follow(self, req):
# toggle follow person on/off # toggle follow person on/off
if req.val == 1: Writer.callback(self, 1 if req.val == 1 else 0)
# self.mp.write_probe(PSEUDO_START_CODE,0)
self.mp.write_probe(FOLLOW_ME_CODE, 1)
else:
self.mp.write_probe(FOLLOW_ME_CODE, 0)
# self.mp.write_probe(PSEUDO_START_CODE,1)
return "success: value set to " + ("true" if req.val == 1 else "false") return "success: value set to " + ("true" if req.val == 1 else "false")
#!/usr/bin/env python #!/usr/bin/env python
# import rospy import rospy
from hv_bridge.srv import mp_toggle, mp_toggleResponse from hv_bridge.srv import mp_toggle, mp_toggleResponse
from hv_bridge.srv import mp_set_target, mp_set_targetResponse from hv_bridge.srv import mp_set_target, mp_set_targetResponse
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 *
# probe id 3486: behaviors.spacing.pick.pickTarget.x type float length 4 # probe id 3486: behaviors.spacing.pick.pickTarget.x type float length 4
# probe id 3487: behaviors.spacing.pick.pickTarget.y type float length 4 # probe id 3487: behaviors.spacing.pick.pickTarget.y type float length 4
# probe id 3304: behaviors.spacing.runCollection type bool length 1 # probe id 3304: behaviors.spacing.runCollection type bool length 1
# probe id 2438: loc.actual.x type float length 4 # probe id 2438: loc.actual.x type float length 4
# probe id 2439: loc.actual.y type float length 4 # probe id 2439: loc.actual.y type float length 4
RUN_SPACING_CODE = 3304
SET_X_CODE = 3486
SET_Y_CODE = 3487
SET_X_LOC_CODE = 2438 SET_X_LOC_CODE = 2438
SET_Y_LOC_CODE = 2439 SET_Y_LOC_CODE = 2439
SET_H_LOC_CODE = 2440 SET_H_LOC_CODE = 2440
PSEUDO_START_CODE = 1125
BOT_GOT_POT_CODE = 1100
POT_IN_GRIPPER = 3151
class PickTargetService: class PickTargetService(Writer):
def __init__(self, mp, ros): def __init__(self, mp, ros):
# contstruct basics for # contstruct basics for
self.mp = mp Writer.__init__(self, [BOT_GOT_POT, SET_PICK_X, SET_PICK_Y])
self.serv_set = ros.Service( self.serv_set = ros.Service(
"SetPickTarget", mp_set_target, self.set_pick_target "SetPickTarget", mp_set_target, self.set_pick_target
) )
self.serv_run = ros.Service("RunPick", mp_toggle, self.toggle_pick)
def stop(self): def stop(self):
try: try:
self.serv_set.shutdown("end of life") self.serv_set.shutdown("end of life")
self.serv_run.shutdown("end of life")
print("pick target service ended") print("pick target service ended")
return True return True
except: except:
...@@ -40,18 +39,26 @@ class PickTargetService: ...@@ -40,18 +39,26 @@ class PickTargetService:
def set_pick_target(self, req): def set_pick_target(self, req):
# set values for target # set values for target
self.mp.write_probe(BOT_GOT_POT_CODE, 0) Writer.callback(self, (0, req.x, req.y))
self.mp.write_probes([(SET_X_CODE, req.x), (SET_Y_CODE, req.y)])
return "suceess: set target to (" + str(req.x) + "," + str(req.y) + ")" return "suceess: set target to (" + str(req.x) + "," + str(req.y) + ")"
class TogglePickTargetService(Writer):
def __init__(self, mp, ros):
# contstruct basics for
Writer.__init__(self, [RUN_SPACING])
self.serv_run = ros.Service("RunPick", mp_toggle, self.toggle_pick)
def stop(self):
try:
self.serv_run.shutdown("end of life")
print("pick target service ended")
return True
except:
print("pick target service shutdown failed")
return False
def toggle_pick(self, req): def toggle_pick(self, req):
# toggle follow person on/off # toggle follow person on/off
if req.val == 1: Writer.callback(self, 1 if req.val == 1 else 0)
# self.mp.write_probe(PSEUDO_START_CODE,0)
self.mp.write_probe(RUN_SPACING_CODE, 1)
else:
self.mp.write_probe(RUN_SPACING_CODE, 0)
# self.mp.write_probe(PSEUDO_START_CODE,1)
return "success: value set to " + ("true" if req.val == 1 else "false") return "success: value set to " + ("true" if req.val == 1 else "false")
...@@ -71,15 +71,15 @@ class HVClient(Client): ...@@ -71,15 +71,15 @@ class HVClient(Client):
if FOLLOW_SRV in services: if FOLLOW_SRV in services:
# enable odometry with gryro correction listener # enable odometry with gryro correction listener
# self.srvs.append(FollowService()) self.srvs.append(FollowService())
pass pass
if PICK_SRV in services: if PICK_SRV in services:
# enable odometry with gryro correction listener # enable odometry with gryro correction listener
# self.svrs.append(PickTargetService()) self.svrs.append(PickTargetService())
pass pass
if AVOID_SRV in services: if AVOID_SRV in services:
# enable odometry with gryro correction listener # enable odometry with gryro correction listener
# self.srvs.append(AvoidService()) self.srvs.append(AvoidService())
pass pass
Client.__init__(self, hostname) Client.__init__(self, hostname)
...@@ -110,6 +110,8 @@ class HVClient(Client): ...@@ -110,6 +110,8 @@ class HVClient(Client):
def stop(self): def stop(self):
# end everything that is running # end everything that is running
try: try:
# turn off ROS services
self.end_ros_services()
# sets the start to FALSE. this 'stops' the robot. # sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true # sets flag pulled to true
Client.stop_probes(self) Client.stop_probes(self)
...@@ -122,7 +124,12 @@ class HVClient(Client): ...@@ -122,7 +124,12 @@ class HVClient(Client):
rospy.logerr(e) rospy.logerr(e)
raise e raise e
finally: finally:
# disconnect client connection
Client.disconnect(self) Client.disconnect(self)
# clear all services
self.cmdr = []
self.lstnr = []
self.srvs = []
def enable_services(self): def enable_services(self):
# take all probes that need to be listened for and enable them # take all probes that need to be listened for and enable them
...@@ -147,7 +154,10 @@ class HVClient(Client): ...@@ -147,7 +154,10 @@ class HVClient(Client):
get_msg_arr = ( get_msg_arr = (
lambda a, b: get_msg_arr(a + b[0], b[1:]) if len(b) > 1 else a + b[0] lambda a, b: get_msg_arr(a + b[0], b[1:]) if len(b) > 1 else a + b[0]
) )
# collect all subscription messages to write to robot
msgs = get_msg_arr([], map((lambda a: a.get_msgs()), self.cmdr)) 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))
v = "" v = ""
if len(msgs) == 0: if len(msgs) == 0:
return return
...@@ -206,3 +216,11 @@ class HVClient(Client): ...@@ -206,3 +216,11 @@ class HVClient(Client):
lstn.publish() lstn.publish()
except Exception as e: except Exception as e:
raise e raise e
def end_ros_services(self):
# stop all ROS services
try:
for ros_srv in self.servs:
ros_srv.stop()
except Exception as e:
raise e
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