class LandingStateWithOpenJoints(smach.State):
def __init__(self, wait_time=20):
smach.State.__init__(self, outcomes=['success', 'fail'])
self.commander = HydrusCommander()
def execute(self, userdata):
self.commander.open_joints()
rospy.sleep(9.0)
self.commander.land()
rospy.sleep(self.wait_time)
return 'success'
class TakeoffStateWithJointsOpen(smach.State):
def __init__(self, wait_time=20):
smach.State.__init__(self, outcomes=['success', 'fail'])
self.commander = HydrusCommander()
self.wait_time = wait_time
def execute(self, userdata):
self.commander.arm_and_takeoff()
rospy.sleep(self.wait_time)
self.commander.open_joints()
rospy.sleep(7)
return 'success'
class PrepareTaskState(smach.State):
def __init__(self, wait_time=20):
smach.State.__init__(
self,
outcomes=['first_waypoint', 'current_waypoint', 'fail'],
input_keys=[
'first_waypoint_flag_input', 'return_first_waypoint_flag_input'
],
output_keys=['first_waypoint_flag_output'])
self.task_commander = TaskCommander()
self.hydrus_commander = HydrusCommander()
def execute(self, userdata):
## 1. estimation stops
## 2. joints open to initial state
self.task_commander.stop_estimation()
self.hydrus_commander.open_joints()
if userdata.first_waypoint_flag_input or userdata.return_first_waypoint_flag_input:
userdata.first_waypoint_flag_output = False
return 'first_waypoint'
else:
return 'current_waypoint'