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