def run(self, sub):
print "Running Mission"
# Set marker position
marker_point = np.random.uniform(-8, 8, size=2)
marker_quat = tf.transformations.quaternion_from_euler(
0, 0, np.random.uniform(-7, 7, size=1))
marker_pose = msg_helpers.numpy_quat_pair_to_pose(
np.append(marker_point, -4.9), marker_quat)
print "Marker at {0}".format(marker_pose)
self.set_model_pose(marker_pose, model='channel_marker_1')
# Call actual mission
start_time = self.nh.get_time()
try:
resp = yield txros.util.wrap_timeout(
missions.align_channel.run(sub), 600)
except txros.util.TimeoutError:
print "TIMEOUT"
defer.returnValue((False, "Timeout"))
else:
print "No timeout."
print "MISSION TIME:", self.nh.get_time() - start_time
sub_odom = msg_helpers.odometry_to_numpy(
(yield self.true_pose.get_next_message()))[0]
print
print np.linalg.norm(sub_odom[0][:2] - marker_point)
print
print np.dot(sub_odom[1], marker_quat)
print
distance_err = np.linalg.norm(sub_odom[0][:2] - marker_point)
ang_err = np.dot(sub_odom[1], marker_quat)
resp_msg = str(msg_helpers.numpy_quat_pair_to_pose(*sub_odom)) + '\n' + str(marker_pose) + '\n' + \
'Distance Error: ' + str(distance_err) + '\n' + \
'Angle Error: ' + str(ang_err) + '\n'
# 1 degree
if 0.01 < np.abs(np.dot(sub_odom[1], marker_quat)) < 0.99:
print "FAIL"
defer.returnValue((False, resp_msg))
# 1 foot
if np.linalg.norm(sub_odom[0][:2] - marker_point) > .3:
print "FAIL"
defer.returnValue((False, 'POSITION\n' + resp_msg))
print "PASS"
print
defer.returnValue((True, resp_msg))
def run(self, sub):
print "Running Mission"
# Set marker position
marker_point = np.random.uniform(-8, 8, size=2)
marker_quat = tf.transformations.quaternion_from_euler(0, 0, np.random.uniform(-7, 7, size=1))
marker_pose = msg_helpers.numpy_quat_pair_to_pose(np.append(marker_point, -4.9), marker_quat)
print "Marker at {0}".format(marker_pose)
self.set_model_pose(marker_pose, model='channel_marker_1')
# Call actual mission
start_time = self.nh.get_time()
try:
resp = yield txros.util.wrap_timeout(missions.align_channel.run(sub), 600)
except txros.util.TimeoutError:
print "TIMEOUT"
defer.returnValue((False, "Timeout"))
else:
print "No timeout."
print "MISSION TIME:", self.nh.get_time() - start_time
sub_odom = msg_helpers.odometry_to_numpy((yield self.true_pose.get_next_message()))[0]
print
print np.linalg.norm(sub_odom[0][:2] - marker_point)
print
print np.dot(sub_odom[1], marker_quat)
print
distance_err = np.linalg.norm(sub_odom[0][:2] - marker_point)
ang_err = np.dot(sub_odom[1], marker_quat)
resp_msg = str(msg_helpers.numpy_quat_pair_to_pose(*sub_odom)) + '\n' + str(marker_pose) + '\n' + \
'Distance Error: ' + str(distance_err) + '\n' + \
'Angle Error: ' + str(ang_err) + '\n'
# 1 degree
if 0.01 < np.abs(np.dot(sub_odom[1], marker_quat)) < 0.99:
print "FAIL"
defer.returnValue((False, resp_msg))
# 1 foot
if np.linalg.norm(sub_odom[0][:2] - marker_point) > .3:
print "FAIL"
defer.returnValue((False, 'POSITION\n' + resp_msg))
print "PASS"
print
defer.returnValue((True, resp_msg))
def setup(self):
# Set sub position
sub_point = np.random.uniform(-5, 5, size=3)
sub_point[2] = -np.abs(sub_point[2]) / 2.1
sub_quat = tf.transformations.quaternion_from_euler(0, 0, np.random.uniform(-7, 7, size=1))
sub_pose = msg_helpers.numpy_quat_pair_to_pose(sub_point, sub_quat)
yield self.set_model_pose(sub_pose)
print "MARKER_TEST - Sub at {0}".format(sub_pose)
# Set marker position
self.marker_point = np.random.uniform(-3, 3, size=2)
self.marker_quat = tf.transformations.quaternion_from_euler(0, 0, np.random.uniform(-7, 7, size=1))
self.marker_pose = msg_helpers.numpy_quat_pair_to_pose(np.append(self.marker_point, -4.9), self.marker_quat)
print "MARKER_TEST - Marker at {0}".format(self.marker_pose)
self.set_model_pose(self.marker_pose, model='channel_marker_1')
def return_pose(self, srv):
'''
Returns the next pose to go to in order to try and find the marker.
Only searches in a circle around our current location (could be extended to searching farther if we need to).
This will skip points that have already been searched.
'''
if srv.reset_search:
self.current_search = 0
self.poly_generator = self.polygon_generator()
return [0, False, srv.intial_position]
# We search at 1.5 * r so that there is some overlay in the search feilds.
np_pose = msg_helpers.pose_to_numpy(srv.intial_position)
rot_mat = make_2D_rotation(
tf.transformations.euler_from_quaternion(np_pose[1])[2])
coor = np.append(np.dot(rot_mat, next(self.poly_generator)) * srv.search_radius * 1.75, 0) \
+ np_pose[0]
# if self.current_search > self.max_searches:
# rospy.logwarn("Searched {} times. Marker not found.".format(self.max_searches))
# return [0, False, srv.intial_position]
self.current_search += 1
rospy.loginfo("Search number: {}".format(self.current_search))
# Should be variable based on height maybe?
pose = msg_helpers.numpy_quat_pair_to_pose(coor, np.array([0, 0, 0,
1]))
return [self.check_searched(coor[:2], srv.search_radius), True, pose]
def return_pose(self, srv):
'''
Returns the next pose to go to in order to try and find the marker.
Only searches in a circle around our current location (could be extended to searching farther if we need to).
This will skip points that have already been searched.
'''
if srv.reset_search:
self.current_search = 0
self.poly_generator = self.polygon_generator()
return [0, False, srv.intial_position]
# We search at 1.5 * r so that there is some overlay in the search feilds.
np_pose = msg_helpers.pose_to_numpy(srv.intial_position)
rot_mat = make_2D_rotation(tf.transformations.euler_from_quaternion(np_pose[1])[2])
coor = np.append(np.dot(rot_mat, next(self.poly_generator)) * srv.search_radius * 1.75, 0) \
+ np_pose[0]
# if self.current_search > self.max_searches:
# rospy.logwarn("Searched {} times. Marker not found.".format(self.max_searches))
# return [0, False, srv.intial_position]
self.current_search += 1
rospy.loginfo("Search number: {}".format(self.current_search))
# Should be variable based on height maybe?
pose = msg_helpers.numpy_quat_pair_to_pose(coor, np.array([0, 0, 0, 1]))
return [self.check_searched(coor[:2], srv.search_radius), True, pose]
def launch_torpedo(self, srv):
'''
Find position of sub and launch the torpedo from there.
TODO:
- Test to make sure it always fires from the right spot in the right direction.
(It seems to but I haven't tested from all rotations.)
'''
sub_state = self.get_model(model_name='sub8')
sub_pose = msg_helpers.pose_to_numpy(sub_state.pose)
# Translate torpedo init velocity so that it first out of the front of the sub.
muzzle_vel = np.array([10, 0, 0])
v = geometry_helpers.rotate_vect_by_quat(np.append(muzzle_vel, 0), sub_pose[1])
launch_twist = Twist()
launch_twist.linear.x = v[0]
launch_twist.linear.y = v[1]
launch_twist.linear.z = v[2]
# This is offset so it fires approx at the torpedo launcher location.
launch_pos = geometry_helpers.rotate_vect_by_quat(np.array([.4, -.15, -.3, 0]), sub_pose[1])
model_state = ModelState()
model_state.model_name = 'torpedo'
model_state.pose = msg_helpers.numpy_quat_pair_to_pose(sub_pose[0] + launch_pos, sub_pose[1])
model_state.twist = launch_twist
self.set_torpedo(model_state)
self.launched = True
return True
def launch_torpedo(self, srv):
'''
Find position of sub and launch the torpedo from there.
TODO:
- Test to make sure it always fires from the right spot in the right direction.
(It seems to but I haven't tested from all rotations.)
'''
sub_state = self.get_model(model_name='sub8')
sub_pose = msg_helpers.pose_to_numpy(sub_state.pose)
# Translate torpedo init velocity so that it first out of the front of the sub.
muzzle_vel = np.array([10, 0, 0])
v = geometry_helpers.rotate_vect_by_quat(np.append(muzzle_vel, 0),
sub_pose[1])
launch_twist = Twist()
launch_twist.linear.x = v[0]
launch_twist.linear.y = v[1]
launch_twist.linear.z = v[2]
# This is offset so it fires approx at the torpedo launcher location.
launch_pos = geometry_helpers.rotate_vect_by_quat(
np.array([.4, -.15, -.3, 0]), sub_pose[1])
model_state = ModelState()
model_state.model_name = 'torpedo'
model_state.pose = msg_helpers.numpy_quat_pair_to_pose(
sub_pose[0] + launch_pos, sub_pose[1])
model_state.twist = launch_twist
self.set_torpedo(model_state)
self.launched = True
return True
def setup(self):
self.reset_grid = yield self.nh.get_service_client('/reset_occ_grid', Empty)
# Set sub position
sub_point = np.random.uniform(-10, 10, size=3)
sub_point[2] = -np.abs(sub_point[2]) / 2.1
sub_quat = tf.transformations.quaternion_from_euler(0, 0, np.random.uniform(-7, 7, size=1))
sub_pose = msg_helpers.numpy_quat_pair_to_pose(sub_point, sub_quat)
yield self.set_model_pose(sub_pose)
print "Sub at {0}".format(sub_pose)
# Reset grid
yield self.nh.sleep(.2)
yield self.reset_grid(EmptyRequest())
yield self.nh.sleep(1)
def setup(self):
self.reset_grid = yield self.nh.get_service_client(
'/reset_occ_grid', Empty)
# Set sub position
sub_point = np.random.uniform(-10, 10, size=3)
sub_point[2] = -np.abs(sub_point[2]) / 2.1
sub_quat = tf.transformations.quaternion_from_euler(
0, 0, np.random.uniform(-7, 7, size=1))
sub_pose = msg_helpers.numpy_quat_pair_to_pose(sub_point, sub_quat)
yield self.set_model_pose(sub_pose)
print "Sub at {0}".format(sub_pose)
# Reset grid
yield self.nh.sleep(.2)
yield self.reset_grid(EmptyRequest())
yield self.nh.sleep(1)
def run(self, sub):
print "Running Mission"
# Call actual mission
start_time = self.nh.get_time()
try:
resp = yield txros.util.wrap_timeout(missions.align_channel.run(sub), 600) # 10 minute timeout
except txros.util.TimeoutError:
print "MARKER_TEST - TIMEOUT"
defer.returnValue((False, "Timeout"))
else:
if resp is None:
print "MARKER_TEST - ERROR"
defer.returnValue((False, "Error"))
print "MARKER_TEST - No timeout."
print "MARKER_TEST - MISSION TIME:", (self.nh.get_time() - start_time) / 1000000000.0
sub_odom = msg_helpers.odometry_to_numpy((yield self.true_pose))[0]
print
print "MARKER_TEST - Distance Error: {}".format(np.linalg.norm(sub_odom[0][:2] - self.marker_point))
print
print "MARKER_TEST - Angle Error: {}".format(np.dot(sub_odom[1], self.marker_quat))
print
distance_err = np.linalg.norm(sub_odom[0][:2] - self.marker_point)
ang_err = np.dot(sub_odom[1], self.marker_quat)
resp_msg = str(msg_helpers.numpy_quat_pair_to_pose(*sub_odom)) + '\n' + str(self.marker_pose) + '\n' + \
'Distance Error: ' + str(distance_err) + '\n' + \
'Angle Error: ' + str(ang_err) + '\n'
if 0.01 < np.abs(np.dot(sub_odom[1], self.marker_quat)) < 0.99:
print "MARKER_TEST - FAIL"
defer.returnValue((False, resp_msg))
if np.linalg.norm(sub_odom[0][:2] - self.marker_point) > .5:
print "MARKER_TEST - FAIL"
defer.returnValue((False, 'POSITION\n' + resp_msg))
print "MARKER_TEST - PASS"
print
defer.returnValue((True, resp_msg))
def initial_setup(self):
'''
Remove all the models in the sim and spawn a ground plane to run tests with.
TODO: Sometimes all the models don't get deleted correctly - probably a gazeb problem.
'''
models = yield self.model_states.get_next_message()
if 'ground_plane' not in models.name:
s = SpawnModelRequest()
s.model_name = 'ground_plane'
s.model_xml = ground_plane
s.initial_pose = msg_helpers.numpy_quat_pair_to_pose([0, 0, -5], [0, 0, 0, 1])
yield self.spawn_model(s)
for model in models.name:
if model == "channel_marker_1" or model == "sub8" or model == 'ground_plane':
continue
print "MARKER_TEST - Deleting {}".format(model)
self.delete_model(DeleteModelRequest(model_name=model))
self.nh.sleep(1)
def run_mission(srv, sub, nh):
print "Running Mission"
# Generate point to go to
point = np.random.uniform(-20, 20, size=3)
point[2] = -np.abs(point[2]) / 4
quat = tf.transformations.quaternion_from_euler(
0, 0, np.random.uniform(-7, 7, size=1))
pose = msg_helpers.numpy_quat_pair_to_pose(point, quat)
print pose
# Go to point
j = yield job_runner.JobManager.set_model_position(nh, pose)
yield txros.util.wall_sleep(5.0)
print "Movement should be complete."
excpeted_position = np.array([point, quat])
actual_odom = msg_helpers.odometry_to_numpy((yield sub.get_next_message()))
actual_position = np.array(actual_odom[0])
print excpeted_position, actual_position
print "Done, err:"
print np.abs(excpeted_position - actual_position)
print
if (np.abs(excpeted_position[0] - actual_position[0]) < .5).all() and \
(np.abs(excpeted_position[1] - actual_position[1]) < .2).all():
print "PASS"
print
defer.returnValue(RunJobResponse(success=True, message=str(pose)))
else:
print "FAIL"
print
err_msg = str(excpeted_position) + '\n' + str(actual_position)
defer.returnValue(RunJobResponse(success=False, message=err_msg))
def run_mission(srv, sub, nh):
print "Running Mission"
# Generate point to go to
point = np.random.uniform(-20, 20, size=3)
point[2] = -np.abs(point[2]) / 4
quat = tf.transformations.quaternion_from_euler(0, 0, np.random.uniform(-7, 7, size=1))
pose = msg_helpers.numpy_quat_pair_to_pose(point, quat)
print pose
# Go to point
j = yield job_runner.JobManager.set_model_position(nh, pose)
yield txros.util.wall_sleep(5.0)
print "Movement should be complete."
excpeted_position = np.array([point, quat])
actual_odom = msg_helpers.odometry_to_numpy((yield sub.get_next_message()))
actual_position = np.array(actual_odom[0])
print excpeted_position, actual_position
print "Done, err:"
print np.abs(excpeted_position - actual_position)
print
if (np.abs(excpeted_position[0] - actual_position[0]) < .5).all() and \
(np.abs(excpeted_position[1] - actual_position[1]) < .2).all():
print "PASS"
print
defer.returnValue(RunJobResponse(success=True, message=str(pose)))
else:
print "FAIL"
print
err_msg = str(excpeted_position) + '\n' + str(actual_position)
defer.returnValue(RunJobResponse(success=False, message=err_msg))
