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"