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 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"
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"
