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