Exemple #1
0
 def go_to_ecef_pos(self, pos, speed=0, turn=True):
     print 'go_to_ecef_pos', pos, speed, turn
     success = False
     try:
         first = True
         while True:
             odom_df = self._odom_sub.get_next_message()
             abs_msg = yield self._absodom_sub.get_next_message()
             msg = yield odom_df
             
             error_ecef = pos - orientation_helpers.xyz_array(abs_msg.pose.pose.position)
             error_enu = gps.enu_from_ecef(ecef_v=error_ecef, ecef_pos=orientation_helpers.xyz_array(abs_msg.pose.pose.position))
             error_enu[2] = 0
             print error_enu, '=>', numpy.linalg.norm(error_enu), '->', 1
             
             enu_pos = orientation_helpers.xyz_array(msg.pose.pose.position) + error_enu
             enu_pos[2] = self.pose.position[2]
             
             if numpy.linalg.norm(error_enu) < 1:
                 yield self.move.set_position(enu_pos).go()
                 print 'go_to_ecef_pos', 'succeeded'
                 return
             
             if first and turn:
                 yield self.move.look_at_without_pitching(enu_pos).go(speed=speed)
                 first = False
             
             self._moveto_action_client.send_goal(
                 self.pose.set_position(enu_pos).as_MoveToGoal(speed=speed)).forget()
         success = True
     finally:
         if not success:
             yield self.move.set_position(self.odom.position).go() # stop moving
             yield self.move.backward(3).go()
def desired_state_callback(desired_posetwist):
    global desired_state,desired_state_dot,desired_state_set
    lock.acquire()
    desired_state_set = True
    desired_state = numpy.concatenate([xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion(xyzw_array(desired_posetwist.posetwist.pose.orientation))])
    desired_state_dot = _jacobian(desired_state).dot(numpy.concatenate([xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular)]))
    lock.release()
Exemple #3
0
    def desired_state_callback(self,desired_posetwist):
        self.lock.acquire()
        self.desired_state_set = True

        self.desired_state = numpy.concatenate([xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion(xyzw_array(desired_posetwist.posetwist.pose.orientation))])
        self.desired_state_dot = numpy.concatenate([xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular)])
        self.lock.release()
	def new_goal(self):
		goal = self.moveto_as.accept_new_goal()
		self.desired_position = tools.position_from_posetwist(goal.posetwist)
		self.desired_orientation = tools.orientation_from_posetwist(goal.posetwist)
		#self.linear_tolerance = goal.linear_tolerance
		#self.angular_tolerance = goal.angular_tolerance

		rospy.loginfo('Desired position:' + str(self.desired_position))
		rospy.loginfo('Current position:' + str(self.current_position))
		rospy.loginfo('Desired orientation:' + str(self.desired_orientation))
		rospy.loginfo('Current orientation:' + str(self.current_orientation))
		rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance))
		rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance))
		# This controller doesn't handle desired twist
		#self.desired_twist = goal.posetwist.twist
		if (xyz_array(goal.posetwist.twist.linear).any() or 
			xyz_array(goal.posetwist.twist.angular).any() ):
			rospy.logwarn('None zero are not handled by the tank steer trajectory generator. Setting twist to 0')
		
		if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius:
			self.line = line(self.current_position, self.desired_position)
			self.redraw_line = True
		else:
			self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.current_position)
			self.redraw_line = False
Exemple #5
0
    def new_goal(self, goal):
        self.desired_position = tools.position_from_posetwist(goal)
        self.desired_orientation = tools.orientation_from_posetwist(goal)

        # Twist not supported
        if (xyz_array(goal.twist.linear).any() or 
            xyz_array(goal.twist.angular).any() ):
            rospy.logwarn('None zero twist are not handled by the azi goal_pp trajectory generator. Setting twist to 0')
def odom_callback(current_posetwist):
    global desired_state,desired_state_dot,state,stat_dot,state_dot_body,desired_state_set,odom_active
    lock.acquire()
    odom_active = True
    state = numpy.concatenate([xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation))])
    state_dot = _jacobian(state).dot(numpy.concatenate([xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)]))
    state_dot_body = numpy.concatenate([xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)])
    if (not desired_state_set):
        desired_state = state
        desired_state_set = True
    lock.release()
