def execute(self, userdata):
sub = rospy.Subscriber('/hydrophones/processed', ProcessedPing, self._callback)
no_ping_ctr = 0
while not self.preempt_requested():
with self._cond:
self._cond.wait(.5)
if self._ping is None:
no_ping_ctr += 1
if no_ping_ctr == 5:
self._shared['moveto'].send_goal(
PoseEditor.from_PoseTwistStamped_topic('/trajectory'))
if no_ping_ctr >= 10:
return 'failed'
continue
ping = self._ping
self._ping = None
no_ping_ctr = 0
goal = self._compute_goal(ping)
if isinstance(goal, str):
self._shared['moveto'].send_goal(
PoseEditor.from_PoseTwistStamped_topic('/trajectory'))
return goal
else:
self._shared['moveto'].send_goal(goal)
return 'preempted'
def _feedback_cb(self, feedback):
with self._cond:
good_results = map(json.loads, feedback.targetreses[0].object_results)
#print good_results
if len(good_results) == 0:
self._fail_ctr += 1
if self._fail_ctr > 10:
self._failed = True;
self._cond.notify_all()
return
else:
self._fail_ctr = 0
current = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
body_from_result_tf = {
'/forward_camera': ((0.566, 0.0, 0.049), (0.5, -0.5, 0.5, -0.5)),
'/down_camera': ((0.297, 0.0, -0.102), (0.7071067811865476, -0.7071067811865475, 0.0, 0.0)),
}[feedback.header.frame_id]
result = self._selector(good_results, self._traj_start, body_from_result_tf)
goal = self._get_goal(result, current, body_from_result_tf)
if goal is None:
self._done = True
self._cond.notify_all()
return
self._shared['moveto'].send_goal(goal)
def _compute_goal(self, ping):
current = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
new = current.yaw_left(ping.heading)
if abs(ping.heading) < 15/180*math.pi:
self._stall_ctr = max(self._stall_ctr - 1, 0)
if ping.declination < 30/180*math.pi:
speed = .7
self._good_ctr = 0
elif ping.declination < 40/180*math.pi:
speed = .3
self._good_ctr = 0
else:
speed = .1
if ping.declination > 55/180*math.pi:
self._good_ctr += 1
if self._good_ctr >= 2:
return 'succeeded'
else:
self._good_ctr = 0
else:
self._stall_ctr += 1
if self._stall_ctr > 5:
return 'failed'
speed = 0
print 'heading', ping.heading/math.pi*180, 'declination', ping.declination/math.pi*180, 'speed', speed, 'stall', self._stall_ctr
return new.as_MoveToGoal(linear=[speed, 0, 0])
def shooter_desc_cb():
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
desc = TargetDesc()
desc.type = TargetDesc.TYPE_OBJECT
desc.object_filename = roslib.packages.resource_file('auvsi_robosub', 'models', '2013/shooter.obj')
desc.prior_distribution.pose.orientation = Quaternion(*traj.turn_left_deg(180).orientation)
desc.disallow_yawing = True
desc.min_dist = BOARD_DIST-1
desc.max_dist = BOARD_DIST+3
return [desc]
def _compute_goal(self, ping):
current = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
if ping.declination < self._last_declination and ping.declination > math.radians(80):
return 'succeeded'
self._last_declination = ping.declination
speed = .1 if ping.declination < math.radians(75) else .05
vel = speed*numpy.array([math.cos(ping.heading), math.sin(ping.heading), 0])
print 'heading', ping.heading/math.pi*180, 'declination', ping.declination/math.pi*180, 'vel', vel
return current.as_MoveToGoal(linear=vel.tolist())
def shooter_desc_cb():
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
desc = TargetDesc()
desc.type = TargetDesc.TYPE_OBJECT
desc.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/shooter.obj')
desc.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
desc.disallow_yawing = True
desc.min_dist = BOARD_DIST - 1
desc.max_dist = BOARD_DIST + 3
return [desc]
def _get_target(self, result):
origin = PoseEditor.from_Pose(self._traj_start.frame_id, result.pose)
markers = dict((markerpoint.name, xyz_array(markerpoint.position))
for markerpoint in result.markers)
pose = origin.relative(markers[self._marker]) \
if self._marker is not None else origin
target = self._traj_start
#target = target.look_at_without_pitching(pose.position)
target = target.set_position(pose.position)
target = target.relative(-self._approach_pos) \
.backward(self._approach_dist)
return target
def execute(self, userdata):
self._done = False
self._failed = False
self._failed_ctr = 0
self._traj_start = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
self._shared[self._action].set_callbacks(
feedback_cb=lambda feedback: self._feedback_cb(feedback, self._shared))
with self._cond:
while not self._done and not self._failed:
self._cond.wait()
self._shared.clear_callbacks()
return 'succeeded' if self._done else 'failed'
def execute(self, userdata):
self._done = False
self._failed = False
self._fail_ctr = 0
self._traj_start = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
self._shared[self._action].set_callbacks(feedback_cb=self._feedback_cb)
with self._cond:
while not (self._done) and not self._failed and not self.preempt_requested():
self._cond.wait(.1)
self._shared.clear_callbacks()
if self.preempt_requested():
return 'preempted'
return 'succeeded' if self._done else 'failed'
def hexagon_desc_cb(size, color):
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
desc = TargetDesc()
desc.type = TargetDesc.TYPE_OBJECT
desc.object_filename = roslib.packages.resource_file('auvsi_robosub', 'models', '2013/shooter_%sin_%s_hexagon.obj' % (7 if size == 'small' else 12, color))
desc.prior_distribution.pose.orientation = Quaternion(*traj.turn_left_deg(180).orientation)
cov = numpy.zeros((6, 6))
a = numpy.array([traj.forward_vector]).T * 100
cov[3:, 3:] += a.dot(a.T)
desc.prior_distribution.covariance = cov.flatten()
desc.min_dist = HEXAGON_DIST*.9
desc.max_dist = HEXAGON_DIST/.9
desc.allow_rolling = True
desc.disallow_yawing = True
return [desc]
def execute(self, userdata):
with self._cond:
for goal_func in self._goal_funcs:
current = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
goal = goal_func(current).as_MoveToGoal(speed=self._speed)
self._shared['moveto'].send_goal(goal, done_cb=self._done_cb)
while not self._done and not self.preempt_requested():
self._cond.wait(0.1)
self._done = False
if self.preempt_requested():
break
self._shared['moveto'].clear_callbacks()
if self.preempt_requested():
return 'preempted'
return 'succeeded'
def hexagon_desc_cb(size, color):
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
desc = TargetDesc()
desc.type = TargetDesc.TYPE_OBJECT
desc.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/shooter_%sin_%s_hexagon.obj' %
(7 if size == 'small' else 12, color))
desc.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
cov = numpy.zeros((6, 6))
a = numpy.array([traj.forward_vector]).T * 100
cov[3:, 3:] += a.dot(a.T)
desc.prior_distribution.covariance = cov.flatten()
desc.min_dist = HEXAGON_DIST * .9
desc.max_dist = HEXAGON_DIST / .9
desc.allow_rolling = True
desc.disallow_yawing = True
return [desc]
def execute(self, userdata):
if not self._name in saved_waypoints:
print 'No saved waypoint ' + self._name
return 'failed'
current = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
goal = saved_waypoints[self._name]
current = current.look_at_without_pitching(goal.position)
if not self._go(current):
self._shared['moveto'].clear_callbacks()
return 'preempted'
current = current.set_position(goal.position)
if not self._go(current):
self._shared['moveto'].clear_callbacks()
return 'preempted'
current = current.set_orientation(goal.orientation)
if not self._go(current):
self._shared['moveto'].clear_callbacks()
return 'preempted'
self._shared['moveto'].clear_callbacks()
return 'succeeded'
def execute(self, userdata):
pose_editor = PoseEditor.from_PoseTwistStamped_topic("/trajectory")
userdata.z = pose_editor.position[2]
return "succeeded"
def execute(self, userdata):
saved_waypoints[self._name] = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
return 'succeeded'
def pose_callback(msg):
global current_pose_editor
current_pose_editor = PoseEditor.from_Odometry(msg)
current_position = (msg.pose.pose.position.x,msg.pose.pose.position.y)
def pose_callback(msg):
global current_position, current_pose_editor
current_pose_editor = PoseEditor.from_Odometry(msg)
current_position = [msg.pose.pose.position.x, msg.pose.pose.position.y]
def make_manipulation(shared):
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
manipulation_desc = TargetDesc()
manipulation_desc.type = TargetDesc.TYPE_OBJECT
manipulation_desc.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/manipulation.obj')
manipulation_desc.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
manipulation_desc.disallow_yawing = True
manipulation_desc.min_dist = BOARD_DIST - 1
manipulation_desc.max_dist = BOARD_DIST + 3
wheel = TargetDesc()
wheel.type = TargetDesc.TYPE_OBJECT
wheel.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/manipulation_wheel.obj')
wheel.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
cov = numpy.zeros((6, 6))
a = numpy.array([traj.forward_vector]).T * 100
cov[3:, 3:] += a.dot(a.T)
wheel.prior_distribution.covariance = cov.flatten()
wheel.min_dist = WHEEL_DIST * .9
wheel.max_dist = WHEEL_DIST / .9
wheel.allow_rolling = True
wheel.disallow_yawing = True
# Create a SMACH state machine
sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
# Open the container
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(1.5)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', lambda: [manipulation_desc], .95))
smach.Sequence.add(
'STOP', common_states.VelocityState(shared, numpy.array([0, 0,
0])))
smach.Sequence.add('SLEEP', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_MANIPULATION',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
BOARD_DIST))
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(
shared,
lambda cur: cur.forward(BOARD_DIST - WHEEL_DIST).left(.5)))
smach.Sequence.add(
'WAIT_WHEEL',
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [wheel], .99))
smach.Sequence.add('WAIT_WHEEL_MORE', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_WHEEL',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
WHEEL_DIST))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\
.relative([0, .06, .06])))
smach.Sequence.add(
'TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.down(.2), lambda cur: cur.right(.2),
lambda cur: cur.up(.2), lambda cur: cur.left(.2),
lambda cur: cur.down(.2), lambda cur: cur.right(.2)
]))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add(
'BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
def execute(self, userdata):
current = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
self._shared['moveto'].send_goal(current.as_MoveToGoal(linear=self._vel))
return 'succeeded'
def make_manipulation(shared):
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
manipulation_desc = TargetDesc()
manipulation_desc.type = TargetDesc.TYPE_OBJECT
manipulation_desc.object_filename = roslib.packages.resource_file('auvsi_robosub', 'models', '2013/manipulation.obj')
manipulation_desc.prior_distribution.pose.orientation = Quaternion(*traj.turn_left_deg(180).orientation)
manipulation_desc.disallow_yawing = True
manipulation_desc.min_dist = BOARD_DIST-1
manipulation_desc.max_dist = BOARD_DIST+3
wheel = TargetDesc()
wheel.type = TargetDesc.TYPE_OBJECT
wheel.object_filename = roslib.packages.resource_file('auvsi_robosub', 'models', '2013/manipulation_wheel.obj')
wheel.prior_distribution.pose.orientation = Quaternion(*traj.turn_left_deg(180).orientation)
cov = numpy.zeros((6, 6))
a = numpy.array([traj.forward_vector]).T * 100
cov[3:, 3:] += a.dot(a.T)
wheel.prior_distribution.covariance = cov.flatten()
wheel.min_dist = WHEEL_DIST*.9
wheel.max_dist = WHEEL_DIST/.9
wheel.allow_rolling = True
wheel.disallow_yawing = True
# Create a SMACH state machine
sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'], 'succeeded')
# Open the container
with sm:
smach.Sequence.add('DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(1.5)))
smach.Sequence.add('APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_MANIPULATION',
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [manipulation_desc], .95))
smach.Sequence.add('STOP',
common_states.VelocityState(shared, numpy.array([0, 0, 0])))
smach.Sequence.add('SLEEP',
common_states.SleepState(5))
smach.Sequence.add('APPROACH_MANIPULATION',
object_finder_states.ApproachObjectState(shared,
'find_forward', 'forward_camera',
BOARD_DIST))
smach.Sequence.add('OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(BOARD_DIST-WHEEL_DIST).left(.5)))
smach.Sequence.add('WAIT_WHEEL',
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [wheel], .99))
smach.Sequence.add('WAIT_WHEEL_MORE',
common_states.SleepState(5))
smach.Sequence.add('APPROACH_WHEEL',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera', WHEEL_DIST))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\
.relative([0, .06, .06])))
smach.Sequence.add('TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.down(.2),
lambda cur: cur.right(.2),
lambda cur: cur.up(.2),
lambda cur: cur.left(.2),
lambda cur: cur.down(.2),
lambda cur: cur.right(.2)]))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add('BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
def execute(self, userdata):
pose_editor = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
return 'higher' if pose_editor.position[2] > userdata.z else 'lower'
def execute(self, userdata):
pose_editor = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
userdata.z = pose_editor.position[2]
return 'succeeded'
def execute(self, userdata):
pose_editor = PoseEditor.from_PoseTwistStamped_topic("/trajectory")
return "higher" if pose_editor.position[2] > userdata.z else "lower"
