def handle_reset(self, msg):
self.sum = 0
self.n = 0
result = TriggerResponse()
result.success = True
return result
def handleIsEnabledSrv(self, req):
try:
res = TriggerResponse()
res.success = self.backgroundMovementProxy.isEnabled()
return res
except RuntimeError, e:
rospy.logerr("Exception caught:\n%s", e)
return None
def _save(self, req):
rospy.logerr('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
rospy.logerr('Save request is called to image buffer.')
rospy.logerr('<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<')
self.stamp = rospy.Time.now()
self.pub_imgs = None
res = TriggerResponse()
res.success = True
return res
def handleGetLifeSrv(self, req):
try:
res = TriggerResponse()
res.success = True
res.message = self.lifeProxy.getState()
rospy.loginfo("current life state is " + str(res.message))
return res
except RuntimeError, e:
rospy.logerr("Exception while getting life state:\n%s", e)
return None
def right_interaction_arm_up_cb(self, req):
resp = TriggerResponse()
if self.right_arm_mann:
rospy.logerr('Right arm in interactive mode')
resp.success = False
else:
resp.success = self.move(self.group_right, target="up_right_arm")
return resp
def look_at_right_feeder_cb(self, req):
resp = TriggerResponse()
point = PointStamped()
point.header.frame_id = "marker"
point.point.x = 1.825
point.point.y = 0.205
point.point.z = 0.0875
self.look_at_cb(point, send_and_wait=False)
resp.success = True
return resp
def train_operator_callback(self, req):
print 'traing operator'
result = TriggerResponse()
result.success = False
with self._l:
if self._rgb_img is None:
return result
if self.memorize_operator(self._rgb_img):
result.success = True
return result
result.success = False
return result
def left_interaction_get_ready_cb(self, req):
resp = TriggerResponse()
if self.left_arm_mann:
rospy.logerr('Left arm in interactive mode')
resp.success = False
else:
resp.success = self.move(self.group_left, target="up_left_arm")
return resp
def look_at_default_cb(self, req):
resp = TriggerResponse()
pt = PointStamped()
pt.header.frame_id = "base_link"
pt.point.x = 0.4
pt.point.y = -0.15
pt.point.z = 0.8
self.look_at_cb(pt, send_and_wait=False)
resp.success = True
return resp
def right_interaction_move_to_user_cb(self, req):
resp = TriggerResponse()
if self.right_arm_mann:
rospy.logerr('Right arm in interactive mode')
resp.success = False
else:
pose = PoseStamped()
pose.pose.position.x = 0.7
pose.pose.position.y = -0.1
pose.pose.position.z = 1.15
pose.pose.orientation.w = 1
pose.header.frame_id = "base_link"
resp.success = self.move(self.group_right, pose=pose)
return resp
def left_interaction_move_to_user_cb(self, req):
resp = TriggerResponse()
if self.left_arm_mann:
rospy.logerr('Left arm in interactive mode')
resp.success = False
else:
pose = PoseStamped()
pose.pose.position.x = 0.7
pose.pose.position.y = 0.1
pose.pose.position.z = 1.2
pose.pose.orientation.w = 1
pose.header.frame_id = "base_link"
# pose_transformed = self.tf_listener.transformPose(pose, self.group_left.get_planning_frame())
resp.success = self.move(self.group_left, pose=pose)
return resp
def left_interaction_off_cb(self, req):
resp = TriggerResponse()
if not self.left_arm_mann:
rospy.logerr('Left arm already in normal mode')
resp.success = True
else:
self.switch_req.stop_controllers = [self.mannequin_controllers[1]]
self.switch_req.start_controllers = [self.standard_controllers[1]]
res = self.switch_control(self.switch_req)
if res.ok:
self.left_arm_mann = False
self.left_int_pub.publish(False)
resp.success = True
else:
resp.success = False
resp.message = "Left arm: failed to switch interaction to OFF"
return resp
def right_interaction_on_cb(self, req):
resp = TriggerResponse()
if self.right_arm_mann:
rospy.logerr('Right arm already in interactive mode')
resp.success = True
else:
self.switch_req.stop_controllers = [self.standard_controllers[0]]
self.switch_req.start_controllers = [self.mannequin_controllers[0]]
res = self.switch_control(self.switch_req)
if res.ok:
self.right_arm_mann = True
self.right_int_pub.publish(True)
resp.success = True
else:
resp.success = False
resp.message = "Left arm: failed to switch interaction to ON"
return resp
def executeStacking(req):
'''
stacking executing
'''
rospy.sleep(5.)
return TriggerResponse(success=True, message="Stacking is executed!")
def executeFetching(req):
'''
fetching executing
'''
rospy.sleep(5.)
return TriggerResponse(success=True, message="Fetching is executed!")
def restart_srv(self, req):
rospy.loginfo('Restart request - new wall pattern')
self.restart()
return TriggerResponse()
def setMimicSurprised_cb(req):
sss.set_mimic("mimic", ["surprised", 0, 1])
return TriggerResponse(True, "")
def __is_status_paused(request):
response = TriggerResponse()
response.success = (CodeStatus.PAUSED == CodeStatus.get_current_status())
return response
mani_resp = mani_move_srv(temp_mani_req)
except (rospy.ServiceException, rospy.ROSException), e:
print "Service call failed: %s"%e
rospy.sleep(0.3)
temp_mani_req.pose.position.y += Y_DIS
try:
rospy.wait_for_service(Move_srv, timeout=10)
mani_move_srv = rospy.ServiceProxy(Move_srv, manipulation)
mani_resp = mani_move_srv(temp_mani_req)
except (rospy.ServiceException, rospy.ROSException), e:
print "Service call failed: %s"%e
rospy.sleep(0.3)
self.shelf_count += 1
return TriggerResponse(success=True, message="Request accepted.")
def read_commodity(self, path):
for line in open(path, "r"):
line = line.rstrip('\n')
self.commodity_list.append(line)
print "Node (FSM): Finish reading list"
def initial(self):
self.mani_req = manipulationRequest()
self.last_img = 0
self.last_depth = 0
self.last_mask = 0
self.last_count = 0
self.rot = 0
def onoff_response(self, onoff):
d = TriggerResponse()
d.success = self.set_power(onoff)
d.message = "ON" if self.is_on else "OFF"
return d
def stay_idle(self):
self.overlaid_object.stay_idle()
return TriggerResponse(True, 'human: stay idle')
def obj_reset(self):
self.overlaid_object.obj_reset()
self.is_reset = False
return TriggerResponse(True, 'object: reset')
def look_around(self):
if not self.overlaid_object.is_la:
self.overlaid_object.look_around()
return TriggerResponse(True, 'human: look around')
else:
return TriggerResponse(True, 'human: action(look around) is not finished')
def reset_shelf(self, req):
self.shelf_count = 0
self.shelf_list = []
return TriggerResponse(success=True, message="Request accepted.")
def srv_start(self, req):
self.state = Initial
self.last_state = Initial
return TriggerResponse(success=True, message="Request accepted.")
def zeor_pressure(req):
rospy.loginfo('Zero pressure sensor')
dvl.write('CPZ\r\n')
return TriggerResponse(success=True)
def executeLookforward(req):
'''
Lookforward executing
'''
rospy.sleep(5.)
return TriggerResponse(success=True, message="Stacking is executed!")
def executeCaption(req):
'''
caption executing
'''
rospy.sleep(5.)
return TriggerResponse(success=True, message="Caption is executed!")
def listen_srv(content):
res = listen()
if res:
return TriggerResponse(success=True, message=res)
else:
return TriggerResponse(success=False, message="Failed to record")
def resetFilter(self, srv):
self.fusionF.resetFilter()
return TriggerResponse(True, "Filter reset")
def __is_status_paused(request):
response = TriggerResponse()
response.success = (
CodeStatus.PAUSED == CodeStatus.get_current_status())
return response
def setMimicAfraid_cb(req):
sss.set_mimic("mimic", ["afraid", 0, 1])
return TriggerResponse(True, "")
def setLightGreen_cb(req):
sss.set_light("light_base", "green")
sss.set_light("light_torso", "green")
return TriggerResponse(True, "")
def setMimicYes_cb(req):
sss.set_mimic("mimic", ["yes", 0, 1])
return TriggerResponse(True, "")
def resetCallback(self, req):
print 'Reset'
return TriggerResponse(True, 'Reset')
def setLightRed_cb(req):
sss.set_light("light_base", "red")
sss.set_light("light_torso", "red")
return TriggerResponse(True, "")
def _handle_set_odom(self, req):
self._odom_updated = False
res = TriggerResponse(True, "map -> odom tf set.")
return res
def setMimicWakingUp_cb(req):
sss.set_mimic("mimic", ["waking_up", 0, 1])
return TriggerResponse(True, "")
def setMimicAngry_cb(req):
sss.set_mimic("mimic", ["angry", 0, 1])
return TriggerResponse(True, "")
def server_callback(req):
rospy.loginfo('Server callback!')
resp = TriggerResponse()
resp.success = True
resp.message = 'OK'
return resp
def soundHello_cb(req):
sss.say("sound", [
"Hello, my name is Care O bot, a mobile service robot from Mojin Robotics."
])
return TriggerResponse(True, "")
def executeGrasping(req):
'''
grasping executing
'''
rospy.sleep(5.)
return TriggerResponse(success=True, message="Grasping is executed!")
def executePlacing(req):
'''
placing executing
'''
rospy.sleep(5.)
return TriggerResponse(success=True, message="Placing is executed!")
def reconfigureCallback(self, req):
# TODO syncronization. It is dangerous not to use syncronization.
self.configure()
return TriggerResponse(success=self.ok)
def setMimicTired_cb(req):
sss.set_mimic("mimic", ["tired", 0, 1])
return TriggerResponse(True, "")