Exemple #7
0
def main(nh, pos, speed=0, turn=True):
    print 'go_to_ecef_pos: ', pos, "\n\tAt: ", speed, 'm/s\n\t', 'Turn first: ', turn

    boat = yield boat_scripting.get_boat(nh)

    success = False
    try:
        first = True
        last_enu_pos = numpy.zeros(3)
        while True:
            odom_df = boat.get_gps_odom_rel()
            abs_msg = yield boat.get_gps_odom()
            msg = yield odom_df
            
            error_ecef = pos - orientation_helpers.xyz_array(abs_msg.pose.pose.position)
            error_enu = gps.enu_from_ecef(ecef_v=error_ecef, ecef_pos=orientation_helpers.xyz_array(abs_msg.pose.pose.position))
            error_enu[2] = 0
            #print error_enu, '=>', numpy.linalg.norm(error_enu), '->', 1
            
            enu_pos = orientation_helpers.xyz_array(msg.pose.pose.position) + error_enu
            enu_pos[2] = boat.pose.position[2]
            
            if numpy.linalg.norm(error_enu) < 1:
                yield boat.move.set_position(enu_pos).go()
                print 'go_to_ecef_pos', 'succeeded'
                success = True
                return
            
            # Turn and send initial move incase enu_pos is within half a metter of [0,0,0]
            if first and turn:
                if turn:
                    yield boat.move.look_at_without_pitching(enu_pos).go(speed=speed)
                boat._moveto_action_client.send_goal(
                    boat.pose.set_position(enu_pos).as_MoveToGoal(speed=speed)).forget()
                first = False
            
            # Check if the position drifted more than half a meter radius
            #   if so send a new goal
            if numpy.linalg.norm(last_enu_pos - enu_pos) > 0.5:
                print 'Set new position'
                boat._moveto_action_client.send_goal(
                    boat.pose.set_position(enu_pos).as_MoveToGoal(speed=speed)).forget()

            last_enu_pos = enu_pos

        success = True
    finally:
        if not success:
            print 'go_to_ecef_pos failed to attain desired position, holding current position'
            yield boat.hold_at_current_pos()
Exemple #8
0
 def trajectory_callback(self, desired_trajectory):
     self.lock.acquire()
     self.desired_state_set = True
     # Get desired pose and orientation 
     desired_pose = xyz_array(desired_trajectory.posetwist.pose.position)
     desired_orientation = transformations.euler_from_quaternion(xyzw_array(desired_trajectory.posetwist.pose.orientation))
     # Get desired linear and angular velocities
     desired_lin_vel = xyz_array(desired_trajectory.posetwist.twist.linear)
     desired_ang_vel = xyz_array(desired_trajectory.posetwist.twist.angular)
     # Add desired position to desired state i_historys
     self.desired_state = numpy.concatenate([desired_pose, desired_orientation])
     # Add desired velocities to velocity i_history
     self.desired_velocity = numpy.concatenate([desired_lin_vel, desired_ang_vel])
     self.lock.release()
    def new_goal(self, goal):
        self.desired_position = tools.position_from_posetwist(goal)
        self.desired_orientation = tools.orientation_from_posetwist(goal)

        # Twist not supported
        if (xyz_array(goal.twist.linear).any() or 
            xyz_array(goal.twist.angular).any() ):
            rospy.logwarn('None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0')

        if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius:
            #print 'Far out dude'
            self.line = line(self.current_position, self.desired_position)
        else:
            #print 'Pretty close'
            self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
Exemple #10
0
def left_position_selector(targetreses, traj_start):
    return sorted(
        targetreses,
        key=lambda result: _body_y_position(xyz_array(result.pose.position),
                                            traj_start),
        reverse=True
        )[0]
