def publish_tf(self, *args): ''' Publishes a tf transform from base_link to pinger frame The orientation of the pinger frame is arbitrary Note there should be a more efficient way of getting this transform into the tf tree, and ideally it would be a child of /map, not /base_link. However, this works so it's ok for now ''' sub_pose = msg_helpers.pose_to_numpy( self.get_model(model_name=SUB_MODEL_NAME).pose) if not hasattr(self, 'pinger_pose'): self.pinger_pose = msg_helpers.pose_to_numpy( self.get_model(model_name=self.pinger_model_name).pose) sub_T, sub_R = sub_pose pinger_T, pinger_R = self.pinger_pose sub_R = tf.transformations.quaternion_matrix(sub_R)[:3, :3] pinger_R = tf.transformations.quaternion_matrix(pinger_R)[:3, :3] sub_pinger_T = np.linalg.inv(sub_R).dot(pinger_T - sub_T) sub_pinger_R = pinger_R sub_pinger_pose = msg_helpers.numpy_pair_to_pose(sub_pinger_T, sub_pinger_R) self.tf.transform.translation = sub_pinger_pose.position self.tf.transform.rotation = sub_pinger_pose.orientation self.tf.header = Header(seq=self.seq, stamp=rospy.Time.now(), frame_id='base_link') self.seq = self.seq + 1 self.br.sendTransform(self.tf)
def request_data(self, srv): ''' Calculate timestamps based on the subs position and the pinger location then use the sonar driver's EchoLocator to estimate the position. ''' sub_state = self.get_model(model_name='sub8') pinger_state = self.get_model(model_name='pinger') # Convert full state to poses sub_pose = msg_helpers.pose_to_numpy(sub_state.pose) hydro_board_pose = sub_pose[0] + self.hydrophone_board_tf pinger_pose = msg_helpers.pose_to_numpy(pinger_state.pose)[0] print sub_pose[1] # Calculate distances to each hydrophone from the pinger (need to be accounting for sub rotation). hydro_poses = hydro_board_pose + np.array( [[ self.hydro_loc['hydro0']['x'], self.hydro_loc['hydro0']['y'], self.hydro_loc['hydro0']['z'] ], [ self.hydro_loc['hydro1']['x'], self.hydro_loc['hydro1']['y'], self.hydro_loc['hydro1']['z'] ], [ self.hydro_loc['hydro2']['x'], self.hydro_loc['hydro2']['y'], self.hydro_loc['hydro2']['z'] ], [ self.hydro_loc['hydro3']['x'], self.hydro_loc['hydro3']['y'], self.hydro_loc['hydro3']['z'] ]]) print hydro_poses distances = np.linalg.norm(pinger_pose - hydro_poses, axis=1) timestamps = np.round(distances / self.wave_propagation_speed, decimals=self.precision) print timestamps - timestamps[0] # Don't forget estimated location is in map frame, transform to sub frame. est_location = self.echo_locator.getPulseLocation(timestamps - timestamps[0]) est_location_np = rotate_vect_by_quat( np.array([est_location.x, est_location.y, est_location.z, 0]) - np.hstack((sub_pose[0], 0)), sub_pose[1]) est_location.x = est_location_np[0] est_location.y = est_location_np[1] est_location.z = est_location_np[2] return est_location
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 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.) ''' rospy.loginfo('Launching torpedo') 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 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 request_data(self, srv): ''' Calculate timestamps based on the subs position and the pinger location then use the sonar driver's EchoLocator to estimate the position. ''' sub_state = self.get_model(model_name='sub8') pinger_state = self.get_model(model_name='pinger') # Convert full state to poses sub_pose = msg_helpers.pose_to_numpy(sub_state.pose) hydro_board_pose = sub_pose[0] + self.hydrophone_board_tf pinger_pose = msg_helpers.pose_to_numpy(pinger_state.pose)[0] print sub_pose[1] # Calculate distances to each hydrophone from the pinger (need to be accounting for sub rotation). hydro_poses = hydro_board_pose + np.array( [[self.hydro_loc['hydro0']['x'], self.hydro_loc['hydro0']['y'], self.hydro_loc['hydro0']['z']], [self.hydro_loc['hydro1']['x'], self.hydro_loc['hydro1']['y'], self.hydro_loc['hydro1']['z']], [self.hydro_loc['hydro2']['x'], self.hydro_loc['hydro2']['y'], self.hydro_loc['hydro2']['z']], [self.hydro_loc['hydro3']['x'], self.hydro_loc['hydro3']['y'], self.hydro_loc['hydro3']['z']]]) print hydro_poses distances = np.linalg.norm(pinger_pose - hydro_poses, axis=1) timestamps = np.round(distances / self.wave_propagation_speed, decimals=self.precision) print timestamps - timestamps[0] # Don't forget estimated location is in map frame, transform to sub frame. est_location = self.echo_locator.getPulseLocation(timestamps - timestamps[0]) est_location_np = rotate_vect_by_quat(np.array([est_location.x, est_location.y, est_location.z, 0]) - np.hstack((sub_pose[0], 0)), sub_pose[1]) est_location.x = est_location_np[0] est_location.y = est_location_np[1] est_location.z = est_location_np[2] return est_location