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

add services to hv_client class

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