class Pinger(): def __init__(self, wave_propagation_speed, precision=14): self.get_model = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.wave_propagation_speed = wave_propagation_speed self.precision = precision self.hydrophone_board_tf = np.array([0, 0, 0]) self.hydro_loc = {'hydro0': {'x': 0, 'y': 0, 'z': 0}, 'hydro1': {'x': -0.0254, 'y': 0, 'z': 0.0254}, 'hydro2': {'x': 0.0254, 'y': 0, 'z': 0}, 'hydro3': {'x': 0, 'y': -0.0254, 'z': 0}} self.echo_locator = EchoLocator(self.hydro_loc, wave_propagation_speed) # Wait for all the models and such to spawn. rospy.Service('/sonar/get_pinger_pulse', Sonar, self.request_data) rospy.sleep(3) print self.request_data(None) 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 __init__(self, wave_propagation_speed, precision=14): self.get_model = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.wave_propagation_speed = wave_propagation_speed self.precision = precision self.hydrophone_board_tf = np.array([0, 0, 0]) self.hydro_loc = { 'hydro0': { 'x': 0, 'y': 0, 'z': 0 }, 'hydro1': { 'x': -0.0254, 'y': 0, 'z': 0.0254 }, 'hydro2': { 'x': 0.0254, 'y': 0, 'z': 0 }, 'hydro3': { 'x': 0, 'y': -0.0254, 'z': 0 } } self.echo_locator = EchoLocator(self.hydro_loc, wave_propagation_speed) # Wait for all the models and such to spawn. rospy.Service('/sonar/get_pinger_pulse', Sonar, self.request_data) rospy.sleep(3) print self.request_data(None)
def __init__(self, wave_propagation_speed, precision=14): self.get_model = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.wave_propagation_speed = wave_propagation_speed self.precision = precision self.hydrophone_board_tf = np.array([0, 0, 0]) self.hydro_loc = {'hydro0': {'x': 0, 'y': 0, 'z': 0}, 'hydro1': {'x': -0.0254, 'y': 0, 'z': 0.0254}, 'hydro2': {'x': 0.0254, 'y': 0, 'z': 0}, 'hydro3': {'x': 0, 'y': -0.0254, 'z': 0}} self.echo_locator = EchoLocator(self.hydro_loc, wave_propagation_speed) # Wait for all the models and such to spawn. rospy.Service('/sonar/get_pinger_pulse', Sonar, self.request_data) rospy.sleep(3) print self.request_data(None)
class Pinger(): def __init__(self, wave_propagation_speed, precision=14): self.get_model = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) self.wave_propagation_speed = wave_propagation_speed self.precision = precision self.hydrophone_board_tf = np.array([0, 0, 0]) self.hydro_loc = { 'hydro0': { 'x': 0, 'y': 0, 'z': 0 }, 'hydro1': { 'x': -0.0254, 'y': 0, 'z': 0.0254 }, 'hydro2': { 'x': 0.0254, 'y': 0, 'z': 0 }, 'hydro3': { 'x': 0, 'y': -0.0254, 'z': 0 } } self.echo_locator = EchoLocator(self.hydro_loc, wave_propagation_speed) # Wait for all the models and such to spawn. rospy.Service('/sonar/get_pinger_pulse', Sonar, self.request_data) rospy.sleep(3) print self.request_data(None) 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