Exemple #11
0
    def odom_callback(self, current_posetwist):
        self.lock.acquire()
        self.odom_active = True

        # Grab position; 0 Z
        pose = xyz_array(current_posetwist.pose.pose.position)
        pose[2] = 0

        # Grab orientation; 0 pitch and roll
        orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation)))
        orientation[0:2] = 0

        self.state = numpy.concatenate([xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation))])
        self.state_dot = numpy.concatenate([xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)])
        if (not self.desired_state_set):
            self.desired_state = self.state
            self.desired_state_set = True
        self.lock.release()
    def new_goal(self, goal):
        self.desired_position = tools.position_from_posetwist(goal)
        self.desired_orientation = tools.orientation_from_posetwist(goal)

        # Twist not supported
        if (xyz_array(goal.twist.linear).any()
                or xyz_array(goal.twist.angular).any()):
            rospy.logwarn(
                'None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0'
            )

        if np.linalg.norm(self.current_position -
                          self.desired_position) > self.orientation_radius:
            #print 'Far out dude'
            self.line = line(self.current_position, self.desired_position)
        else:
            #print 'Pretty close'
            self.line = line(
                self.desired_position,
                tools.normal_vector_from_rotvec(self.desired_orientation) +
                self.desired_position)
    def _get_target(self, result):
        dest_pos = xyz_array(result.pose.position)
        dest_orientation = xyzw_array(result.pose.orientation)

        # Rotate dest_orientation by 180 degrees if we're about to turn more than 90
        if numpy.dot(self._traj_start.orientation, dest_orientation)**2 < .5:
            dest_orientation = transformations.quaternion_multiply(dest_orientation,
                                                                   [0, 0, 1, 0])

        return self._traj_start.set_position(dest_pos) \
                               .set_orientation(dest_orientation) \
                               .height(self._traj_start.position[2])
        return target
 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
Exemple #15
0
def odom_callback(current_posetwist):
    global desired_state, desired_state_dot, state, stat_dot, state_dot_body, desired_state_set, odom_active
    lock.acquire()
    odom_active = True
    state = numpy.concatenate([
        xyz_array(current_posetwist.pose.pose.position),
        transformations.euler_from_quaternion(
            xyzw_array(current_posetwist.pose.pose.orientation))
    ])
    state_dot = _jacobian(state).dot(
        numpy.concatenate([
            xyz_array(current_posetwist.twist.twist.linear),
            xyz_array(current_posetwist.twist.twist.angular)
        ]))
    state_dot_body = numpy.concatenate([
        xyz_array(current_posetwist.twist.twist.linear),
        xyz_array(current_posetwist.twist.twist.angular)
    ])
    if (not desired_state_set):
        desired_state = state
        desired_state_set = True
    lock.release()
Exemple #16
0
    def odom_callback(self, current_pos):
        self.lock.acquire()
        self.odom_active = True
        # Grab current position and orientation and 0 linear Z and angluar X and Y
        # Get current position 
        current_position = xyz_array(current_pos.pose.pose.position)
        current_orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_pos.pose.pose.orientation)))
        # Zero unneccesary elements
        current_position[2] = 0
        current_orientation[0:2] = 0
        # Add current position to state i_history
        self.current_state = numpy.concatenate([current_position, current_orientation])
        # Get current velocities
        current_lin_vel = xyz_array(current_pos.twist.twist.linear)
        current_ang_vel = xyz_array(current_pos.twist.twist.angular)
        # Add current velocities to velocity i_history
        self.current_velocity = numpy.concatenate([current_lin_vel, current_ang_vel])
        # If the desired state has not yet been set, set desired and current as the same
        # Resets the controller to current position on bootup
        if (not self.desired_state_set):
            self.desired_state = self.current_state
            self.desired_state_set = True

        self.lock.release()
Exemple #17
0
    def visual_approach_3d(self, camera, distance, targetdesc, loiter_time=0):
        goal_mgr = self._camera_3d_action_clients[camera].send_goal(
            object_finder_msg.FindGoal(
                header=Header(frame_id='/enu', ),
                targetdescs=[targetdesc],
            ))
        start_pose = self.pose

        try:
            last_good_pos = None
            loiter_start = None

            while True:
                feedback = yield goal_mgr.get_feedback()
                targ = feedback.targetreses[0]

                if targ.P > 0.55:
                    last_good_pos = orientation_helpers.xyz_array(
                        targ.pose.position)

                print targ.P

                if last_good_pos is not None:
                    desired_pos = start_pose.set_position(
                        last_good_pos).backward(distance).position
                    desired_pos[2] = start_pose.position[2]

                    print ' ' * 20, numpy.linalg.norm(
                        desired_pos -
                        self.pose.position), desired_pos, self.pose.position

                    if numpy.linalg.norm(desired_pos -
                                         self.pose.position) < 0.5:
                        if loiter_start is None:
                            loiter_start = time.time()

                        if time.time() > loiter_start + loiter_time:
                            yield self._moveto_action_client.send_goal(
                                start_pose.set_position(
                                    desired_pos).as_MoveToGoal()).get_result()
                            return

                    self._moveto_action_client.send_goal(
                        start_pose.set_position(desired_pos).as_MoveToGoal(
                            speed=.5)).forget()
        finally:
            goal_mgr.cancel()
            yield self.move.go()  # stop moving
