import roslib; roslib.load_manifest('uf_smach') from auvsi_robosub import constants from uf_smach import common_states, object_finder_states, legacy_vision_states, missions from object_finder.msg import TargetDesc import numpy import smach SEARCH_WIDTH = 4 # How far to strafe SEARCH_ADVANCE = 1.5 # How far to move forward after strafing buoy_desc = TargetDesc() buoy_desc.type = TargetDesc.TYPE_SPHERE buoy_desc.sphere_radius = 4*.0254 # 4 in buoy_desc.prior_distribution.pose.orientation.w = 1 buoy_desc.min_dist = 2 buoy_desc.max_dist = 6 def make_buoy(shared): buoy_sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with buoy_sm: smach.Sequence.add('DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(2.5))) smach.Sequence.add('APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add('WAIT_BUOYS', object_finder_states.WaitForObjectsState(shared, 'find_forward', lambda: [buoy_desc], .90), transitions={'timeout': 'failed'}) smach.Sequence.add('APPROACH_BUOY', object_finder_states.ApproachObjectState(shared,
import roslib roslib.load_manifest('uf_smach') from auvsi_robosub import constants from uf_smach import common_states, object_finder_states, legacy_vision_states, missions from object_finder.msg import TargetDesc import numpy import smach SEARCH_WIDTH = 4 # How far to strafe SEARCH_ADVANCE = 1.5 # How far to move forward after strafing buoy_desc = TargetDesc() buoy_desc.type = TargetDesc.TYPE_SPHERE buoy_desc.sphere_radius = 4 * .0254 # 4 in buoy_desc.prior_distribution.pose.orientation.w = 1 buoy_desc.min_dist = 2 buoy_desc.max_dist = 6 def make_buoy(shared): buoy_sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded') with buoy_sm: smach.Sequence.add( 'DEPTH', common_states.WaypointState(shared, lambda cur: cur.depth(2.5))) smach.Sequence.add( 'APPROACH', common_states.VelocityState(shared, numpy.array([.2, 0, 0]))) smach.Sequence.add('WAIT_BUOYS', object_finder_states.WaitForObjectsState(