def run(sub):
# Go to max height to see as much as we can.
yield sub.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go(speed=SPEED)
yield sub._node_handle.sleep(1.0)
# Do a little jig - change this for the pool.
pattern = [sub.move.right(1), sub.move.forward(1), sub.move.left(1), sub.move.backward(1),
sub.move.right(2), sub.move.forward(2), sub.move.left(2), sub.move.backward(2)]
s = Searcher(sub, sub.channel_marker.get_pose, pattern)
resp = None
try:
resp = yield s.start_search(loop=False, timeout=600)
except:
print "MARKER_MISSION - Timed out probably."
defer.returnValue(None)
if resp is None:
print "MARKER_MISSION - Marker not found."
defer.returnValue(None)
yield sub.move.set_position(pose_to_numpy(resp.pose.pose)[0]).go(speed=SPEED)
# How many times should we attempt to reposition ourselves
iterations = 2
# To make sure we don't go too far off.
est_target_rotations = []
for i in range(iterations):
print "Iteration {}.".format(i + 1)
response = yield sub.channel_marker.get_pose()
if response is None:
continue
response = pose_to_numpy(response.pose.pose)
# Using euler - shoot me.
yaw = tf.transformations.euler_from_quaternion(response[1])[2]
est_target_rotations.append(yaw)
avg_rotation = sum(est_target_rotations) / len(est_target_rotations)
yield sub.move.set_position(response[0]).yaw_left(yaw).zero_roll_and_pitch().go(speed=SPEED)
yield sub._node_handle.sleep(3.0)
print "MARKER_MISSION - Done!"
defer.returnValue(True)
def run(sub_singleton):
global marker_found
print "Searching"
nh = sub_singleton._node_handle
next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
intial_pose = sub_singleton.last_pose()
intial_height = yield sub_singleton.get_dvl_range()
raidus = calculate_visual_radius(cam, intial_height)
s = SearchPoseRequest()
s.intial_position = (yield intial_pose).pose.pose
s.search_radius = raidus
s.reset_search = True
yield next_pose(s)
s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
while not marker_found:
# Move in search pattern until we find the marker
resp = yield next_pose(s)
print resp
if resp.area > .8:
continue
print pose_to_numpy(resp.target_pose)[0]
yield sub_singleton.move.set_position(
pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
print "Searcher arrived"
yield goer
def run(sub_singleton):
global marker_found
print "Searching"
nh = sub_singleton._node_handle
next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
intial_pose = sub_singleton.last_pose()
intial_height = yield sub_singleton.get_dvl_range()
raidus = calculate_visual_radius(cam, intial_height)
s = SearchPoseRequest()
s.intial_position = (yield intial_pose).pose.pose
s.search_radius = raidus
s.reset_search = True
yield next_pose(s)
s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
while not marker_found:
# Move in search pattern until we find the marker
resp = yield next_pose(s)
print resp
if resp.area > .8:
continue
print pose_to_numpy(resp.target_pose)[0]
yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
print "Searcher arrived"
yield goer
def sanity_check(sub, est_pos):
yield None
est_pos_np = pose_to_numpy(est_pos.pose)[0]
print (est_pos_np == np.array([5, 5, 5])).all()
if (est_pos_np == np.array([5, 5, 5])).all():
print "BUOY MISSION - Problem with guess."
defer.returnValue(None)
if est_pos_np[2] > -0.2:
print 'BUOY MISSION - Detected buoy above the water'
defer.returnValue(None)
defer.returnValue(True)
def state_cb(self, msg):
"""
Position is offset so first message is taken as zero point. (More reflective of actual sub).
Z position is absolute and so is rotation.
TODO: Add noise
"""
if self.target not in msg.name:
return
if self.last_odom is None or self.position_offset is None:
pose = msg.pose[msg.name.index(self.target)]
position, orientation = sub8_utils.pose_to_numpy(pose)
self.position_offset = position
self.position_offset[2] = 0
self.last_odom = msg
def state_cb(self, msg):
'''
Position is offset so first message is taken as zero point. (More reflective of actual sub).
Z position is absolute and so is rotation.
TODO: Add noise
'''
if self.target not in msg.name:
return
if (self.last_odom is None or self.position_offset is None):
pose = msg.pose[msg.name.index(self.target)]
position, orientation = sub8_utils.pose_to_numpy(pose)
self.position_offset = position
self.position_offset[2] = 0
self.last_odom = msg
def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame="/map")
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = sub8_utils.pose_to_numpy(msg.pose[target_index])
pose = sub8_utils.numpy_quat_pair_to_pose(position_np - self.position_offset, orientation_np)
self.state_pub.publish(
header=header,
child_frame_id="/base_link",
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist),
)
header = sub8_utils.make_header(frame="/world")
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id="/base_link",
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist),
)
dist = np.linalg.norm(sub8_utils.twist_to_numpy(twist)) * self.odom_freq
self.pedometry += dist
else:
# fail
return
def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame='/map')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = sub8_utils.pose_to_numpy(
msg.pose[target_index])
pose = sub8_utils.numpy_quat_pair_to_pose(
position_np - self.position_offset, orientation_np)
self.state_pub.publish(header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
header = sub8_utils.make_header(frame='/world')
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
dist = np.linalg.norm(
sub8_utils.twist_to_numpy(twist)) * self.odom_freq
self.pedometry += dist
else:
# fail
return
