#!/usr/bin/env python
import sys
import rospy
import samplereturn.util as util
from executive import costmap_check
if __name__=="__main__":
rospy.init_node('costmap_checker')
#to allow logging immediately, we must wait for the rosout node to be alive
util.wait_for_rosout()
checker = costmap_check.ExecutiveCostmapChecker()
rospy.spin()
break
retries -= 1
return not (self.motion_mode == SelectMotionModeRequest.MODE_PAUSE)
def pause(self, state):
rospy.logdebug("pause %s", state)
self.motion_mode = self.select_motion_mode( SelectMotionModeRequest.MODE_QUERY ).mode
if ((state and self.motion_mode == SelectMotionModeRequest.MODE_PAUSE) and
(self.carousel_servo_status == (not state))):
self.set_pause_state(state)
return
carousel_toggled = not state == self.enable_carousel(not state)
wheelpods_toggled = not state == self.enable_wheelpods(not state)
manipulator_toggled = self.manipulator_pause_service(state).state == state
if wheelpods_toggled and carousel_toggled and manipulator_toggled:
self.set_pause_state(state)
self.announce_pause_state()
else:
state_str = 'paused' if state else 'active'
message = "Wheelpods %s, "%(state_str if wheelpods_toggled else "failed")
message += "Carousel %s, "%(state_str if carousel_toggled else "failed")
message += "Manipulator %s. "%(state_str if manipulator_toggled else "failed")
self.say(message)
if __name__=="__main__":
rospy.init_node('pause_switch')
wait_for_rosout()
p=PauseSwitch()
rospy.spin()
