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 = 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']]]) + hydro_board_pose
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 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