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(
