def get_normal(self):
req = CameraToLidarTransformRequest()
req.header = self.found_shape.header
req.point = Point()
req.point.x = self.found_shape.CenterX
req.point.y = self.found_shape.CenterY
rect = self._bounding_rect(self.found_shape.points)
req.tolerance = int(min(rect[0] - rect[3], rect[1] - rect[4]) / 2.0)
self.normal_res = yield self.cameraLidarTransformer(req)
if self.normal_res.success:
transformObj = yield self.navigator.tf_listener.get_transform(
'/enu', '/' + req.header.frame_id)
self.enunormal = transformObj.transform_vector(
navigator_tools.rosmsg_to_numpy(self.normal_res.normal))
self.enupoint = transformObj.transform_point(
navigator_tools.rosmsg_to_numpy(self.normal_res.closest))
target_marker = Marker()
target_marker.scale.x = 0.1
target_marker.scale.y = 0.5
target_marker.scale.z = 0.5
target_marker.header.frame_id = "enu"
target_marker.action = Marker.ADD
target_marker.header.stamp = self.navigator.nh.get_time()
target_marker.points.append(
navigator_tools.numpy_to_point(self.enupoint))
target_marker.points.append(
navigator_tools.numpy_to_point(self.enupoint + self.enunormal))
target_marker.type = Marker.ARROW
#target_marker.text = "Shooter Target Pose"
target_marker.ns = "detect_deliver"
target_marker.id = 1
target_marker.color.r = 1
target_marker.color.a = 1
self.markers.markers.append(target_marker)
defer.returnValue(self.normal_res.success)
def get_dock_search_markers(self):
left_res = yield self.navigator.database_query("DockEdgeLeft")
left_pose = navigator_tools.rosmsg_to_numpy(left_res.objects[0].position)
right_res = yield self.navigator.database_query("DockEdgeRight")
right_pose = navigator_tools.rosmsg_to_numpy(right_res.objects[0].position)
search_line_vector = right_pose - left_pose
search_line_vector_normal = search_line_vector / np.linalg.norm(search_line_vector)
if np.isnan(search_line_vector_normal[0]):
raise Exception("Gate Edge Markers are not in a line. Perhaps they were not placed")
rot_right = np.array([[0, 1, 0], [-1, 0, 0], [0,0,0]])
search_line_rot = rot_right.dot(search_line_vector_normal)
left_search = left_pose + search_line_rot * self.MARKER_DISTANCE
right_search = right_pose + search_line_rot * self.MARKER_DISTANCE
move_left = self.navigator.move.set_position(left_search).look_at(left_pose)
move_right = self.navigator.move.set_position(right_search).look_at(right_pose)
pose = self.navigator.pose[0][:2]
distance_test = np.array([np.linalg.norm(pose - move_left.position[:2]),
np.linalg.norm(pose - move_right.position[:2])])
if np.argmin(distance_test) == 0:
self.search_moves = (move_left, move_right)
else:
self.search_moves = (move_right, move_left)
def circle_dock_bays(self):
'''
Circle the dock until the bays are identified by the perception service
'''
done_circle = False
@txros.util.cancellableInlineCallbacks
def circle_bay():
yield self.navigator.move.look_at(self.dock_pose).set_position(self.dock_pose).backward(self.CIRCLE_RADIUS).look_at(self.dock_pose).go()
yield self.navigator.move.circle_point(self.dock_pose, direction='ccw').go()
done_circle = True
print_good("Circling dock to get bays")
circle_bay()
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(self.BAY_SEARCH_TIMEOUT) and not done_circle:
res = yield self.call_get_bays()
if not res.success:
yield self.navigator.nh.sleep(0.1)
continue
self.bay_poses = (navigator_tools.rosmsg_to_numpy(res.bays[0]),
navigator_tools.rosmsg_to_numpy(res.bays[1]),
navigator_tools.rosmsg_to_numpy(res.bays[2]))
self.bay_normal = navigator_tools.rosmsg_to_numpy(res.normal)
print_good("Got GetDockShapes response")
return
raise Exception("Bays not found after circling or timed out")
def circle_dock_bays(self):
'''
Circle the dock until the bays are identified by the perception service
'''
done_circle = False
@txros.util.cancellableInlineCallbacks
def circle_bay():
yield self.navigator.move.look_at(self.dock_pose).set_position(
self.dock_pose).backward(self.CIRCLE_RADIUS).look_at(
self.dock_pose).go()
yield self.navigator.move.circle_point(self.dock_pose,
direction='ccw').go()
done_circle = True
print_good("Circling dock to get bays")
circle_bay()
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(
self.BAY_SEARCH_TIMEOUT) and not done_circle:
res = yield self.call_get_bays()
if not res.success:
yield self.navigator.nh.sleep(0.1)
continue
self.bay_poses = (navigator_tools.rosmsg_to_numpy(res.bays[0]),
navigator_tools.rosmsg_to_numpy(res.bays[1]),
navigator_tools.rosmsg_to_numpy(res.bays[2]))
self.bay_normal = navigator_tools.rosmsg_to_numpy(res.normal)
print_good("Got GetDockShapes response")
return
raise Exception("Bays not found after circling or timed out")
def get_normal(self):
req = CameraToLidarTransformRequest()
req.header = self.found_shape.header
req.point = Point()
req.point.x = self.found_shape.CenterX
req.point.y = self.found_shape.CenterY
req.tolerance = self.normal_approx_tolerance_proportion*self.found_shape.img_width
self.normal_res = yield self.cameraLidarTransformer(req)
if self.normal_res.success:
transformObj = yield self.navigator.tf_listener.get_transform('/enu', '/'+req.header.frame_id)
self.enunormal = transformObj.transform_vector(navigator_tools.rosmsg_to_numpy(self.normal_res.normal))
self.enupoint = transformObj.transform_point(navigator_tools.rosmsg_to_numpy(self.normal_res.closest))
target_marker = Marker()
target_marker.scale.x = 0.1;
target_marker.scale.y = 0.5;
target_marker.scale.z = 0.5;
target_marker.header.frame_id = "enu"
target_marker.action = Marker.ADD;
target_marker.header.stamp = self.navigator.nh.get_time()
target_marker.points.append(navigator_tools.numpy_to_point(self.enupoint))
target_marker.points.append(navigator_tools.numpy_to_point(self.enupoint+self.enunormal))
target_marker.type = Marker.ARROW
#target_marker.text = "Shooter Target Pose"
target_marker.ns = "detect_deliver"
target_marker.id = 1
target_marker.color.r = 1
target_marker.color.a = 1
self.markers.markers.append(target_marker)
defer.returnValue(self.normal_res.success)
def circle_search(self):
print "Starting circle search"
yield self.navigator.move.look_at(navigator_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position)).go()
pattern = self.navigator.move.circle_point(navigator_tools.rosmsg_to_numpy(
self.waypoint_res.objects[0].position), radius=self.circle_radius, theta_offset=self.theta_offset)
yield next(pattern).go()
searcher = self.navigator.search(
vision_proxy='get_shape', search_pattern=pattern, Shape=self.Shape, Color=self.Color)
yield searcher.start_search(timeout=self.search_timeout_seconds, spotings_req=self.spotings_req, move_type="skid")
print "Ended Circle Search"
def get_closest_objects(position, objects, max_len=3, max_dist=30):
num = len(objects)
idx = max_len
if num < max_len:
idx = num
objects = sorted(objects, key=lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)))
objects = objects[:idx]
dists = map(lambda x: np.linalg.norm(position - rosmsg_to_numpy(x.position)), objects)
final_objs = []
for i, d in enumerate(dists):
if d < max_dist:
final_objs.append(objects[i])
else:
break
return final_objs
def get_objects(self):
"""Get position of 3 gates from database"""
gate_1 = yield self.navigator.database_query("Gate_1")
assert gate_1.found, "Gate 1 Not found"
gate_1_pos = navigator_tools.rosmsg_to_numpy(gate_1.objects[0].position)[:2]
gate_2 = yield self.navigator.database_query("Gate_2")
assert gate_2.found, "Gate 2 Not found"
gate_2_pos = navigator_tools.rosmsg_to_numpy(gate_2.objects[0].position)[:2]
gate_3 = yield self.navigator.database_query("Gate_3")
assert gate_3.found, "Gate 3 Not found"
gate_3_pos = navigator_tools.rosmsg_to_numpy(gate_3.objects[0].position)[:2]
self.gate_poses = np.array([gate_1_pos, gate_2_pos, gate_3_pos])
def check_color(totem, color_map):
for name, color in color_map:
if navigator_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g',
'b']) == color:
return name
return DEFAULT_COLOR
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 move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = navigator_tools.make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(navigator_tools.rosmsg_to_numpy(msg.goal.orientation))[2]
if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def go_to_objects(navigator, position, objs):
objects = get_closest_objects(position, objs)
for o in objects:
fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green")
yield navigator.nh.sleep(5)
pos = nt.rosmsg_to_numpy(o.position)
yield navigator.move.look_at(pos).set_position(pos).backward(7).go()
def get_pinger_pose(self):
"""Query pinger perception for the location of the pinger after observing"""
res = yield self.pinger_client(FindPingerRequest())
pinger_pose = res.pinger_position
self.pinger_pose = navigator_tools.rosmsg_to_numpy(pinger_pose)[:2]
self.distances = np.array([np.linalg.norm(self.pinger_pose - self.gate_poses[0]),
np.linalg.norm(self.pinger_pose - self.gate_poses[1]),
np.linalg.norm(self.pinger_pose - self.gate_poses[2])])
def get_objects(self):
"""Get position of 3 gates from database"""
gate_1 = yield self.navigator.database_query("Gate_1")
assert gate_1.found, "Gate 1 Not found"
gate_1_pos = navigator_tools.rosmsg_to_numpy(
gate_1.objects[0].position)[:2]
gate_2 = yield self.navigator.database_query("Gate_2")
assert gate_2.found, "Gate 2 Not found"
gate_2_pos = navigator_tools.rosmsg_to_numpy(
gate_2.objects[0].position)[:2]
gate_3 = yield self.navigator.database_query("Gate_3")
assert gate_3.found, "Gate 3 Not found"
gate_3_pos = navigator_tools.rosmsg_to_numpy(
gate_3.objects[0].position)[:2]
self.gate_poses = np.array([gate_1_pos, gate_2_pos, gate_3_pos])
def get_pinger_pose(self):
"""Query pinger perception for the location of the pinger after observing"""
res = yield self.pinger_client(FindPingerRequest())
pinger_pose = res.pinger_position
self.pinger_pose = navigator_tools.rosmsg_to_numpy(pinger_pose)[:2]
self.distances = np.array([
np.linalg.norm(self.pinger_pose - self.gate_poses[0]),
np.linalg.norm(self.pinger_pose - self.gate_poses[1]),
np.linalg.norm(self.pinger_pose - self.gate_poses[2])
])
def _dist(self, x):
if self.position is None:
success = yield threads.deferToThread(self._wait_for_position)
if not success:
raise Exception("There is a problem with odom.")
if self.navigator is not None:
position = yield self.navigator.tx_pose
position = position[0]
self.position = position
defer.returnValue(np.linalg.norm(nt.rosmsg_to_numpy(x.position) - self.position))
def myfunc(navigator, looking_for, center_marker):
# Updated
high_prob_objs = ["shooter", "dock"]
pos = yield navigator.tx_pose
pos = pos[0]
if center_marker is None or center_marker == "None":
defer.returnValue(True)
try:
center_marker = yield navigator.database_query(object_name=center_marker.name)
center_marker = center_marker.objects[0]
print center_marker.name
except:
fprint("A marker has not been set", msg_color="red")
defer.returnValue(False)
mark_pos = nt.rosmsg_to_numpy(center_marker.position)
dist = np.linalg.norm(pos - mark_pos)
if dist > 10:
yield navigator.move.look_at(mark_pos).set_position(mark_pos).go()
defer.returnValue(True)
if looking_for is None or looking_for == "None":
defer.returnValue(True)
pos = yield navigator.tx_pose
pos = pos[0]
if looking_for in high_prob_objs:
try:
obj = yield navigator.database_query(object_name=looking_for)
fprint("EXPLORER FOUND OBJECT", msg_color="blue")
yield navigator.database_query(cmd="lock {} {}".format(obj.id, looking_for))
defer.returnValue(True)
except MissingPerceptionObject:
fprint("The object {} is not in the database".format(looking_for), msg_color="red")
try:
objs = yield navigator.database_query(object_name="all")
objs = get_closest_objects(pos, objs.objects)
except Exception as e:
print e
defer.returnValue(False)
# for o in objs:
# obj_pos = nt.rosmsg_to_numpy(o.position)
# yield navigator.move.look_at(obj_pos).set_position(mark_pos).backward(7).go()
# cam_obj = yield navigator.camera_database_query(object_name=looking_for, id=o.id)
# if cam_obj.found:
# yield navigator.database_query(cmd="lock {} {}".format(o.id, looking_for))
# fprint("EXPLORER FOUND OBJECT", msg_color="blue")
# defer.returnValue(True)
fprint("NO OBJECT FOUND", msg_color="blue")
defer.returnValue(False)
def get_normal(self):
req = CameraToLidarTransformRequest()
req.header = self.found_shape.header
req.point = Point()
req.point.x = self.found_shape.CenterX
req.point.y = self.found_shape.CenterY
rect = self._bounding_rect(self.found_shape.points)
req.tolerance = int(min(rect[0]-rect[3],rect[1]-rect[4])/2.0)
self.normal_res = yield self.cameraLidarTransformer(req)
if not self.normal_res.success:
defer.returnValue(False)
if not self.normal_is_sane(self.normal_res.normal):
self.normal_res.success = False
self.normal_res.error = "UNREASONABLE NORMAL"
defer.returnValue(False)
enu_cam_tf = yield self.navigator.tf_listener.get_transform('/enu', '/'+req.header.frame_id)
self.shooter_baselink_tf = yield self.navigator.tf_listener.get_transform('/base_link','/shooter')
self.enunormal = enu_cam_tf.transform_vector(navigator_tools.rosmsg_to_numpy(self.normal_res.normal))
self.enupoint = enu_cam_tf.transform_point(navigator_tools.rosmsg_to_numpy(self.normal_res.closest))
defer.returnValue(True)
def _do_mission(self, mission, **kwargs):
"""Perform a mission, and ensure that all of the post conditions are enforced."""
if "redo" not in kwargs:
redo = False
else:
redo = kwargs["redo"]
if not redo and not self.sim_mode and mission.item_dep is not None:
print mission.item_dep.name
marker = yield self.navigator.database_query(mission.item_dep.name)
marker = marker.objects[0]
print nt.rosmsg_to_numpy(marker.position)
move = self.navigator.move.set_position(nt.rosmsg_to_numpy(marker.position))
print move
yield move.go()
mission.start_time = self.nh.get_time()
mission.attempts += 1
res = yield mission.do_mission(self.navigator, self, self.module, **kwargs)
defer.returnValue(res)
def get_all_object_rois(self):
req = ObjectDBQueryRequest()
req.name = 'all'
obj = yield self._database(req)
if obj is None or not obj.found:
defer.returnValue((None, None))
rois = []
ros_img = yield self._get_closest_image(obj.objects[0].header.stamp)
if ros_img is None:
defer.returnValue((None, None))
img = self.bridge.imgmsg_to_cv2(ros_img, "mono8")
for o in obj.objects:
if o.id not in self.id_to_perist:
self.id_to_perist[o.id] = []
ppoints = self.id_to_perist[o.id]
ppoints.extend(o.points)
if len(ppoints) > 500:
ppoints = ppoints[:500]
if self.training and o.name not in self.classes:
continue
position = yield self._get_position()
o_pos = nt.rosmsg_to_numpy(o.position)
diff = np.linalg.norm(position - o_pos)
if diff > self.max_dist:
continue
points, bbox = yield self._get_bounding_box_2d(ppoints, obj.objects[0].header.stamp)
if bbox is None:
continue
xmin, ymin, xmax, ymax = bbox
h, w, r = img.shape
if xmin < 0 or xmax < 0 or xmin > w or xmax > w or xmax - xmin == 0 or ymax - ymin == 0:
continue
if ymin < 0:
ymin = 0
print "bbox", bbox
roi = img[ymin:ymax, xmin:xmax]
print "preshape", roi.shape
roi = self._resize_image(roi)
print "postshape", roi.shape
ret_obj = {}
ret_obj["id"] = o.id
ret_obj["points"] = points
ret_obj["img"] = roi
ret_obj["time"] = o.header.stamp
ret_obj["name"] = o.name
ret_obj["bbox"] = [xmin, ymin, xmax, ymax]
rois.append(ret_obj)
defer.returnValue((img, rois))
def get_dock_search_markers(self):
left_res = yield self.navigator.database_query("DockEdgeLeft")
left_pose = navigator_tools.rosmsg_to_numpy(
left_res.objects[0].position)
right_res = yield self.navigator.database_query("DockEdgeRight")
right_pose = navigator_tools.rosmsg_to_numpy(
right_res.objects[0].position)
search_line_vector = right_pose - left_pose
search_line_vector_normal = search_line_vector / np.linalg.norm(
search_line_vector)
if np.isnan(search_line_vector_normal[0]):
raise Exception(
"Gate Edge Markers are not in a line. Perhaps they were not placed"
)
rot_right = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 0]])
search_line_rot = rot_right.dot(search_line_vector_normal)
left_search = left_pose + search_line_rot * self.MARKER_DISTANCE
right_search = right_pose + search_line_rot * self.MARKER_DISTANCE
move_left = self.navigator.move.set_position(left_search).look_at(
left_pose)
move_right = self.navigator.move.set_position(right_search).look_at(
right_pose)
pose = self.navigator.pose[0][:2]
distance_test = np.array([
np.linalg.norm(pose - move_left.position[:2]),
np.linalg.norm(pose - move_right.position[:2])
])
if np.argmin(distance_test) == 0:
self.search_moves = (move_left, move_right)
else:
self.search_moves = (move_right, move_left)
def get_objects_in_radius(self, pos, radius, objects="all"):
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
ans = []
if not resp.found:
defer.returnValue(ans)
for o in resp.objects:
if objects == "all" or o.name in objects:
dist = np.linalg.norm(pos - nt.rosmsg_to_numpy(o.position))
if dist < radius:
ans.append(o)
defer.returnValue(ans)
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 circle_search(self):
print "Starting circle search"
#yield self.navigator.move.look_at(navigator_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position)).go()
pattern = self.navigator.move.circle_point(
navigator_tools.rosmsg_to_numpy(
self.waypoint_res.objects[0].position),
radius=self.circle_radius,
theta_offset=self.theta_offset)
yield next(pattern).go()
searcher = self.navigator.search(vision_proxy='get_shape',
search_pattern=pattern,
Shape=self.Shape,
Color=self.Color)
yield searcher.start_search(timeout=self.search_timeout_seconds,
spotings_req=self.spotings_req,
move_type="skid")
print "Ended Circle Search"
def myfunc(navigator, **kwargs):
pos = yield navigator.tx_pose
pos = pos[0]
exploring = ["Exploring1", "Exploring2", "Exploring3", "Exploring4"]
for e in exploring:
try:
objects = yield navigator.database_query(e)
o = objects.objects[0]
print o.name
pos = nt.rosmsg_to_numpy(o.position)
yield navigator.move.set_position(pos).go()
nt.fprint(o.name, msg_color='green')
except:
nt.fprint("Missing Marker", msg_color="red")
pass
def circle_search(self):
platform_np = navigator_tools.rosmsg_to_numpy(self.waypoint_res.objects[0].position)
yield self.navigator.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius).yaw_left(90,unit='deg').go(move_type="drive")
done_circle = False
@txros.util.cancellableInlineCallbacks
def do_circle():
yield self.navigator.move.circle_point(platform_np, direction=self.circle_direction).go()
done_circle = True
circle_defer = do_circle()
while not done_circle:
res = yield self.get_any_shape()
if res == False:
yield self.navigator.nh.sleep(0.25)
continue
fprint("Shape ({}found, using normal to look at other 3 shapes if needed".format(res[0]), title="DETECT DELIVER", msg_color="green")
# circle_defer.cancel()
shape_color, found_shape_pose = res
if self.correct_shape(shape_color):
self.shape_pose = found_shape_pose
return
#Pick other 3 to look at
rot_right = np.array([[0, -1], [1, 0]])
(shape_point, shape_normal) = found_shape_pose
rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0)
center_point = shape_point - shape_normal * (self.platform_radius/2.0)
point_opposite_side = center_point - shape_normal * self.circle_radius
move_opposite_side = self.navigator.move.set_position(point_opposite_side).look_at(center_point).yaw_left(90, unit='deg')
left_or_whatever_point = center_point + rotated_norm * self.circle_radius
move_left_or_whatever = self.navigator.move.set_position(left_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg')
right_or_whatever_point = center_point - rotated_norm * self.circle_radius
move_right_or_whatever = self.navigator.move.set_position(right_or_whatever_point).look_at(center_point).yaw_left(90, unit='deg')
yield self.search_sides((move_right_or_whatever, move_opposite_side, move_left_or_whatever))
return
fprint("No shape found after complete circle",title="DETECT DELIVER", msg_color='red')
raise Exception("No shape found on platform")
def _get_bounding_box_2d(self, points_3d_enu, time):
if self.cam_info is None:
defer.returnValue((None, None))
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.cam_info)
points_2d = []
try:
trans = yield self.tf_listener.get_transform(self.tf_frame, "/enu", time)
except Exception:
defer.returnValue((None, None))
transformation = trans.as_matrix()
for point in points_3d_enu:
point = nt.rosmsg_to_numpy(point)
point = np.append(point, 1.0)
t_p = transformation.dot(point)
if t_p[3] < 1E-15:
defer.returnValue((None, None))
t_p[0] /= t_p[3]
t_p[1] /= t_p[3]
t_p[2] /= t_p[3]
t_p = t_p[0:3]
if t_p[2] < 0:
defer.returnValue((None, None))
point_2d = self.camera_model.project3dToPixel(t_p)
points_2d.append((int(point_2d[0]), int(point_2d[1])))
xmin, ymin = sys.maxint, sys.maxint
xmax, ymax = -sys.maxint, -sys.maxint
for i, point in enumerate(points_2d):
if point[0] < xmin:
xmin = point[0]
if point[0] > xmax:
xmax = point[0]
if point[1] < ymin:
ymin = point[1]
if point[1] > ymax:
ymax = point[1]
defer.returnValue((points_2d, (xmin, ymin, xmax, ymax)))
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.rosmsg_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 get_stc_points(self, msg, stc_pos):
trans = yield self.my_tf.get_transform("/stereo_right_cam", "/velodyne", msg.header.stamp)
trans1 = yield self.my_tf.get_transform("/stereo_right_cam", "/enu", msg.header.stamp)
stc_pos = rosmsg_to_numpy(stc_pos)
stc_pos = np.append(stc_pos, 1)
position = trans1.as_matrix().dot(stc_pos)
if position[3] < 1E-15:
raise ZeroDivisionError
position[0] /= position[3]
position[1] /= position[3]
position[2] /= position[3]
position = position[:3]
stereo_points = []
for point in pc2.read_points(msg, skip_nans=True):
stereo_points.append(np.array([point[0], point[1], point[2], 1]))
stereo_points = map(lambda x: trans.as_matrix().dot(x), stereo_points)
points = []
for p in stereo_points:
if p[3] < 1E-15:
raise ZeroDivisionError
p[0] /= p[3]
p[1] /= p[3]
p[2] /= p[3]
points.append(p[:3])
points_keep = []
for p in points:
# print npl.norm(p - poition)
if npl.norm(p - position) < 20:
points_keep.append(p)
points_keep = sorted(points_keep, key=lambda x: x[1])
keep_num = int(.1 * len(points_keep))
points_keep = points_keep[:keep_num]
# self.pers_points.extend(points_keep)
# max_num = 200
# if len(self.pers_points) > max_num:
# self.pers_points = self.pers_points[len(self.pers_points) - max_num:len(self.pers_points)]
defer.returnValue(points_keep)
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.rosmsg_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 get_shape_pos(self, normal_res, enu_cam_tf):
enunormal = enu_cam_tf.transform_vector(navigator_tools.rosmsg_to_numpy(normal_res.normal))
enupoint = enu_cam_tf.transform_point(navigator_tools.rosmsg_to_numpy(normal_res.closest))
return (enupoint, enunormal)
def get_colored_buoys(self):
estimated_3_endbuoy = self.gate_poses[2] + (self.g_line * 5.0)
estimated_1_endbuoy = self.gate_poses[0] - (self.g_line * 5.0)
if self.negate:
estimated_circle_buoy = self.gate_poses[1] + (
self.g_perp * 30.0
) #should check on correct side (negate bool)
else:
estimated_circle_buoy = self.gate_poses[1] - (
self.g_perp * 30.0
) #should check on correct side (negate bool)
cur_time = yield self.navigator.nh.get_time()
self.new_marker(position=np.append(estimated_3_endbuoy, 0),
color=(1, 1, 1),
time=cur_time)
self.new_marker(position=np.append(estimated_circle_buoy, 0),
time=cur_time)
self.new_marker(position=np.append(estimated_1_endbuoy, 0),
color=(1, 1, 1),
time=cur_time)
totems = yield self.navigator.database_query("totem",
raise_exception=False)
if totems.found:
sorted_1 = sorted(totems.objects,
key=lambda t: np.linalg.
norm(estimated_1_endbuoy - navigator_tools.
rosmsg_to_numpy(t.position)[:2]))
sorted_3 = sorted(totems.objects,
key=lambda t: np.linalg.
norm(estimated_3_endbuoy - navigator_tools.
rosmsg_to_numpy(t.position)[:2]))
sorted_circle = sorted(
totems.objects,
key=lambda t: abs(
np.linalg.norm(estimated_circle_buoy - navigator_tools.
rosmsg_to_numpy(t.position)[:2])))
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_circle[0].position),
color=(1, 1, 1),
time=cur_time)
if np.linalg.norm(
navigator_tools.rosmsg_to_numpy(sorted_circle[0].position)
[:2] - estimated_circle_buoy) < self.MAX_CIRCLE_BUOY_ERROR:
self.circle_totem = navigator_tools.rosmsg_to_numpy(
sorted_circle[0].position)
fprint("PINGER: found buoy to circle at {}".format(
self.circle_totem),
msg_color='green')
else:
fprint(
"PINGER: circle buoy is too far from where it should be",
msg_color='red')
if sorted_3[0].color.r > 0.9:
self.color_wrong = True
color_3 = "RED"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_3[0].position),
color=(1, 0, 0),
time=cur_time)
elif sorted_3[0].color.g > 0.9:
color_3 = "GREEN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_3[0].position),
color=(0, 1, 0),
time=cur_time)
else:
color_3 = "UNKNOWN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_3[0].position),
color=(1, 1, 1),
time=cur_time)
if sorted_1[0].color.r > 0.9:
color_1 = "RED"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_1[0].position),
color=(1, 0, 0),
time=cur_time)
elif sorted_1[0].color.g > 0.9:
self.color_wrong = True
color_1 = "GREEN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_1[0].position),
color=(0, 1, 0),
time=cur_time)
else:
color_1 = "UNKNOWN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(
sorted_1[0].position),
color=(1, 1, 1),
time=cur_time)
if int(self.gate_index) == 0:
if color_1 == "RED":
active_colors = "RED-WHITE"
elif color_1 == "GREEN":
active_colors = "WHITE-GREEN"
else:
active_colors = "UNKNOWN"
elif int(self.gate_index) == 2:
if color_3 == "RED":
active_colors = "RED-WHITE"
elif color_3 == "GREEN":
active_colors = "WHITE-GREEN"
else:
active_colors = "UNKNOWN"
else:
active_colors = "WHITE-WHITE"
if active_colors != "UNKNOWN":
fprint("PINGER: setting active pinger colors to {}".format(
active_colors),
msg_color='green')
yield self.navigator.mission_params[
"acoustic_pinger_active"].set(active_colors)
else:
fprint("PINGER: cannot determine gate colors".format(
sorted_3[0].color),
msg_color='red')
# fprint("PINGER: gate 3 color {}".format(sorted_3[0].color), msg_color='blue')
# fprint("PINGER: gate 1 color {}".format(sorted_1[0].color), msg_color='blue')
else:
fprint("PINGER: no totems found", msg_color='red')
yield self.marker_pub.publish(self.markers)
def check_color(totem, color_map):
for name, color in color_map:
if navigator_tools.rosmsg_to_numpy(totem.color, keys=['r', 'g', 'b']) == color:
return name
return DEFAULT_COLOR
def get_colored_buoys(self):
estimated_3_endbuoy = self.gate_poses[2] + (self.g_line * 5.0)
estimated_1_endbuoy = self.gate_poses[0] - (self.g_line * 5.0)
if self.negate:
estimated_circle_buoy = self.gate_poses[1] + (self.g_perp * 30.0) #should check on correct side (negate bool)
else:
estimated_circle_buoy = self.gate_poses[1] - (self.g_perp * 30.0) #should check on correct side (negate bool)
cur_time = yield self.navigator.nh.get_time()
self.new_marker(position=np.append(estimated_3_endbuoy,0), color=(1,1,1), time=cur_time)
self.new_marker(position=np.append(estimated_circle_buoy,0), time=cur_time)
self.new_marker(position=np.append(estimated_1_endbuoy,0), color=(1,1,1), time=cur_time)
totems = yield self.navigator.database_query("totem", raise_exception=False)
if totems.found:
sorted_1 = sorted(totems.objects, key=lambda t: np.linalg.norm(estimated_1_endbuoy - navigator_tools.rosmsg_to_numpy(t.position)[:2]))
sorted_3 = sorted(totems.objects, key=lambda t: np.linalg.norm(estimated_3_endbuoy - navigator_tools.rosmsg_to_numpy(t.position)[:2]))
sorted_circle = sorted(totems.objects, key=lambda t: abs(np.linalg.norm(estimated_circle_buoy - navigator_tools.rosmsg_to_numpy(t.position)[:2])))
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_circle[0].position), color=(1,1,1), time=cur_time)
if np.linalg.norm(navigator_tools.rosmsg_to_numpy(sorted_circle[0].position)[:2] - estimated_circle_buoy) < self.MAX_CIRCLE_BUOY_ERROR:
self.circle_totem = navigator_tools.rosmsg_to_numpy(sorted_circle[0].position)
fprint("PINGER: found buoy to circle at {}".format(self.circle_totem), msg_color='green')
else:
fprint("PINGER: circle buoy is too far from where it should be", msg_color='red')
if sorted_3[0].color.r > 0.9:
self.color_wrong = True
color_3 = "RED"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_3[0].position), color=(1,0,0), time=cur_time)
elif sorted_3[0].color.g > 0.9:
color_3 = "GREEN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_3[0].position), color=(0,1,0), time=cur_time)
else:
color_3 = "UNKNOWN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_3[0].position), color=(1,1,1), time=cur_time)
if sorted_1[0].color.r > 0.9:
color_1 = "RED"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_1[0].position), color=(1,0,0), time=cur_time)
elif sorted_1[0].color.g > 0.9:
self.color_wrong = True
color_1 = "GREEN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_1[0].position), color=(0,1,0), time=cur_time)
else:
color_1 = "UNKNOWN"
self.new_marker(position=navigator_tools.rosmsg_to_numpy(sorted_1[0].position), color=(1,1,1), time=cur_time)
if int(self.gate_index) == 0:
if color_1 == "RED":
active_colors = "RED-WHITE"
elif color_1 == "GREEN":
active_colors = "WHITE-GREEN"
else:
active_colors = "UNKNOWN"
elif int(self.gate_index) == 2:
if color_3 == "RED":
active_colors = "RED-WHITE"
elif color_3 == "GREEN":
active_colors = "WHITE-GREEN"
else:
active_colors = "UNKNOWN"
else:
active_colors = "WHITE-WHITE"
if active_colors != "UNKNOWN":
fprint("PINGER: setting active pinger colors to {}".format(active_colors), msg_color='green')
yield self.navigator.mission_params["acoustic_pinger_active"].set(active_colors)
else:
fprint("PINGER: cannot determine gate colors".format(sorted_3[0].color), msg_color='red')
# fprint("PINGER: gate 3 color {}".format(sorted_3[0].color), msg_color='blue')
# fprint("PINGER: gate 1 color {}".format(sorted_1[0].color), msg_color='blue')
else:
fprint("PINGER: no totems found", msg_color='red')
yield self.marker_pub.publish(self.markers)
def circle_search(self):
platform_np = navigator_tools.rosmsg_to_numpy(
self.waypoint_res.objects[0].position)
yield self.navigator.move.look_at(platform_np).set_position(
platform_np).backward(self.circle_radius).yaw_left(
90, unit='deg').go(move_type="drive")
done_circle = False
@txros.util.cancellableInlineCallbacks
def do_circle():
yield self.navigator.move.circle_point(
platform_np, direction=self.circle_direction).go()
done_circle = True
circle_defer = do_circle()
while not done_circle:
res = yield self.get_any_shape()
if res == False:
yield self.navigator.nh.sleep(0.25)
continue
fprint(
"Shape ({}found, using normal to look at other 3 shapes if needed"
.format(res[0]),
title="DETECT DELIVER",
msg_color="green")
# circle_defer.cancel()
shape_color, found_shape_pose = res
if self.correct_shape(shape_color):
self.shape_pose = found_shape_pose
return
#Pick other 3 to look at
rot_right = np.array([[0, -1], [1, 0]])
(shape_point, shape_normal) = found_shape_pose
rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0)
center_point = shape_point - shape_normal * (self.platform_radius /
2.0)
point_opposite_side = center_point - shape_normal * self.circle_radius
move_opposite_side = self.navigator.move.set_position(
point_opposite_side).look_at(center_point).yaw_left(90,
unit='deg')
left_or_whatever_point = center_point + rotated_norm * self.circle_radius
move_left_or_whatever = self.navigator.move.set_position(
left_or_whatever_point).look_at(center_point).yaw_left(
90, unit='deg')
right_or_whatever_point = center_point - rotated_norm * self.circle_radius
move_right_or_whatever = self.navigator.move.set_position(
right_or_whatever_point).look_at(center_point).yaw_left(
90, unit='deg')
yield self.search_sides(
(move_right_or_whatever, move_opposite_side,
move_left_or_whatever))
return
fprint("No shape found after complete circle",
title="DETECT DELIVER",
msg_color='red')
raise Exception("No shape found on platform")
def get_waypoint(self):
res = yield self.navigator.database_query(self.WAYPOINT_NAME)
self.dock_pose = navigator_tools.rosmsg_to_numpy(
res.objects[0].position)
def get_shape_pos(self, normal_res, enu_cam_tf):
enunormal = enu_cam_tf.transform_vector(
navigator_tools.rosmsg_to_numpy(normal_res.normal))
enupoint = enu_cam_tf.transform_point(
navigator_tools.rosmsg_to_numpy(normal_res.closest))
return (enupoint, enunormal)
def normal_is_sane(self, vector3):
return abs(navigator_tools.rosmsg_to_numpy(vector3)[1]) < 0.4
def get_waypoint(self):
res = yield self.navigator.database_query(self.WAYPOINT_NAME)
self.dock_pose = navigator_tools.rosmsg_to_numpy(res.objects[0].position)
def normal_is_sane(self, vector3):
return abs(navigator_tools.rosmsg_to_numpy(vector3)[1]) < 0.4
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.rosmsg_to_numpy(tf.transformPoint())
dir_ = navigator_tools.rosmsg_to_numpy(processed_ping.position)
mv_mag = 2
mv_hyd_frame = dir_ / np.linalg.norm(dir_)
pinger_move = navigator.move.set_position(navigator_tools.rosmsg_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")