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()
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
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()
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()
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)
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]
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
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()
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()
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
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()
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()
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
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()
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)
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()
def get_ecef_pos(self): msg = yield self._absodom_sub.get_next_message() defer.returnValue(orientation_helpers.xyz_array( msg.pose.pose.position))
def right_position_selector(targetreses, traj_start): return sorted( targetreses, key=lambda result: _body_y_position(xyz_array(result.pose.position), traj_start) )[0]
def position_from_pose(p): return xyz_array(p.position)
def translate(v): return xyz_array(v.position)[0:2] - position
def get_ecef_pos(self): msg = yield self._absodom_sub.get_next_message() defer.returnValue(orientation_helpers.xyz_array(msg.pose.pose.position))