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