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