Exemple #18
0
 def visual_approach_3d(self, camera, distance):
     start_pose = self.pose
     goal_mgr = self._camera_3d_action_clients[camera].send_goal(object_finder_msg.FindGoal(
         header=Header(
             frame_id='/map',
         ),
         targetdescs=[object_finder_msg.TargetDesc(
             type=object_finder_msg.TargetDesc.TYPE_SPHERE,
             sphere_radius=4*.0254, # 4 in
             prior_distribution=PoseWithCovariance(
                 pose=Pose(
                     orientation=Quaternion(x=0, y=0, z=0, w=1),
                 ),
             ),
             min_dist=1,
             max_dist=8,
             sphere_color=object_finder_msg.Color(r=1, g=0, b=0),
             sphere_background_color=object_finder_msg.Color(r=0, g=1, b=1),
         )],
     ))
     
     try:
         last_good_pos = None
         
         while True:
             feedback = yield goal_mgr.get_feedback()
             targ = feedback.targetreses[0]
             
             if targ.P > 0.4:
                 last_good_pos = orientation_helpers.xyz_array(targ.pose.position)
             
             print targ.P
             
             if last_good_pos is not None:
                 desired_pos = start_pose.set_position(last_good_pos).backward(distance).position
                 
                 print ' '*20, numpy.linalg.norm(desired_pos - self.pose.position)
                 
                 if numpy.linalg.norm(desired_pos - self.pose.position) < 0.5:
                     yield self._moveto_action_client.send_goal(
                         start_pose.set_position(desired_pos).as_MoveToGoal()).get_result()
                     return
                 
                 self._moveto_action_client.send_goal(
                     start_pose.set_position(desired_pos).as_MoveToGoal()).forget()
     finally:
         goal_mgr.cancel()
Exemple #19
0
def main(nh, entrance='x', exit='1'):
    boat = yield boat_scripting.get_boat(nh)

    try:
        boat.switch_path_planner('a_star_rpp')
        while True:
            gates = yield boat.get_gates()
            if len(gates) == 0:
                print 'No gates detected'
                continue

            current_boat_pos = boat.odom.position
            yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi

            # Zip up numpy positions in enu - boat_pos
            gate_pos = [
                xyz_array(g.position) - current_boat_pos for g in gates
            ]
            gates = zip(gate_pos, gates)

            # Filter out yaw

            gate = None
            if len(gates) > 0:
                gate = min(gates, key=lambda x: np.linalg.norm(x[0]))
            else:
                gate = gates[0]

            #print gate

            # Translate back to enu
            gate_pos = gate[0] + current_boat_pos
            gate_orientation = gate[1].yaw + np.pi / 2

            if abs(gate_orientation - yaw) < np.pi / 2:
                yield move_on_line.main(nh, gate_pos,
                                        np.array([0, 0, gate_orientation]))
            else:
                yield move_on_line.main(nh,
                                        gate_pos,
                                        np.array([0, 0, gate_orientation]),
                                        flip=True)

    finally:
        boat.default_state()
