def get_closest_buoy(navigator, explored_ids):
pose = yield navigator.tx_pose
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_np = navigator_tools.point_to_numpy(buoy_field.objects[0].position)
# Find which totems we haven't explored yet
totems = yield navigator.database_query("totem", raise_exception=False)
if not totems.found:
# Need to search for more totems
defer.returnValue([None, explored_ids])
u_totems = [totem for totem in totems.objects if totem.id not in explored_ids]
u_totems_np = map(lambda totem: navigator_tools.point_to_numpy(totem.position), u_totems)
if len(u_totems_np) == 0:
defer.returnValue([None, explored_ids])
# Find the closest buoys, favoring the ones closest to the boat.
# J = wa * ca ** 2 + wb * c2 ** 2
# a := boat
# b := buoy field ROI
wa = .7
wb = .3
ca = np.linalg.norm(u_totems_np - pose[0], axis=1)
cb = np.linalg.norm(u_totems_np - buoy_field_np, axis=1)
J = wa * ca + wb * cb
target_totem = u_totems[np.argmin(J)]
explored_ids.append(target_totem.id)
defer.returnValue([target_totem, explored_ids])
def get_closest_buoy(navigator, explored_ids):
pose = yield navigator.tx_pose
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_np = navigator_tools.point_to_numpy(buoy_field.objects[0].position)
# Find which totems we haven't explored yet
totems = yield navigator.database_query("totem", raise_exception=False)
if not totems.found:
# Need to search for more totems
defer.returnValue([None, explored_ids])
u_totems = [totem for totem in totems.objects if totem.id not in explored_ids]
u_totems_np = map(lambda totem: navigator_tools.point_to_numpy(totem.position), u_totems)
if len(u_totems_np) == 0:
defer.returnValue([None, explored_ids])
# Find the closest buoys, favoring the ones closest to the boat.
# J = wa * ca ** 2 + wb * c2 ** 2
# a := boat
# b := buoy field ROI
wa = .7
wb = .3
ca = np.linalg.norm(u_totems_np - pose[0], axis=1)
cb = np.linalg.norm(u_totems_np - buoy_field_np, axis=1)
J = wa * ca + wb * cb
target_totem = u_totems[np.argmin(J)]
explored_ids.append(target_totem.id)
defer.returnValue([target_totem, explored_ids])
def get_colored_buoy(navigator, color):
"""
Returns the closest colored buoy with the specified color
"""
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(
buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
totems = yield navigator.database_query("totem")
correct_colored = [
totem for totem in totems.objects if np.all(
np.round(
navigator_tools.rosmsg_to_numpy(
totem.color, keys=['r', 'g', 'b'])) == color)
]
if len(correct_colored) == 0:
closest = None
else:
closest = sorted(
correct_colored,
key=lambda totem: _dist_from_bf(
navigator_tools.point_to_numpy(totem.position)))[0]
defer.returnValue(closest)
def main(navigator):
result = navigator.fetch_result()
#middle_point = np.array([-10, -70, 0])
est_coral_survey = yield navigator.database_query("Coral_Survey")
yield navigator.move.set_position(est_coral_survey.objects[0]).go()
totem = yield navigator.database_query("totem")
# Get the closest totem object to the boat
totem_np = map(lambda obj: navigator_tools.point_to_numpy(obj), totem.objects)
dist = map(lambda totem_np: np.linalg.norm(totem_np - navigator_tools.point_to_numpy(est_coral_survey.objects[0])), totems_np)
middle_point = navigator_tools.point_to_numpy(totem.objects[0].position)
quads_to_search = [1, 2, 3, 4]
if (yield navigator.nh.has_param("/mission/coral_survey/quadrants")):
quads_to_search = yield navigator.nh.get_param("/mission/coral_survey/quadrants")
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
waypoints = []
for quad in quads_to_search:
mid = navigator.move.set_position(middle_point).set_orientation(directions[quad - 1])
waypoints.append(mid.yaw_left(45, "deg").forward(waypoint_from_center).set_orientation(NORTH))
# Get into the coral survey area
yield waypoints[0].go()
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
searcher = navigator.search("coral_survey", waypoints)
yield searcher.start_search(move_type='skid', spotings_req=1)
fprint("Centering over the thing!", title="CORAL_SURVEY")
# TODO: Center over the thing.
boat_position = (yield navigator.tx_pose)[0]
# TODO: make this easier
quad = np.argmin(np.linalg.norm(boat_position - [[w.pose[0][0][0], w.pose[0][1][0],w.pose[0][2][0]] for w in waypoints], axis=1))
quad = quads_to_search[quad]
fprint("Estimated quadrant: {}".format(quad), title="CORAL_SURVEY", msg_color='green')
yield navigator.nh.sleep(5)
defer.returnValue(result)
def main(navigator):
res = navigator.fetch_result()
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(buoy_field.objects[0])
#yield navigator.move.set_position(buoy_field_point).go()
circle_colors = ['blue', 'red']
color_map = {'blue': (255, 0, 0), 'red': (0, 0, 255)}
explored_ids = []
all_found = False
while not all_found:
target_totem, explored_ids = yield get_closest_buoy(navigator, explored_ids)
if target_totem is None:
fprint("No suitable totems found.", msg_color='red', title="CIRCLE_TOTEM")
continue
# Visualization
points = [target_totem.position]
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=points)
yield navigator._point_cloud_pub.publish(pc)
# Let's go there
target_distance = 7 # m
target_totem_np = navigator_tools.point_to_numpy(target_totem.position)
q = get_sun_angle()
lookat = navigator.move.set_position(target_totem_np).set_orientation(q).backward(target_distance)
yield lookat.go()
# Now that we're looking him in the eyes, aim no higher.
# Check the color and see if it's one we want.
fprint("Color request", title="CIRCLE_TOTEM")
#if target_totem is not None:
# all_found = True
defer.returnValue(res)
pattern = navigator.move.circle_point(focus, radius=5)
for p in pattern:
yield p.go(move_type='skid', focus=focus)
print "Nexting"
def get_colored_buoy(navigator, color):
"""
Returns the closest colored buoy with the specified color
"""
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
totems = yield navigator.database_query("totem")
correct_colored = [totem for totem in totems.objects if np.all(np.round(navigator_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b'])) == color)]
if len(correct_colored) == 0:
closest = None
else:
closest = sorted(correct_colored, key=lambda totem: _dist_from_bf(navigator_tools.point_to_numpy(totem.position)))[0]
defer.returnValue(closest)
def main(navigator, **kwargs):
#middle_point = np.array([-10, -70, 0])
est_coral_survey = yield navigator.database_query("CoralSurvey")
# Going to get all the objects and using the closest one as the totem
totem = yield navigator.database_query("all")
# Get the closest totem object to the boat
totems_np = map(lambda obj: navigator_tools.point_to_numpy(obj.position),
totem.objects)
dist = map(
lambda totem_np: np.linalg.norm(
totem_np - navigator_tools.point_to_numpy(est_coral_survey.objects[
0].position)), totems_np)
middle_point = totems_np[np.argmin(dist)]
print "Totem sorted:", totems_np
print "Totem selected: ", totems_np[0]
quads_to_search = [1, 2, 3, 4]
quad = yield navigator.mission_params[
"acoustic_pinger_active_index_correct"].get()
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
waypoints = []
#for quad in quads_to_search:
mid = navigator.move.set_position(middle_point).set_orientation(
directions[quad - 1])
search_center = mid.yaw_left(
45, "deg").forward(waypoint_from_center).set_orientation(NORTH)
yield search_center.left(6).go()
yield navigator.move.circle_point(search_center.position,
direction='cw').go()
yield navigator.mission_params["coral_survey_shape1"].set(shapes[0])
latched.cancel()
defer.returnValue(None)
def main(navigator, **kwargs):
#middle_point = np.array([-10, -70, 0])
est_coral_survey = yield navigator.database_query("CoralSurvey")
# Going to get all the objects and using the closest one as the totem
totem = yield navigator.database_query("all")
# Get the closest totem object to the boat
totems_np = map(lambda obj: navigator_tools.point_to_numpy(obj.position), totem.objects)
dist = map(lambda totem_np: np.linalg.norm(totem_np - navigator_tools.point_to_numpy(est_coral_survey.objects[0].position)), totems_np)
middle_point = totems_np[np.argmin(dist)]
print "Totem sorted:", totems_np
print "Totem selected: ", totems_np[0]
quads_to_search = [1, 2, 3, 4]
quad = yield navigator.mission_params["acoustic_pinger_active_index_correct"].get()
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
waypoints = []
#for quad in quads_to_search:
mid = navigator.move.set_position(middle_point).set_orientation(directions[quad - 1])
search_center = mid.yaw_left(45, "deg").forward(waypoint_from_center).set_orientation(NORTH)
yield search_center.left(6).go()
yield navigator.move.circle_point(search_center.position, direction='cw').go()
yield navigator.mission_params["coral_survey_shape1"].set(shapes[0])
latched.cancel()
defer.returnValue(None)
def ping_cb(self, ping):
print rospkg.get_ros_package_path()
print "PINGERFINDER: freq={p.freq:.0f} amp={p.amplitude:.0f}".format(
p=ping)
if abs(
ping.freq - self.target_freq
) < self.freq_tol and ping.amplitude > self.min_amp and self.listen:
trans, rot = None, None
try:
self.tf_listener.waitForTransform(self.map_frame,
self.hydrophone_frame,
ping.header.stamp,
rospy.Duration(0.25))
trans, rot = self.tf_listener.lookupTransform(
self.map_frame, self.hydrophone_frame, ping.header.stamp)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print e
return
p0 = np.array([trans[0], trans[1]])
R = tf.transformations.quaternion_matrix(rot)[:3, :3]
delta = R.dot(navigator_tools.point_to_numpy(ping.position))[:2]
p1 = p0 + self.heading_pseudorange * delta / npl.norm(delta)
# p0 and p1 define a line
line_coeffs = np.array([[p0[0], p0[1], p1[0], p1[1]]])
self.visualize_arrow(Point(p0[0], p0[1], 0),
Point(p1[0], p1[1], 0))
self.line_array = np.append(self.line_array, line_coeffs, 0)
self.observation_amplitudes = np.append(
self.observation_amplitudes, ping.amplitude)
# delete softest samples if we have over max_observations
if len(self.line_array) >= self.max_observations:
softest_idx = np.argmin(self.observation_amplitudes)
self.line_array = np.delete(self.line_array,
softest_idx,
axis=0)
self.observation_amplitudes = np.delete(
self.observation_amplitudes, softest_idx)
print "PINGERFINDER: Observation collected. Total: {}".format(
self.line_array.shape[0])
def head_for_pinger(navigator):
ping_sub = yield navigator.nh.subscribe("/hydrophones/processed", ProcessedPing)
yield ping_sub.get_next_message()
while True:
yield navigator.nh.sleep(1)
processed_ping = yield ping_sub.get_last_message()
print processed_ping
if isinstance(processed_ping, ProcessedPing):
print "Got processed ping message:\n{}".format(processed_ping)
if processed_ping.freq > 35000 and processed_ping.freq < 36000:
print "Trustworthy pinger heading"
t = yield navigator.tf_listener.get_transform("/enu", "/hydrophones", processed_ping.header.stamp)
base_vect = t.transform_vector(navigator_tools.point_to_numpy(processed_ping.position))
base_vect = base_vect / np.linalg.norm(base_vect) * 10
odom_enu = (yield navigator.tx_pose)[0]
#pinger_move = navigator.move.set_position(navigator_tools.point_to_numpy(processed_ping.position)).go()
pinger_move = yield navigator.move.set_position(odom_enu + base_vect).go()
print "Heading towards pinger"
else:
print "Untrustworthy pinger heading. Freq = {} kHZ".format(processed_ping.freq)
else:
print "Expected ProcessedPing, got {}".format(type(processed_ping))
def ping_cb(self, ping):
print rospkg.get_ros_package_path()
print "PINGERFINDER: freq={p.freq:.0f} amp={p.amplitude:.0f}".format(p=ping)
if abs(ping.freq - self.target_freq) < self.freq_tol and ping.amplitude > self.min_amp and self.listen:
trans, rot = None, None
try:
self.tf_listener.waitForTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp, rospy.Duration(0.25))
trans, rot = self.tf_listener.lookupTransform(self.map_frame, self.hydrophone_frame, ping.header.stamp)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print e
return
p0 = np.array([trans[0], trans[1]])
R = tf.transformations.quaternion_matrix(rot)[:3, :3]
delta = R.dot(navigator_tools.point_to_numpy(ping.position))[:2]
p1 = p0 + self.heading_pseudorange * delta / npl.norm(delta)
line_coeffs = np.array([[p0[0], p0[1], p1[0], p1[1]]]) # p0 and p1 define a line
self.visualize_arrow(Point(p0[0], p0[1], 0), Point(p1[0], p1[1], 0))
self.line_array = np.append(self.line_array, line_coeffs, 0)
self.observation_amplitudes = np.append(self.observation_amplitudes, ping.amplitude)
if len(self.line_array) >= self.max_observations: # delete softest samples if we have over max_observations
softest_idx = np.argmin(self.observation_amplitudes)
self.line_array = np.delete(self.line_array, softest_idx, axis=0)
self.observation_amplitudes = np.delete(self.observation_amplitudes, softest_idx)
print "PINGERFINDER: Observation collected. Total: {}".format(self.line_array.shape[0])
def do_update(self, *args):
resp = self.make_request(name='all')
if resp.found:
# temp
time_of_marker = rospy.Time.now() - ros_t(
.2) # header.stamp not filled out
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
return
header = navigator_tools.make_header(
frame="/enu", stamp=image_holder.time) #resp.objects[0].header
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker,
ros_t(1))
t_mat44 = self.tf_listener.asMatrix(
cam_tf, header) # homogenous 4x4 transformation matrix
for obj in resp.objects:
if len(obj.points) > 0 and obj.name == "totem":
fprint("{} {}".format(obj.id, "=" * 50))
print obj.position
# Get objects position in camera frame and make sure it's in view
object_cam = t_mat44.dot(
np.append(navigator_tools.point_to_numpy(obj.position),
1))
object_px = map(
int,
self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
continue
#print object_px
points_np = np.array(
map(navigator_tools.point_to_numpy, obj.points))
# We dont want points below a certain level
points_np = points_np[points_np[:, 2] > -2.5]
# Shove ones in there to make homogenous points
points_np_homo = np.hstack(
(points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel,
points_cam[:, :3])
#print points_px
roi = self._get_ROI_from_points(points_px)
color_info = self._get_color_from_ROI(
roi, image) # hue, string_color, error
bgr = (0, 0, 0)
if color_info is not None:
hue, color, error = color_info
c = (int(hue), 255, 255)
hsv = np.array([[c]])[:, :3].astype(np.uint8)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0]
bgr = tuple(bgr.astype(np.uint8).tolist())
if hue is not None:
if obj.id not in self.colored:
self.colored[obj.id] = []
self.colored[obj.id].append({
'color': color,
'err': error
})
if self.valid_color(obj.id):
fprint("COLOR IS VALID", msg_color='green')
self.make_request(
cmd=
'{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.
format(name=obj.name, _id=obj.id, bgr=bgr))
[
cv2.circle(self.debug.image, tuple(map(int, p)), 2,
bgr, -1) for p in points_px
]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id),
tuple(object_px), font, 1, bgr, 2)
print '\n' * 2
def main(navigator, **kwargs):
# rgb color map to param vlues
color_map = {'BLUE': [0, 0, 1], 'RED': [1, 0, 0], 'YELLOW': [1, 1, 0], 'GREEN': [0, 1, 0]}
explored_ids = []
all_found = False
# Get colors of intrest and directions
c1 = navigator.mission_params['totem_color_1'].get()
d1 = navigator.mission_params['totem_direction_1'].get()
c2 = navigator.mission_params['totem_color_2'].get()
d2 = navigator.mission_params['totem_direction_2'].get()
colors = [c1, c2]
directions = [d1, d2]
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
# We want to go to an observation point based on solar position
# TODO: Check which side to go to based on relative distance
center = navigator.move.set_position(buoy_field_point).set_orientation(get_solar_q())
obs_point = center.backward(BF_WIDTH / 2 + BF_EST_COFIDENCE)
yield obs_point.go()
# Next jig around to see buoys, first enforce that we're looking at the buoy field
buoy_field_point[2] = 1
#yield navigator.move.look_at(buoy_field_point).go(move_type='skid', focus=buoy_field_point)
yield navigator.nh.sleep(3)
#yield navigator.move.yaw_right(.5).go(move_type='skid')
#yield navigator.nh.sleep(3)
#yield navigator.move.yaw_left(1).go(move_type='skid')
# TODO: What if we don't see the colors?
for color, direction in zip(colors, directions):
color = yield color
direction = yield direction
target = yield get_colored_buoy(navigator, color_map[color])
if target is None or _dist_from_bf(navigator_tools.point_to_numpy(target.position)) > (BF_WIDTH / 2 + BF_EST_COFIDENCE):
# Need to do something
fprint("No suitable totems found.", msg_color='red', title="CIRCLE_TOTEM")
defer.returnValue(None)
target_np = navigator_tools.point_to_numpy(target.position)
set_up = navigator.move.look_at(target_np).set_position(target_np).backward(TOTEM_SAFE_DIST)
# Approach totem, making sure we actually get there.
res = yield set_up.go(initial_plan_time=2)
while res.failure_reason is not '':
set_up = set_up.backward(.1)
res = yield set_up.go(move_type='skid')
fprint("Going {}".format(direction), title="CIRCLE_TOTEM")
print TOTEM_SAFE_DIST - ROT_SAFE_DIST
if direction == "COUNTER-CLOCKWISE":
rot_move = navigator.move.yaw_right(1.57).left(TOTEM_SAFE_DIST - ROT_SAFE_DIST)
while (yield rot_move.go(move_type='skid')).failure_reason is not '':
rot_move = rot_move.right(.25)
print rot_move
circle = navigator.move.d_circle_point(target_np, radius=ROT_SAFE_DIST, direction='ccw', theta_offset=-1.57)
for c in circle:
yield c.go(move_type='skid')
elif direction == "CLOCKWISE":
rot_move = navigator.move.yaw_left(1.57).right(TOTEM_SAFE_DIST - ROT_SAFE_DIST)
while (yield rot_move.go(move_type='skid')).failure_reason is not '':
rot_move = rot_move.left(.25)
print rot_move
res = yield navigator.move.circle_point(target_np, direction='cw').go()
print "Mission result:", res
defer.returnValue(None)
def main(navigator, **kwargs):
# rgb color map to param vlues
color_map = {
'BLUE': [0, 0, 1],
'RED': [1, 0, 0],
'YELLOW': [1, 1, 0],
'GREEN': [0, 1, 0]
}
directions = {'RED': 'cw', 'GREEN': 'ccw', 'BLUE': 'cw', 'YELLOW': 'ccw'}
ogrid = OccupancyGridFactory(navigator)
explored_ids = []
all_found = False
# Get colors of intrest and directions
c1 = navigator.mission_params['scan_the_code_color1'].get()
c2 = navigator.mission_params['scan_the_code_color2'].get()
c3 = navigator.mission_params['scan_the_code_color3'].get()
colors = [c1, c2, c3]
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(
buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
# We want to go to an observation point based on solar position
center = navigator.move.set_position(buoy_field_point).set_orientation(
get_solar_q())
searched = []
for color in colors:
target = None
need_recolor = False
color = yield color
direction = directions[color]
fprint("Going to totem colored {} in direction {}".format(
color, direction),
title="CIRCLE_TOTEM")
target = yield get_colored_buoy(navigator, color_map[color])
if target is None or _dist_from_bf(
navigator_tools.point_to_numpy(
target.position)) > (BF_WIDTH / 2 + BF_EST_COFIDENCE):
# Need to do something
fprint(
"No suitable totems found, going to circle any nearby totems.",
msg_color='red',
title="CIRCLE_TOTEM")
target, searched = yield get_closest_totem(navigator, searched)
need_recolor = True
target_np = navigator_tools.point_to_numpy(target.position)
circler = navigator.move.d_circle_point(point=target_np,
radius=TOTEM_SAFE_DIST,
direction=direction)
# Give the totem a look at
for p in circler:
res = yield p.go(speed_factor=SPEED_FACTOR)
if res.failure_reason is '':
break
if need_recolor:
fprint("Recoloring...")
# Check for color?
yield navigator.nh.sleep(5)
target = yield navigator.database_query(str(target.id),
raise_exception=False)
if len(target.objects) == 0:
direction = directions[DEFAULT_COLOR]
else:
direction = directions[check_color(target.objects[0],
color_map)]
mult = 1 if direction == 'cw' else -1
left_offset = mult * CIRCLE_OFFSET
tangent_circler = navigator.move.d_circle_point(point=target_np,
radius=ROT_SAFE_DIST,
theta_offset=mult *
1.57,
direction=direction)
# Point tangent
for p in tangent_circler:
res = yield p.go(speed_factor=SPEED_FACTOR)
if res.failure_reason is '':
break
msg, goal = ogrid.circle_around(target_np, direction=direction)
latched_pub = ogrid.latching_publisher(msg)
yield navigator.nh.sleep(2)
goal = navigator.move.set_position(np.append(goal, 1)).look_at(
navigator.pose[0]).left(left_offset).backward(2)
yield goal.go(move_type='drive!', initial_plan_time=10)
latched_pub.cancel()
fprint("Canceling ogrid")
yield navigator.nh.sleep(3)
print "Mission result:", res
defer.returnValue(None)
def from_srv(cls, srv):
return cls(navigator_tools.point_to_numpy(srv.position), srv.color)
def do_update(self, *args):
resp = self.make_request(name='all')
if resp.found:
# temp
time_of_marker = rospy.Time.now() - ros_t(.2) # header.stamp not filled out
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
return
header = navigator_tools.make_header(frame="/enu", stamp=image_holder.time) #resp.objects[0].header
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
fprint("Getting transform between /enu and {}...".format(cam_tf))
self.tf_listener.waitForTransform("/enu", cam_tf, time_of_marker, ros_t(1))
t_mat44 = self.tf_listener.asMatrix(cam_tf, header) # homogenous 4x4 transformation matrix
for obj in resp.objects:
if len(obj.points) > 0 and obj.name == "totem":
fprint("{} {}".format(obj.id, "=" * 50))
print obj.position
# Get objects position in camera frame and make sure it's in view
object_cam = t_mat44.dot(np.append(navigator_tools.point_to_numpy(obj.position), 1))
object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
continue
#print object_px
points_np = np.array(map(navigator_tools.point_to_numpy, obj.points))
# We dont want points below a certain level
points_np = points_np[points_np[:, 2] > -2.5]
# Shove ones in there to make homogenous points
points_np_homo = np.hstack((points_np, np.ones((points_np.shape[0], 1)))).T
points_cam = t_mat44.dot(points_np_homo).T
points_px = map(self.camera_model.project3dToPixel, points_cam[:, :3])
#print points_px
roi = self._get_ROI_from_points(points_px)
color_info = self._get_color_from_ROI(roi, image) # hue, string_color, error
bgr = (0, 0, 0)
if color_info is not None:
hue, color, error = color_info
c = (int(hue), 255, 255)
hsv = np.array([[c]])[:, :3].astype(np.uint8)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)[0, 0]
bgr = tuple(bgr.astype(np.uint8).tolist())
if hue is not None:
if obj.id not in self.colored:
self.colored[obj.id] = []
self.colored[obj.id].append({'color':color, 'err':error})
if self.valid_color(obj.id):
fprint("COLOR IS VALID", msg_color='green')
self.make_request(cmd='{name}={bgr[2]},{bgr[1]},{bgr[0]},{_id}'.format(name=obj.name,_id= obj.id, bgr=bgr))
[cv2.circle(self.debug.image, tuple(map(int, p)), 2, bgr, -1) for p in points_px]
cv2.circle(self.debug.image, tuple(object_px), 10, bgr, -1)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, str(obj.id), tuple(object_px), font, 1, bgr, 2)
print '\n' * 2
def main(navigator, **kwargs):
# rgb color map to param vlues
color_map = {'BLUE': [0, 0, 1], 'RED': [1, 0, 0], 'YELLOW': [1, 1, 0], 'GREEN': [0, 1, 0]}
directions = {'RED': 'cw', 'GREEN': 'ccw', 'BLUE': 'cw', 'YELLOW': 'ccw'}
ogrid = OccupancyGridFactory(navigator)
explored_ids = []
all_found = False
# Get colors of intrest and directions
c1 = navigator.mission_params['scan_the_code_color1'].get()
c2 = navigator.mission_params['scan_the_code_color2'].get()
c3 = navigator.mission_params['scan_the_code_color3'].get()
colors = [c1, c2, c3]
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(buoy_field.objects[0].position)
_dist_from_bf = lambda pt: np.linalg.norm(buoy_field_point - pt)
# We want to go to an observation point based on solar position
center = navigator.move.set_position(buoy_field_point).set_orientation(get_solar_q())
searched = []
for color in colors:
target = None
need_recolor = False
color = yield color
direction = directions[color]
fprint("Going to totem colored {} in direction {}".format(color, direction), title="CIRCLE_TOTEM")
target = yield get_colored_buoy(navigator, color_map[color])
if target is None or _dist_from_bf(navigator_tools.point_to_numpy(target.position)) > (BF_WIDTH / 2 + BF_EST_COFIDENCE):
# Need to do something
fprint("No suitable totems found, going to circle any nearby totems.", msg_color='red', title="CIRCLE_TOTEM")
target, searched = yield get_closest_totem(navigator, searched)
need_recolor = True
target_np = navigator_tools.point_to_numpy(target.position)
circler = navigator.move.d_circle_point(point=target_np, radius=TOTEM_SAFE_DIST, direction=direction)
# Give the totem a look at
for p in circler:
res = yield p.go(speed_factor=SPEED_FACTOR)
if res.failure_reason is '':
break
if need_recolor:
fprint("Recoloring...")
# Check for color?
yield navigator.nh.sleep(5)
target = yield navigator.database_query(str(target.id), raise_exception=False)
if len(target.objects) == 0:
direction = directions[DEFAULT_COLOR]
else:
direction = directions[check_color(target.objects[0], color_map)]
mult = 1 if direction == 'cw' else -1
left_offset = mult * CIRCLE_OFFSET
tangent_circler = navigator.move.d_circle_point(point=target_np, radius=ROT_SAFE_DIST, theta_offset=mult * 1.57, direction=direction)
# Point tangent
for p in tangent_circler:
res = yield p.go(speed_factor=SPEED_FACTOR)
if res.failure_reason is '':
break
msg, goal = ogrid.circle_around(target_np, direction=direction)
latched_pub = ogrid.latching_publisher(msg)
yield navigator.nh.sleep(2)
goal = navigator.move.set_position(np.append(goal, 1)).look_at(navigator.pose[0]).left(left_offset).backward(2)
yield goal.go(move_type='drive!', initial_plan_time=10)
latched_pub.cancel()
fprint("Canceling ogrid")
yield navigator.nh.sleep(3)
print "Mission result:", res
defer.returnValue(None)
import tf
from tf import transformations as trans
ping_sub = yield navigator.nh.subscribe("/hydrophones/processed", ProcessedPing)
yield ping_sub.get_next_message()
target_freq = 35000
while True:
processed_ping = yield ping_sub.get_next_message()
print processed_ping
if isinstance(processed_ping, ProcessedPing):
print "Got processed ping message:\n{}".format(processed_ping)
if processed_ping.freq > 35000 and processed_ping.freq < 36000:
freq_dev = abs(target_freq - processed_ping.freq)
print "Trustworthy pinger heading"
hydrophones_enu_p, hydrophones_enu_q = tf.lookupTransform("/hydrophones", "/enu", processed_ping.header.stamp)
pinger_enu_p = navigator_tools.point_to_numpy(tf.transformPoint())
dir_ = navigator_tools.point_to_numpy(processed_ping.position)
mv_mag = 2
mv_hyd_frame = dir_ / np.linalg.norm(dir_)
pinger_move = navigator.move.set_position(navigator_tools.point_to_numpy(processed_ping.position)).go()
print "Heading towards pinger"
else:
print "Untrustworthy pinger heading. Freq = {} kHZ".format(processed_ping.freq)
else:
print "Expected ProcessedPing, got {}".format(type(processed_ping))
# Hydrophone locate mission
@txros.util.cancellableInlineCallbacks
def main(navigator):
kill_alarm_broadcaster, kill_alarm = single_alarm('kill', action_required=True, problem_description="Killing wamv to listen to pinger")
def from_srv(cls, srv):
return cls(navigator_tools.point_to_numpy(srv.position), srv.color)
def main(navigator):
result = navigator.fetch_result()
#middle_point = np.array([-10, -70, 0])
resp = yield navigator.database_query("coral_survey")
if not resp.found:
result.success = False
result.response = "coral_survey database entry not found!"
result.need_rerun = True
defer.returnValue(result)
middle_point = navigator_tools.point_to_numpy(resp.objects[0].position)
quads_to_search = [1, 2, 3, 4]
if (yield navigator.nh.has_param("/mission/coral_survey/quadrants")):
quads_to_search = yield navigator.nh.get_param(
"/mission/coral_survey/quadrants")
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
waypoints = []
for quad in quads_to_search:
mid = navigator.move.set_position(middle_point).set_orientation(
directions[quad - 1])
waypoints.append(
mid.yaw_left(
45,
"deg").forward(waypoint_from_center).set_orientation(NORTH))
# Get into the coral survey area
yield waypoints[0].go()
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid,
msg)
searcher = navigator.search("coral_survey", waypoints)
yield searcher.start_search(move_type='skid', spotings_req=1)
fprint("Centering over the thing!", title="CORAL_SURVEY")
# TODO: Center over the thing.
boat_position = (yield navigator.tx_pose)[0]
# TODO: make this easier
quad = np.argmin(
np.linalg.norm(boat_position -
[[w.pose[0][0][0], w.pose[0][1][0], w.pose[0][2][0]]
for w in waypoints],
axis=1))
quad = quads_to_search[quad]
fprint("Estimated quadrant: {}".format(quad),
title="CORAL_SURVEY",
msg_color='green')
yield navigator.nh.sleep(5)
defer.returnValue(result)
