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 to_lat_long(self, lat, lon, alt=0): """ Goes to a lat long position and keep the same orientation Note: lat and long need to be degrees """ ecef_pos, enu_pos = self.nav.ecef_pose[0], self.nav.pose[0] # These functions want radians lat, lon = np.radians([lat, lon], dtype=np.float64) ecef_vector = ecef_from_latlongheight(lat, lon, alt) - ecef_pos enu_vector = enu_from_ecef(ecef_vector, ecef_pos) enu_vector[2] = 0 # We don't want to move in the z at all return self.set_position(enu_pos + enu_vector)
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 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 execute(self,goal): global pos,origin,current_position ecef = [goal.waypoint.x,goal.waypoint.y,goal.waypoint.z] offset = [goal.offset.x,goal.offset.y] diff = numpy.array(ecef) - numpy.array(pos) goal = enu_from_ecef(diff,origin) print 'diff',diff print 'ecef',ecef print 'pos',pos print 'goal',goal final_goal = current_position + goal self.waypoint.send_goal_and_wait(current_pose_editor.look_at_without_pitching([final_goal[0],final_goal[1],0])) self.waypoint.send_goal(current_pose_editor.set_position([final_goal[0] + offset[0],final_goal[1] + offset[1],0]).asMoveToGoal(linear_tolerance = 1)) while (self.waypoint.get_state() == 'ACTIVE'): rospy.sleep(2) self.waypoint.send_goal(current_pose_editor.set_position([final_goal[0] + offset[0],final_goal[1] + offset[1],0])) self.server.set_succeeded()
def waypoint_ecef_callback(msg): global pos,origin,current_position ecef = [msg.point.x,msg.point.y,msg.point.z] diff = numpy.array(ecef) - numpy.array(pos) print 'diff',diff goal = enu_from_ecef(diff,origin) print 'ecef',ecef print 'pos',pos print 'goal',goal print 'offset goal',[goal[0] ,goal[1],0] final_goal = current_position + goal x = current_pose_editor.look_at_without_pitching([final_goal[0],final_goal[1],0]) print x.__dict__ print "result:", waypoint.send_goal_and_wait(x) print "aligned" x = current_pose_editor.set_position([final_goal[0],final_goal[1],0]) print x.__dict__ waypoint.send_goal_and_wait(x) print "done"