Exemple #20
0
 def visual_approach_3d(self, camera, distance, targetdesc, loiter_time=0):
     goal_mgr = self._camera_3d_action_clients[camera].send_goal(object_finder_msg.FindGoal(
         header=Header(
             frame_id='/enu',
         ),
         targetdescs=[targetdesc],
     ))
     start_pose = self.pose
     
     try:
         last_good_pos = None
         loiter_start = None
         
         while True:
             feedback = yield goal_mgr.get_feedback()
             targ = feedback.targetreses[0]
             
             if targ.P > 0.55:
                 last_good_pos = orientation_helpers.xyz_array(targ.pose.position)
             
             print targ.P
             
             if last_good_pos is not None:
                 desired_pos = start_pose.set_position(last_good_pos).backward(distance).position
                 desired_pos[2] = start_pose.position[2]
                 
                 print ' '*20, numpy.linalg.norm(desired_pos - self.pose.position), desired_pos, self.pose.position
                 
                 if numpy.linalg.norm(desired_pos - self.pose.position) < 0.5:
                     if loiter_start is None:
                         loiter_start = time.time()
                     
                     if time.time() > loiter_start + loiter_time:
                         yield self._moveto_action_client.send_goal(
                             start_pose.set_position(desired_pos).as_MoveToGoal()).get_result()
                         return
                 
                 self._moveto_action_client.send_goal(
                     start_pose.set_position(desired_pos).as_MoveToGoal(speed=.5)).forget()
     finally:
         goal_mgr.cancel()
         yield self.move.go() # stop moving
Exemple #21
0
def main(nh, entrance='x', exit='1'):
    boat = yield boat_scripting.get_boat(nh)

    try:
        boat.switch_path_planner('a_star_rpp')
        while True:
            gates = yield boat.get_gates()
            if len(gates) == 0:
                print 'No gates detected'
                continue

            current_boat_pos = boat.odom.position
            yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi

            # Zip up numpy positions in enu - boat_pos
            gate_pos = [xyz_array(g.position) - current_boat_pos for g in gates]
            gates = zip(gate_pos, gates)

            # Filter out yaw 


            gate = None
            if len(gates) > 0:
                gate = min(gates, key = lambda x: np.linalg.norm(x[0]))
            else:
                gate = gates[0]

            #print gate

            # Translate back to enu
            gate_pos = gate[0] + current_boat_pos
            gate_orientation = gate[1].yaw + np.pi/2


            if abs(gate_orientation - yaw) < np.pi / 2:
                yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]))
            else:
                yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]), flip=True)


    finally:
        boat.default_state()
Exemple #22
0
	def odom_cb(self, msg):
		# Pose is relative to /enu!!!!!!!!
		# Twist is relative to /base_link!!!!!!!!!
		self.current_position = tools.position_from_pose(msg.pose.pose)
		self.current_orientation = tools.orientation_from_pose(msg.pose.pose)

		# Transform velocity from boat frame of reference to enu
		vec = Vector3Stamped(
				header = Header(
					frame_id = '/base_link',
					stamp = msg.header.stamp + rospy.Duration(-0.1)),		# Look a tenth of a second into the past
				vector = msg.twist.twist.linear)
		try:
			#rospy.loginfo('Vec before transform: ' + str(vec))
			vec = self.tf_listener.transformVector3('/enu', vec)
			#rospy.loginfo('Vec after transform: ' + str(vec))
		except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as err:
			#rospy.logwarn('Tf error: ' + str(err))
			return

		self.current_velocity = xyz_array(vec.vector)
    def odom_cb(self, msg):
        # Pose is relative to /enu!!!!!!!!
        # Twist is relative to /base_link!!!!!!!!!
        self.current_position = tools.position_from_pose(msg.pose.pose)
        self.current_orientation = tools.orientation_from_pose(msg.pose.pose)

        # Transform velocity from boat frame of reference to enu
        vec = Vector3Stamped(
            header=Header(frame_id='/base_link',
                          stamp=msg.header.stamp + rospy.Duration(-0.1)
                          ),  # Look a tenth of a second into the past
            vector=msg.twist.twist.linear)
        try:
            #rospy.loginfo('Vec before transform: ' + str(vec))
            vec = self.tf_listener.transformVector3('/enu', vec)
            #rospy.loginfo('Vec after transform: ' + str(vec))
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException) as err:
            #rospy.logwarn('Tf error: ' + str(err))
            return

        self.current_velocity = xyz_array(vec.vector)
