def handleSrv(self, req): if req.start_request: rospy.loginfo("Received Start Request") self.isAborted = False self.defaultDepth = req.start_ctrl.depth_setpoint self.laneSearchDepth = self.defaultDepth self.inputHeading = req.start_ctrl.heading_setpoint self.expectedLanes = req.numLanes self.chosenLane = self.LEFT if req.chosenLane == 0 else self.RIGHT rospy.loginfo("Input: d={}, h={}, num={}, chosen={}".format( self.defaultDepth, self.inputHeading, self.expectedLanes, self.chosenLane)) return mission_to_visionResponse( start_response=True, abort_response=False, data=controller(heading_setpoint=self.curHeading)) elif req.abort_request: rospy.loginfo("Received Abort Request") self.motionClient.cancel_all_goals() self.isAborted = True while not self.abortedDone: rospy.sleep(rospy.Duration(0.3)) self.motionClient.cancel_all_goals() return mission_to_visionResponse( start_response=False, abort_response=True, data=controller(heading_setpoint=self.curHeading))
def handleSrv(self, req): if req.start_request: rospy.loginfo("Received Start Request") self.isAborted = False self.defaultDepth = req.start_ctrl.depth_setpoint self.laneSearchDepth = self.defaultDepth self.inputHeading = req.start_ctrl.heading_setpoint self.expectedLanes = req.numLanes self.chosenLane = self.LEFT if req.chosenLane == 0 else self.RIGHT rospy.loginfo("Input: d={}, h={}, num={}, chosen={}".format( self.defaultDepth, self.inputHeading, self.expectedLanes, self.chosenLane)) return mission_to_visionResponse(start_response=True, abort_response=False, data=controller(heading_setpoint= self.curHeading)) elif req.abort_request: rospy.loginfo("Received Abort Request") self.motionClient.cancel_all_goals() self.isAborted = True while not self.abortedDone: rospy.sleep(rospy.Duration(0.3)) self.motionClient.cancel_all_goals() return mission_to_visionResponse(start_response=False, abort_response=True, data=controller(heading_setpoint= self.curHeading))
def handle_srv(self, req): rospy.loginfo("RGB Service handled") if req.start_request: rospy.loginfo("RGB starting") self.isStart = True self.isAborted = False self.canPublish = True self.missionStart = time.time() self.defaultDepth = req.start_ctrl.depth_setpoint self.inputHeading = req.start_ctrl.heading_setpoint self.curHeading = self.inputHeading self.depthFromMission = self.defaultDepth self.registerMission() rospy.loginfo("Received heading: {}".format(self.inputHeading)) rospy.loginfo("Received depth: {}".format(self.defaultDepth)) return mission_to_visionResponse(start_response=True, abort_response=False, data=controller(heading_setpoint=self.inputHeading)) if req.abort_request: rospy.loginfo("RGB abort received") self.isAborted=True # self.isStart = False self.canPublish = False self.sendMovement(forward=0.0, sidemove=0.0) self.unregisterMission() rospy.loginfo("Time taken: {}".format(time.time()-self.missionStart)) rospy.loginfo("Aborted complete") return mission_to_visionResponse(start_response=False, abort_response=True, data=controller(heading_setpoint=self.curHeading))
def handleSrv(self, req): if req.start_request: rospy.loginfo("Received Start Request") self.isAborted = False self.defaultDepth = req.start_ctrl.depth_setpoint self.inputHeading = req.start_ctrl.heading_setpoint return mission_to_visionResponse(start_response=True, abort_response=False, data=controller(heading_setpoint= self.curHeading)) elif req.abort_request: rospy.loginfo("Received Abort Request!!!") self.motionClient.cancel_all_goals() self.isAborted = True while not self.abortedDone: rospy.sleep(rospy.Duration(0.3)) self.motionClient.cancel_all_goals() return mission_to_visionResponse(start_response=False, abort_response=True, data=controller(heading_setpoint= self.curHeading))
def handle_srv(self, req): global isStart global isAborted global locomotionGoal rospy.loginfo("Pegs Service handled") if req.start_request: rospy.loginfo("Pegs starting") self.isStart = True self.isAborted = False self.canPublish = True self.defaultDepth = req.start_ctrl.depth_setpoint self.inputHeading = req.start_ctrl.heading_setpoint self.curHeading = self.inputHeading self.registerMission() rospy.loginfo("Received heading: {}".format(self.inputHeading)) rospy.loginfo("Received depth: {}".format(self.defaultDepth)) return mission_to_visionResponse( start_response=True, abort_response=False, data=controller(heading_setpoint=self.curHeading)) elif req.abort_request: rospy.loginfo("Pegs abort received") self.sendMovement(forward=0.0, sidemove=0.0) self.isAborted = True self.isStart = False self.canPublsih = False self.unregisterMission() return mission_to_visionResponse( start_response=False, abort_response=True, data=controller(heading_setpoint=self.curHeading))
def handle_srv(self, req): global isStart global isAborted global locomotionGoal rospy.loginfo("Pegs Service handled") if req.start_request: rospy.loginfo("Pegs starting") self.isStart = True self.isAborted = False self.canPublish = True self.defaultDepth = req.start_ctrl.depth_setpoint self.inputHeading = req.start_ctrl.heading_setpoint self.curHeading = self.inputHeading self.registerMission() rospy.loginfo("Received heading: {}".format(self.inputHeading)) rospy.loginfo("Received depth: {}".format(self.defaultDepth)) return mission_to_visionResponse(start_response=True, abort_response=False, data=controller(heading_setpoint= self.curHeading)) elif req.abort_request: rospy.loginfo("Pegs abort received") self.sendMovement(forward=0.0, sidemove=0.0) self.isAborted=True self.isStart = False self.canPublsih = False self.unregisterMission() return mission_to_visionResponse(start_response=False, abort_response=True, data=controller(heading_setpoint=self.curHeading))
def handle_srv(self, req): global isStart global isAborted global locomotionGoal rospy.loginfo("Torpedo Service handled") if req.start_request: rospy.loginfo("Torpedo starting") self.isStart = True self.isAborted = False self.canPublish = True self.defaultDepth = req.start_ctrl.depth_setpoint self.inputHeading = req.start_ctrl.heading_setpoint self.curHeading = self.inputHeading self.depthFromMission = self.defaultDepth rospy.loginfo("Received depth: {}".format(self.defaultDepth)) rospy.loginfo("Received heading: {}".format(self.inputHeading)) self.registerMission() self.regCompass() # self.registerSonar() # self.curHeading = self.curHeading - 10.0 # self.sendMovement(forward=0.0, sidemove=0.0, # heading=self.curHeading, blocking=True) return mission_to_visionResponse(start_response=True, abort_response=False, data=controller(heading_setpoint= self.curHeading)) elif req.abort_request: rospy.loginfo("Torpedo abort received") self.isAborted=True # self.isStart = False self.canPublish = False self.unregisterMission() # Randomly shoot stuff if self.numShoot == 0: self.shootTopTorpedo() else: self.shootBotTorpedo() self.sendMovement(forward=0.0, sidemove=0.0) rospy.loginfo("Aborted complete") rospy.loginfo("Time taken: {}".format(time.time()-self.missionStart)) # if self.isMovingState: # return mission_to_visionResponse(start_response=False, # abort_response=True, # search_request=True, # data=controller(heading_setpoint= # self.curHeading)) # else: return mission_to_visionResponse(start_response=False, abort_response=True, data=controller(heading_setpoint= self.curHeading))