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, "")