Exemple #24
0
 def hydrophone_align(self, frequency):
     start_pose = self.pose
     start_map_transform = tf.Transform(
         start_pose.position, start_pose.orientation)
     orientation = start_pose.orientation
     move_goal_mgr = None
     good = []
     try:
         while True:
             feedback = yield self.get_processed_ping(frequency)
             print feedback
             bottom_z = self.pose.position[2] - (yield self.get_dvl_range())
             print 'bottom_z:', bottom_z
             
             try:
                 transform = yield self._tf_listener.get_transform('/base_link',
                     feedback.header.frame_id, feedback.header.stamp)
                 map_transform = yield self._tf_listener.get_transform('/map',
                     '/base_link', feedback.header.stamp)
             except Exception:
                 traceback.print_exc()
                 continue
             
             ray_start_sensor = numpy.array([0, 0, 0])
             ray_dir_sensor = orientation_helpers.xyz_array(feedback.position)
             ray_dir_sensor = ray_dir_sensor / numpy.linalg.norm(ray_dir_sensor)
             
             ray_start_world = map_transform.transform_point(
                 transform.transform_point(ray_start_sensor))
             ray_dir_world = map_transform.transform_vector(
                 transform.transform_vector(ray_dir_sensor))
             
             movement_plane_world = numpy.array([0, 0, 1])
             
             plane_point = numpy.array([0, 0, bottom_z])
             plane_vector = numpy.array([0, 0, 1])
             
             x = plane_vector.dot(ray_start_world - plane_point) / plane_vector.dot(ray_dir_world)
             object_pos = ray_start_world - ray_dir_world * x
             print 'object_pos_body:', map_transform.inverse().transform_point(object_pos)
             
             desired_pos = object_pos - map_transform.transform_vector(transform.transform_point(ray_start_sensor))
             desired_pos = desired_pos - movement_plane_world * movement_plane_world.dot(desired_pos - start_pose.position)
             
             error_pos = desired_pos - map_transform.transform_point([0, 0, 0])
             error_pos = error_pos - movement_plane_world * movement_plane_world.dot(error_pos)
             
             threshold = 1 # m
             angle_error = abs(math.acos((error_pos / numpy.linalg.norm(error_pos)).dot(self.pose.forward_vector)))
             print 'pos error:', numpy.linalg.norm(error_pos), 'angle error:', math.degrees(angle_error), 'rel pos error:', numpy.linalg.norm(error_pos)/threshold
             
             if numpy.linalg.norm(error_pos) < threshold:
                 good.append(desired_pos)
                 if len(good) > 3:
                     yield (self.move
                         .set_orientation(orientation)
                         .set_position(numpy.mean(good, axis=0))
                         .go())
                     
                     return
             else:
                 good = []
             
             # go towards desired position
             if numpy.linalg.norm(error_pos) > 4:
                 move_goal_mgr = self._moveto_action_client.send_goal(
                     self.pose.look_at_without_pitching(desired_pos).set_position((desired_pos+self.pose.position)/2).as_MoveToGoal())
                 orientation = self.pose.look_at_without_pitching(desired_pos).orientation
             else:
                 move_goal_mgr = self._moveto_action_client.send_goal(
                     self.pose.set_orientation(orientation).set_position((desired_pos+self.pose.position)/2).as_MoveToGoal())
     finally:
         if move_goal_mgr is not None: yield move_goal_mgr.cancel()
Exemple #25
0
 def get_ecef_pos(self):
     msg = yield self._absodom_sub.get_next_message()
     defer.returnValue(orientation_helpers.xyz_array(
         msg.pose.pose.position))
Exemple #26
0
def right_position_selector(targetreses, traj_start):
    return sorted(
        targetreses,
        key=lambda result: _body_y_position(xyz_array(result.pose.position),
                                            traj_start)
        )[0]
Exemple #27
0
def position_from_pose(p):
    return xyz_array(p.position)
Exemple #28
0
 def translate(v):
     return xyz_array(v.position)[0:2] - position
Exemple #29
0
 def get_ecef_pos(self):
     msg = yield self._absodom_sub.get_next_message()
     defer.returnValue(orientation_helpers.xyz_array(msg.pose.pose.position))
Exemple #30
0
 def translate(v):
     return xyz_array(v.position)[0:2] - position
Exemple #31
0
def position_from_pose(p):
	return xyz_array(p.position)