def get_object_by_id(self, my_id):
print my_id
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
ans = [obj for obj in resp.objects if obj.id == my_id][0]
defer.returnValue(ans)
def get_object_by_id(self, my_id):
print my_id
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
ans = [obj for obj in resp.objects if obj.id == my_id][0]
defer.returnValue(ans)
def get_unknown_and_low_conf(self):
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
m = []
for o in resp.objects:
if o.name == 'unknown':
m.append(o)
elif o.confidence < 50:
pass
# m.append(o)
defer.returnValue(m)
def get_unknown_and_low_conf(self):
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
m = []
for o in resp.objects:
if o.name == 'unknown':
m.append(o)
elif o.confidence < 50:
pass
# m.append(o)
defer.returnValue(m)
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_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_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 poller(nh):
db_srv = nh.get_service_client('/database/requests', ObjectDBQuery)
while True:
yield util.wall_sleep(3)
while True:
try:
db_res = yield db_srv(
ObjectDBQueryRequest(
name='shooter',
cmd='',
))
except:
traceback.print_exc()
yield util.wall_sleep(1)
else:
break
if not db_res.found:
print 'shooter not found'
continue
obj = db_res.objects[0]
points = map(msg_helpers.ros_to_np_3D, obj.points)
if points:
center_holder[0] = numpy.mean(points, axis=0)
def begin_observing(self, cb):
"""
Get notified when a new object is added to the database.
cb : The callback that is called when a new object is added to the database
"""
self.new_object_subscriber = cb
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
for o in resp.objects:
# The is upper call is because the first case is upper case if it is a 'fake' object... WHYYYYY
if o.name not in self.found and not o.name[0].isupper():
self.found.add(o.name)
self.new_object_subscriber(o)
def get_object(self, object_name, volume_only=False, thresh=50, thresh_strict=50):
"""Get an object from the database."""
if volume_only:
req = ObjectDBQueryRequest()
req.name = object_name
resp = yield self._database(req)
if not resp.found:
raise MissingPerceptionObject(object_name)
defer.returnValue(resp.objects)
else:
req = ObjectDBQueryRequest()
req.name = "all"
resp = yield self._database(req)
closest_potential_object = None
min_dist = sys.maxint
actual_objects = []
for o in resp.objects:
distance = yield self._dist(o)
if o.name == object_name and distance < thresh_strict:
actual_objects.append(o)
if distance < thresh and distance < min_dist:
min_dist = distance
closest_potential_object = o
# print [x.name for x in actual_objects]
# print closest_potential_object.name
# sys.exit()
if len(actual_objects) == 0 and min_dist == sys.maxint:
raise MissingPerceptionObject(object_name)
if len(actual_objects) > 1:
min_dist = sys.maxint
min_obj = None
for o in actual_objects:
dist = yield self._dist(o)
if dist < min_dist:
min_dist = dist
min_obj = o
defer.returnValue(min_obj)
if len(actual_objects) == 1:
defer.returnValue(actual_objects[0])
defer.returnValue(closest_potential_object)
def __init__(self):
info_topic = camera_root + "/camera_info"
image_topic = camera_root + "/image_raw" # Change this to rect on boat
self.tf_listener = tf.TransformListener()
# Map id => [{color, error}, ...] for determining when a color is good
self.colored = {}
db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
self.make_request = lambda **kwargs: db_request(
ObjectDBQueryRequest(**kwargs))
rospy.Service("/vision/buoy_colorizer", VisionRequest,
self.got_request)
self.image_history = ImageHistory(image_topic)
# Wait for camera info, and exit if not found
try:
fprint("Waiting for camera info on: '{}'".format(info_topic))
camera_info_msg = rospy.wait_for_message(info_topic,
CameraInfo,
timeout=3)
except rospy.exceptions.ROSException:
fprint("Camera info not found! Terminating.", msg_color="red")
rospy.signal_shutdown("Camera not found!")
return
fprint("Camera info found!", msg_color="green")
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info_msg)
self.debug = DebugImage("/colorama/debug", prd=.5)
# These are color mappings from Hue values [0, 360]
self.color_map = {
'red': np.radians(0),
'yellow': np.radians(60),
'green': np.radians(120),
'blue': np.radians(240)
}
# Some tunable parameters
self.update_time = .5 # s
self.saturation_reject = 50
self.value_reject = 50
self.hue_error_reject = .4 # rads
# To decide when to accept color observations
self.error_tolerance = .4 # rads
self.spottings_req = 4
self.similarity_reject = .1 # If two colors are within this amount of error, reject
rospy.Timer(ros_t(self.update_time), self.do_update)
def main(name, lla):
nh = yield txros.NodeHandle.from_argv("esitmated_object_setter")
db = yield nh.get_service_client("/database/requests", ObjectDBQuery)
convert = Converter()
yield convert.init(nh)
# Convert the name to Be_Like_This
name = '_'.join(map(lambda txt: txt.title(), name.split("_")))
point = yield convert.request(CoordinateConversionRequest(frame="lla", point=lla))
yield db(ObjectDBQueryRequest(cmd="{}={p[0]}, {p[1]}".format(name, p=point.enu)))
def get_closest_object(self, objects):
"""Get the closest mission."""
pobjs = []
for obj in objects:
req = ObjectDBQueryRequest()
req.name = obj.name
resp = yield self._database(req)
if len(resp.objects) != 0:
pobjs.extend(resp.objects)
if len(pobjs) == 0:
raise MissingPerceptionObject("All")
min_dist = sys.maxsize
min_obj = None
for o in pobjs:
dist = yield self._dist(o)
if dist < min_dist:
min_dist = dist
min_obj = o
defer.returnValue(min_obj)
def get_object_rois(self, name=None):
req = ObjectDBQueryRequest()
if name is None:
req.name = 'all'
else:
req.name = name
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")
o = obj.objects[0]
points_3d = yield self.get_3d_points(o)
points_2d_all = map(lambda x: self.camera_model.project3dToPixel(x),
points_3d)
points_2d = self._get_2d_points(points_3d)
xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d, img)
xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax)
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
roi = img[ymin:ymax, xmin:xmax]
roi = self._resize_image(roi)
ret_obj = {}
ret_obj["id"] = o.id
ret_obj["points"] = points_2d_all
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_object_rois(self, name=None):
req = ObjectDBQueryRequest()
if name is None:
req.name = 'all'
else:
req.name = name
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")
o = obj.objects[0]
points_3d = yield self.get_3d_points(o)
points_2d_all = map(
lambda x: self.camera_model.project3dToPixel(x), points_3d)
points_2d = self._get_2d_points(points_3d)
xmin, ymin, xmax, ymax = self._get_bounding_rect(points_2d, img)
xmin, ymin, xmax, ymax = int(xmin), int(ymin), int(xmax), int(ymax)
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:
defer.returnValue((None, None))
if ymin < 0:
ymin = 0
roi = img[ymin:ymax, xmin:xmax]
roi = self._resize_image(roi)
ret_obj = {}
ret_obj["id"] = o.id
ret_obj["points"] = points_2d_all
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_closest_object(self, objects):
"""Get the closest mission."""
pobjs = []
for obj in objects:
req = ObjectDBQueryRequest()
req.name = obj.name
resp = yield self._database(req)
if len(resp.objects) != 0:
pobjs.extend(resp.objects)
if len(pobjs) == 0:
raise MissingPerceptionObject("All")
min_dist = sys.maxint
min_obj = None
for o in pobjs:
dist = yield self._dist(o)
if dist < min_dist:
min_dist = dist
min_obj = o
defer.returnValue(min_obj)
def begin_observing(self, cb):
"""
Get notified when a new object is added to the database.
cb : The callback that is called when a new object is added to the database
"""
self.new_object_subscriber = cb
req = ObjectDBQueryRequest()
req.name = 'all'
resp = yield self._database(req)
req.name = 'All'
resp1 = yield self._database(req)
for o in resp.objects:
# The is upper call is because the first case is upper case if it is a 'fake' object... WHYYYYY
if o.name not in self.found:
self.found.add(o.name)
self.new_object_subscriber(o)
for o in resp1.objects:
# The is upper call is because the first case is upper case if it is a 'fake' object... WHYYYYY
if o.name not in self.found:
self.found.add(o.name)
self.new_object_subscriber(o)
def get_object(self,
object_name,
volume_only=False,
thresh=50,
thresh_strict=50):
"""Get an object from the database."""
if volume_only:
req = ObjectDBQueryRequest()
req.name = object_name
resp = yield self._database(req)
if not resp.found:
raise MissingPerceptionObject(object_name)
defer.returnValue(resp.objects)
else:
req = ObjectDBQueryRequest()
req.name = "all"
resp = yield self._database(req)
closest_potential_object = None
min_dist = sys.maxsize
actual_objects = []
for o in resp.objects:
distance = yield self._dist(o)
if o.name == object_name and distance < thresh_strict:
actual_objects.append(o)
if distance < thresh and distance < min_dist:
min_dist = distance
closest_potential_object = o
# print [x.name for x in actual_objects]
# print closest_potential_object.name
# sys.exit()
if len(actual_objects) == 0 and min_dist == sys.maxsize:
raise MissingPerceptionObject(object_name)
if len(actual_objects) > 1:
min_dist = sys.maxsize
min_obj = None
for o in actual_objects:
dist = yield self._dist(o)
if dist < min_dist:
min_dist = dist
min_obj = o
defer.returnValue(min_obj)
if len(actual_objects) == 1:
defer.returnValue(actual_objects[0])
defer.returnValue(closest_potential_object)
def _get_scan_the_code(self):
req = ObjectDBQueryRequest()
req.name = 'scan_the_code'
scan_the_code = yield self.database(req)
defer.returnValue(scan_the_code.objects[0])
def __init__(self):
info_topic = camera_root + "/camera_info"
image_topic = camera_root + "/image_rect_color"
self.tf_listener = tf.TransformListener()
self.status_pub = rospy.Publisher("/database_color_status",
ColoramaDebug,
queue_size=1)
self.odom = None
set_odom = lambda msg: setattr(
self, "odom", navigator_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber("/odom", Odometry, set_odom)
fprint("Waiting for odom...")
while self.odom is None and not rospy.is_shutdown():
rospy.sleep(1)
fprint("Odom found!", msg_color='green')
db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
self.make_request = lambda **kwargs: db_request(
ObjectDBQueryRequest(**kwargs))
self.image_history = ImageHistory(image_topic)
# Wait for camera info, and exit if not found
fprint("Waiting for camera info on: '{}'".format(info_topic))
while not rospy.is_shutdown():
try:
camera_info_msg = rospy.wait_for_message(info_topic,
CameraInfo,
timeout=3)
except rospy.exceptions.ROSException:
rospy.sleep(1)
continue
break
fprint("Camera info found!", msg_color="green")
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info_msg)
self.debug = DebugImage("/colorama/debug", prd=.5)
# These are color mappings from Hue values [0, 360]
self.color_map = {
'red': np.radians(0),
'yellow': np.radians(60),
'green': np.radians(120),
'blue': np.radians(200)
}
# RGB map for database setting
self.database_color_map = {
'red': (255, 0, 0),
'yellow': (255, 255, 0),
'green': (0, 255, 0),
'blue': (0, 0, 255)
}
# ========= Some tunable parameters ===================================
self.update_time = 1 # s
# Observation parameters
self.saturation_reject = 20 # Reject color obs with below this saturation
self.value_reject = 50 # Reject color obs with below this value
self.height_remove = 0.4 # Remove points that are this percent down on the object (%)
# 1 keeps all, .4 removes the bottom 40%
# Update parameters
self.history_length = 100 # How many of each color to keep
self.min_obs = 5 # Need atleast this many observations before making a determination
self.conf_reject = .5 # When to reject an observation based on it's confidence
# Confidence weights
self.v_factor = 0.6 # Favor value values close to the nominal value
self.v_u = 200 # Mean of norm for variance error
self.v_sig = 60 # Sigma of norm for variance error
self.dist_factor = 0.3 # Favor being closer to the totem
self.dist_sig = 30 # Sigma of distance (m)
self.q_factor = 0 # Favor not looking into the sun
self.q_sig = 1.2 # Sigma of norm for quaternion error (rads)
# Maps id => observations
self.colored = {}
rospy.Timer(ros_t(self.update_time), self.do_observe)