class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.camera_model.fromCameraInfo(SubscriberValue('camera_info', CameraInfo).value)
self.depth_image = SubscriberValue('camera_depth_image', Image, transform=self.bridge.imgmsg_to_cv2)
self.service = rospy.Service('cam_pixel_to_point', CamPixelToPoint, self.handle_service)
self.tf_listener = TransformListener()
print('Service is ready.')
def handle_service(self, req): # type: (CamPixelToPoint) -> CamPixelToPointResponse
x, y = int(req.cam_pixel.x), int(req.cam_pixel.y)
methods = [self.read_depth_simple,
# self.read_depth_average,
self.read_depth_as_floor_depth]
for method in methods:
d = method(x, y)
if not np.isnan(d):
break
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
return CamPixelToPointResponse(point)
def read_depth_simple(self, x, y): # (int, int) -> float
return self.depth_image.value[y, x]
def read_depth_average(self, x, y): # (int, int) -> float
print('Fallback to average')
s = 5
return np.nanmean(self.depth_image.value[y-s:y+s, x-s:x+s])
def read_depth_as_floor_depth(self, x, y): # (int, int) -> float
print('Fallback to floor model')
min_distance = 10.0
# Extend the camera ray until it passes through where the floor should be. Use its length as the depth.
camera_origin = PointStamped()
camera_origin.header.frame_id = self.camera_model.tfFrame()
camera_origin.point.x, camera_origin.point.y, camera_origin.point.z = 0.0, 0.0, 0.0
point_along_ray = PointStamped()
point_along_ray.header.frame_id = self.camera_model.tfFrame()
point_along_ray.point.x, point_along_ray.point.y, point_along_ray.point.z = self.camera_model.projectPixelTo3dRay((x, y))
self.tf_listener.waitForTransform('base_footprint', self.camera_model.tfFrame(), rospy.Time(rospy.get_time()), rospy.Duration(1))
camera_origin = self.tf_listener.transformPoint('base_footprint', camera_origin)
point_along_ray = self.tf_listener.transformPoint('base_footprint', point_along_ray)
camera_origin = np_from_poinstamped(camera_origin)
point_along_ray = np_from_poinstamped(point_along_ray)
ray_dir = point_along_ray - camera_origin
# Assuming this transformation was orthogonal, |ray_dir| = 1, at least approximately
d = camera_origin[1]/max(-ray_dir[1], camera_origin[1]/min_distance)
if d <= 0.01:
d = np.nan
return d
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.model_set = False
self.tf_listener = TransformListener()
def pixel_to_point(self,
cam_pos,
out_frame='map'): # type: (np.ndarray) -> PointStamped
if not self.model_set:
self.camera_model.fromCameraInfo(
rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo))
self.model_set = True
x, y = int(cam_pos[0]), int(cam_pos[1])
d = self.read_depth_simple(x, y)
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
point = convert_frame(self.tf_listener, point, out_frame)
return point
def read_depth_simple(self, x, y): # (int, int) -> float
image = rospy.wait_for_message('/camera/depth_registered/image_raw',
Image)
image = self.bridge.imgmsg_to_cv2(image)
return image[y, x]
def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print 'cam_model = ', np.array(cam_model.intrinsicMatrix())
print 'cam tf = ', cam_model.tfFrame()
return cam_model
def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print "cam_model = ", np.array(cam_model.intrinsicMatrix())
print "cam tf = ", cam_model.tfFrame()
return cam_model
class OrangeRectangleFinder():
"""
Node which finds orange rectangular objects in image frame.
This can be used for the path marker challenge and to detect
the lid of the bins challenge. The node estimates the 2d and 3d
position/orientation of this object and returns this estimate when service is called.
Unit tests for this node is in test_path_marker.py
Finding the marker works as follows:
* blur image
* threshold image mostly for highly saturated, orange/yellow/red objects
* run canny edge detection on thresholded image
* find contours on edge frame
* filter contours to find those that may be contours by:
* checking # of sides in appox polygon
* checking ratio of length/width close to known model
* estimates 3D pose using cv2.solvePnP with known object demensions and camera model
* Use translation vector from PnP and direction vector from 2d contour for pose
* Transform this frames pose into /map frame
* Plug this frames pose in /map into a kalman filter to reduce noise
TODO: Allow for two objects to be identifed at once, both filtered through its own KF
"""
# Coordinate axes for debugging image
REFERENCE_POINTS = np.array([[0, 0, 0],
[0.3, 0, 0],
[0, 0.3, 0],
[0, 0, 0.3]], dtype=np.float)
def __init__(self):
self.debug_gui = False
self.enabled = False
self.cam = None
# Constants from launch config file
self.debug_ros = rospy.get_param("~debug_ros", True)
self.canny_low = rospy.get_param("~canny_low", 100)
self.canny_ratio = rospy.get_param("~canny_ratio", 3.0)
self.thresh_hue_high = rospy.get_param("~thresh_hue_high", 60)
self.thresh_saturation_low = rospy.get_param("~thresh_satuation_low", 100)
self.min_contour_area = rospy.get_param("~min_contour_area", 100)
self.epsilon_range = rospy.get_param("~epsilon_range", (0.01, 0.1))
self.epsilon_step = rospy.get_param("~epsilon_step", 0.01)
self.shape_match_thresh = rospy.get_param("~shape_match_thresh", 0.4)
self.min_found_count = rospy.get_param("~min_found_count", 10)
self.timeout_seconds = rospy.get_param("~timeout_seconds", 2.0)
# Default to scale model of path marker. Please use set_geometry service
# to set to correct model of object.
length = rospy.get_param("~length", 1.2192)
width = rospy.get_param("~width", 0.1524)
self.rect_model = RectFinder(length, width)
self.do_3D = rospy.get_param("~do_3D", True)
camera = rospy.get_param("~image_topic", "/camera/down/left/image_rect_color")
self.tf_listener = tf.TransformListener()
# Create kalman filter to track 3d position and direction vector for marker in /map frame
self.state_size = 5 # X, Y, Z, DY, DX
self.filter = cv2.KalmanFilter(self.state_size, self.state_size)
self.filter.transitionMatrix = 1. * np.eye(self.state_size, dtype=np.float32)
self.filter.measurementMatrix = 1. * np.eye(self.state_size, dtype=np.float32)
self.filter.processNoiseCov = 1e-5 * np.eye(self.state_size, dtype=np.float32)
self.filter.measurementNoiseCov = 1e-4 * np.eye(self.state_size, dtype=np.float32)
self.filter.errorCovPost = 1. * np.eye(self.state_size, dtype=np.float32)
self.reset()
self.service_set_geometry = rospy.Service('~set_geometry', SetGeometry, self._set_geometry_cb)
if self.debug_ros:
self.debug_pub = Image_Publisher("~debug_image")
self.markerPub = rospy.Publisher('~marker', Marker, queue_size=10)
self.service2D = rospy.Service('~2D', VisionRequest2D, self._vision_cb_2D)
if self.do_3D:
self.service3D = rospy.Service('~pose', VisionRequest, self._vision_cb_3D)
self.toggle = rospy.Service('~enable', SetBool, self._enable_cb)
self.image_sub = Image_Subscriber(camera, self._img_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
assert self.camera_info is not None
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(self.camera_info)
def _set_geometry_cb(self, req):
self.rect_model = RectFinder.from_polygon(req.model)
self.reset()
rospy.loginfo("Resetting rectangle model to LENGTH=%f, WIDTH=%f", self.rect_model.length, self.rect_model.width)
return {'success': True}
def _send_debug_marker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the object.
This marker is a scaled cube with the demensions of the model.
Only called if debug_ros param == True
'''
if self.last3d is None or not self.found:
return
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = self.last_found_time_3D
m.ns = "orange_rectangle"
m.id = 0
m.type = 1
m.action = 0
# Real demensions of path marker
m.scale.x = self.rect_model.length
m.scale.y = self.rect_model.width
m.scale.z = 0.05
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(self.timeout_seconds)
self.markerPub.publish(m)
def _enable_cb(self, x):
if x.data != self.enabled:
self.reset()
self.tf_listener.clear()
self.enabled = x.data
return SetBoolResponse(success=True)
def _vision_cb_3D(self, req):
res = VisionRequestResponse()
if self.last_found_time_3D is None or self.image_sub.last_image_time is None:
res.found = False
return res
dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec()
if dt < 0 or dt > self.timeout_seconds:
res.found = False
elif (self.last3d is None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = "/map"
res.pose.header.stamp = self.last_found_time_3D
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def _vision_cb_2D(self, req):
res = VisionRequest2DResponse()
if (self.last2d is None or not self.enabled):
res.found = False
else:
res.header.frame_id = self.cam.tfFrame()
res.header.stamp = self.last_found_time_2D
res.pose.x = self.last2d[0][0]
res.pose.y = self.last2d[0][1]
res.camera_info = self.camera_info
res.max_x = self.camera_info.width
res.max_y = self.camera_info.height
if self.last2d[1][0] < 0:
self.last2d[1][0] = -self.last2d[1][0]
self.last2d[1][1] = -self.last2d[1][1]
angle = np.arctan2(self.last2d[1][1], self.last2d[1][0])
res.pose.theta = angle
res.found = True
return res
def reset(self):
self.last_found_time_2D = None
self.last_found_time_3D = None
self.last2d = None
self.last3d = None
self._clear_filter(None)
def _clear_filter(self, state):
'''
Reset filter and found state. This will ensure that the object
is seen consistently before vision request returns true
'''
rospy.loginfo("MARKER LOST")
self.found_count = 0
self.found = False
self.last3d = None
self.filter.errorCovPre = 1. * np.eye(self.state_size, dtype=np.float32)
if state is not None:
self.found_count = 1
state = np.array(state, dtype=np.float32)
self.filter.statePost = state
def _update_kf(self, (x, y, z, dy, dx)):
'''
Updates the kalman filter using the pose estimation
from the most recent frame. Also tracks time since last seen and how
often is has been seen to set the boolean "found" for the vision request
'''
if self.last_found_time_3D is None: # First time found, set initial KF pose to this frame
self._clear_filter((x, y, z, dy, dx))
self.last_found_time_3D = self.image_sub.last_image_time
return
dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec()
self.last_found_time_3D = self.image_sub.last_image_time
if dt < 0 or dt > self.timeout_seconds:
rospy.logwarn("Timed out since last saw marker, resetting. DT={}".format(dt))
self._clear_filter((x, y, z, dy, dx))
return
self.found_count += 1
measurement = 1. * np.array([x, y, z, dy, dx], dtype=np.float32)
self.filter.predict()
estimated = self.filter.correct(measurement)
if self.found_count > self.min_found_count:
angle = np.arctan2(estimated[3], estimated[4])
self.last3d = ((estimated[0], estimated[1], estimated[2]),
quaternion_from_euler(0.0, 0.0, angle))
if not self.found:
rospy.loginfo("Marker Found")
self.found = True
class Colorama(object):
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)
def _compute_average_angle(self, angles, weights):
"""
Returns weighted average of angles.
Tends to break down with too many angles 180 degrees of each other - try not to do that.
"""
angles = np.array(angles)
if np.linalg.norm(weights) == 0:
return None
w = weights / np.linalg.norm(weights)
s = np.sum(w * np.sin(angles))
c = np.sum(w * np.cos(angles))
avg_angle = np.arctan2(s, c)
return avg_angle
def _get_quaternion_error(self, q, target_q):
"""
Returns an angluar differnce between q and target_q in radians
"""
dq = trns.quaternion_multiply(np.array(target_q), trns.quaternion_inverse(np.array(q)))
return 2 * np.arccos(dq[3])
def _get_closest_color(self, hue_angle):
"""
Returns a pair of the most likely color and the radian error associated with that prediction
`hue_angle` := The radian value associated with hue [0, 2*pi]
Colors are found from `self.color_map`
"""
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'undef'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
# We need a signed error for some math later on so no absolute value
this_error = np.arctan2(sg*c - cg*s, cg*c + sg*s)
if np.abs(this_error) < np.abs(error):
error = this_error
likely_color = color
fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
return [likely_color, error]
def do_estimate(self, totem_id):
"""Calculates best color estimate for a given totem id"""
fprint("DOING ESTIMATE", msg_color='blue')
if totem_id not in self.colored:
fprint("Totem ID {} not found!".format(totem_id), msg_color='red')
return None
t_color = self.colored[totem_id]
if len(t_color) < self.min_obs:
fprint("Only {} observations. {} required.".format(len(t_color), self.min_obs), msg_color='red')
return None
kwargs = {'v_u': self.v_u, 'v_sig': self.v_sig, 'dist_sig': self.dist_sig,
'q_factor': self.q_factor, 'q_sig': self.q_sig}
w, weights = t_color.compute_confidence([self.v_factor, self.dist_factor, self.q_factor], True, **kwargs)
fprint("CONF: {}".format(w))
if np.mean(w) < self.conf_reject:
return None
hue_angles = np.radians(np.array(t_color.hues) * 2)
angle = self._compute_average_angle(hue_angles, w)
color = self._get_closest_color(angle)
msg = t_color.as_message
msg.id = totem_id
msg.confidence = w
msg.labels = ["value_errs", "dists", "q_diffs"]
msg.weights = weights
msg.color = colors[0]
msg.est_hues = angle * 2
msg.hues = np.array(t_color.hues) * 2
self.status_pub.publish(msg)
fprint("Color: {}".format(color[0]))
return color
def got_request(self, req):
# Threading blah blah something unsafe
colored_ids = [_id for _id, color_err in self.colored.iteritems() if self.valid_color(_id) == req.color]
fprint("Colored IDs: {}".format(colored_ids), msg_color='blue')
print '\n' * 50
if len(colored_ids) == 0:
return ColorRequestResponse(found=False)
return ColorRequestResponse(found=True, ids=colored_ids)
def do_observe(self, *args):
resp = self.make_request(name='totem')
# If atleast one totem was found start observing
if resp.found:
# Time of the databse request
time_of_marker = resp.objects[0].header.stamp# - ros_t(1)
fprint("Looking for image at {}".format(time_of_marker.to_sec()), msg_color='yellow')
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
t = self.image_history.newest_image.time
if t is None:
fprint("No images found.")
return
fprint("No valid image found for t={} ({}) dt: {}".format(time_of_marker.to_sec(), t.to_sec(), (rospy.Time.now() - t).to_sec()), msg_color='red')
return
header = navigator_tools.make_header(frame='/enu', stamp=image_holder.time)
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
try:
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)
except tf.ExtrapolationException as e:
fprint("TF error found and excepted: {}".format(e))
return
for obj in resp.objects:
if len(obj.points) <= 1:
fprint("No points in object")
continue
fprint("{} {}".format(obj.id, "=" * 50))
# Get object position in px coordinates to determine if it's in frame
object_cam = t_mat44.dot(np.append(p2np(obj.position), 1))
object_px = map(int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
fprint("Object not in frame")
continue
# Get enu points associated with this totem and remove ones that are too low
points_np = np.array(map(p2np, obj.points))
height = np.max(points_np[:, 2]) - np.min(points_np[:, 2])
if height < .1:
# If the height of the object is too small, skip (units are meters)
fprint("Object too small")
continue
threshold = np.min(points_np[:, 2]) + self.height_remove * height
points_np = points_np[points_np[:, 2] > threshold]
# Shove ones in there to make homogenous points to get points in image frame
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])
[cv2.circle(self.debug.image, tuple(map(int, p)), 2, (255, 255, 255), -1) for p in points_px]
# Get color information from the points
roi = self._get_ROI_from_points(points_px)
h, s, v = self._get_hsv_from_ROI(roi, image)
# Compute parameters that go into the confidence
boat_q = self.odom[1]
target_q = self._get_solar_angle()
q_err = self._get_quaternion_error(boat_q, target_q)
dist = np.linalg.norm(self.odom[0] - p2np(obj.position))
fprint("H: {}, S: {}, V: {}".format(h, s, v))
fprint("q_err: {}, dist: {}".format(q_err, dist))
# Add to database and setup debug image
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')
else:
if obj.id not in self.colored:
self.colored[obj.id] = Observation()
# Add this observation in
self.colored[obj.id] += h, v, dist, q_err
print self.colored[obj.id]
rgb = (0, 0, 0)
color = self.do_estimate(obj.id)
# Do we have a valid color estimate
if color:
fprint("COLOR IS VALID", msg_color='green')
rgb = self.database_color_map[color[0]]
cmd = '{name}={rgb[0]},{rgb[1]},{rgb[2]},{_id}'
self.make_request(cmd=cmd.format(name=obj.name,_id=obj.id, rgb=rgb))
bgr = rgb[::-1]
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)
def _get_solar_angle(self):
return [0, 0, 0, 1]
def _get_ROI_from_points(self, image_points):
cnt = np.array([image_points]).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_hsv_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
return h, s, v
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v, self.value_reject), msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
# Display the current values
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image, "h_val: {}".format(np.degrees(hue_angle)),
tuple(roi[1]), font, 1, (255, 255, 255), 2)
likely_color, error = self.get_closest_color(hue_angle)
if error > self.hue_error_reject:
fprint("Closest color was {} with an error of {} rads (> {}). Rejecting.".format(likely_color, np.round(error, 3),
self.hue_error_reject), msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
return [likely_color, error]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
return not (np.any([0, 0] > px) or np.any(px > resoultion))
class CameraHelper:
"""
A helper class to take pictures with the Sawyer camera and unproject points from the images
"""
def __init__(self, camera_name, base_frame, table_height):
"""
Initialize the instance
:param camera_name: The camera name. One of (head_camera, right_hand_camera)
:param base_frame: The frame for the robot base
:param table_height: The table height with respect to base_frame
"""
self.camera_name = camera_name
self.base_frame = base_frame
self.table_height = table_height
self.image_queue = Queue.Queue()
self.pinhole_camera_model = PinholeCameraModel()
self.tf_listener = tf.TransformListener()
camera_info_topic = "/io/internal_camera/{}/camera_info".format(
camera_name)
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
self.pinhole_camera_model.fromCameraInfo(camera_info)
cameras = intera_interface.Cameras()
cameras.set_callback(camera_name,
self.__show_image_callback,
rectify_image=True)
def __show_image_callback(self, img_data):
"""
Image callback for the camera
:param img_data: The image received from the camera
"""
if self.image_queue.empty():
self.image_queue.put(img_data)
def take_single_picture(self):
"""
Take one picture from the specified camera
:returns: The image
"""
with self.image_queue.mutex:
self.image_queue.queue.clear()
cameras = intera_interface.Cameras()
cameras.start_streaming(self.camera_name)
image_data = self.image_queue.get(block=True, timeout=None)
cameras.stop_streaming(self.camera_name)
return image_data
def project_point_on_table(self, point):
"""
Projects the 2D point from the camera image on the table
:param point: The 2D point in the form (x, y)
:return: The 3D point in the coordinate space of the frame that was specified when the object was initialized
"""
# Get camera frame name
camera_frame = self.pinhole_camera_model.tfFrame()
# Check that both frames exist
if self.tf_listener.frameExists(
self.base_frame) and self.tf_listener.frameExists(
camera_frame):
# Get transformation
time = self.tf_listener.getLatestCommonTime(
self.base_frame, camera_frame)
camera_translation_base, camera_orientation_base = self.tf_listener.lookupTransform(
self.base_frame, camera_frame, time)
# Unproject point
unprojected_ray_camera = self.pinhole_camera_model.projectPixelTo3dRay(
point)
# Rotate ray based on the frame transformation
camera_frame_rotation_matrix = tf.transformations.quaternion_matrix(
camera_orientation_base)
unprojected_ray_base = numpy.dot(
camera_frame_rotation_matrix[:3, :3], unprojected_ray_camera)
# Intersect ray with base plane
point_height = camera_translation_base[2] - self.table_height
factor = -point_height / unprojected_ray_base[2]
intersection = camera_translation_base + unprojected_ray_base * factor
return intersection
class ros_cv_testing_node:
def __init__(self):
# Get information for this particular camera
camera_info = get_single('head_camera/depth_registered/camera_info',
CameraInfo)
print camera_info
# Set information for the camera
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Subscribe to the camera's image topic and depth image topic
self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw",
Image, self.on_rgb)
self.depth_image_sub = rospy.Subscriber(
"head_camera/depth_registered/image_raw", Image, self.on_depth)
# Publish where the button is in the base link frame
self.point_pub = rospy.Publisher('button_point',
PointStamped,
queue_size=10)
# Set up connection to CvBridge
self.bridge = CvBridge()
# Open up viewing windows
cv2.namedWindow('depth')
cv2.namedWindow('rgb')
# Set up the class variables
self.rgb_image = None
self.rgb_image_time = None
self.depth_image = None
self.center = None
self.return_point = PointStamped()
self.tf_listener = TransformListener()
def on_rgb(self, image):
# Convert image to something that cv2 can work with
try:
self.rgb_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
except e:
print e
return
self.rgb_image_time = image.header.stamp
# Get height and width of image
h = self.rgb_image.shape[0]
w = self.rgb_image.shape[1]
# Create empty image
color_dst = numpy.empty((h, w), 'uint8')
# Convert picture to grayscale
cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2GRAY, color_dst)
# Find circles given the camera image
dp = 1.1
minDist = 90
circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp,
minDist)
# If no circles are detected then exit the program
if circles == None:
print "No circles found using these parameters"
sys.exit()
circles = numpy.uint16(numpy.around(circles))
# Draw the center of the circle closest to the top
ycoord = []
for i in circles[0, :]:
ycoord.append(i[1])
# Find the top circle in the frame
top_circ_y_coor = min(ycoord)
x_coor = 0
y_coor = 0
for i in circles[0, :]:
if i[1] == top_circ_y_coor:
# draw the center of the circle
#cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3)
# draw outer circle
#cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2)
x_coor = i[0]
y_coor = i[1]
cv2.circle(self.rgb_image, (327, 247), 2, (0, 0, 255), 3)
# Set the coordinates for the center of the circle
self.center = (x_coor, y_coor)
#self.center = (328,248)
cv2.imshow('rgb', self.rgb_image)
cv2.waitKey(1)
def on_depth(self, image):
# Use numpy so that cv2 can use image
self.depth_image = numpy.fromstring(image.data,
dtype='float32').reshape(
(480, 640))
nmask = numpy.isnan(self.depth_image)
#Find the minimum and the maximum of the depth image
Dmin = self.depth_image[~nmask].min()
Dmax = self.depth_image[~nmask].max()
# If a circle has been found find its depth and apply that to the ray
if self.center != None:
ray = self.cam_model.projectPixelTo3dRay(self.center)
depth = self.depth_image[self.center[1]][self.center[0]]
# If the depth is not a number exit
if math.isnan(depth):
print "Incomplete information on depth at this point"
return
# Otherwise mulitply the depth by the unit vector of the ray
else:
print "depth", depth
cam_coor = [depth * ray[0], depth * ray[1], depth * ray[2]]
#print "camera frame coordinate:", cam_coor
else:
return
# Rescale to [0,1]
cv2.imshow('depth', self.depth_image / 2.0)
cv2.waitKey(1)
# Only use depth if the RGB image is running
if self.rgb_image_time is None:
rospy.loginfo('not using depth cause no RGB yet')
return
time_since_rgb = image.header.stamp - self.rgb_image_time
# Only use depth if it is close in time to the RGB image
if time_since_rgb > rospy.Duration(0.5):
rospy.loginfo('not using depth because time since RGB is ' +
str(time_since_rgb))
#return
# Find transform from camera frame to world frame
self.tf_listener.waitForTransform(self.cam_model.tfFrame(),
"base_link", image.header.stamp,
rospy.Duration(1.0))
# Set up the point to be published
self.return_point.header.stamp = image.header.stamp
self.return_point.header.frame_id = self.cam_model.tfFrame()
self.return_point.point.x = cam_coor[0]
self.return_point.point.y = cam_coor[1]
self.return_point.point.z = cam_coor[2]
# Transform point to the baselink frame
self.return_point = self.tf_listener.transformPoint(
"base_link", self.return_point)
print "world frame coordinate:", self.return_point.point.x, \
self.return_point.point.y,self.return_point.point.z, "\n"
self.point_pub.publish(self.return_point)
class CLAHE_generator:
def __init__(self):
self.camera = rospy.get_param('~camera_topic',
'/camera/front/left/image_rect_color')
# Instantiate remaining variables and objects
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
# Image Subscriber and Camera Information
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
'''
These variables store the camera information required to perform
the transformations on the coordinates to move from the subs
perspective to our global map perspective. This information is
also necessary to perform the least squares intersection which
will find the 3D coordinates of the torpedo board based on 2D
observations from the Camera. More info on this can be found in
sub8_vision_tools.
'''
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
self.image_pub = Image_Publisher("CLAHE/debug")
# Debug
self.debug = rospy.get_param('~debug', True)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find the torpedo board.
'''
self.last_image = image
if self.last_image_time is not None and \
self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in
# loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.CLAHE(image)
print('published')
def CLAHE(self, cv_image):
'''
CLAHE (Contrast Limited Adaptive Histogram Equalization)
This increases the contrast between color channels and allows us to
better differentiate colors under certain lighting conditions.
'''
clahe = cv2.createCLAHE(clipLimit=9.5, tileGridSize=(4, 4))
# convert from BGR to LAB color space
lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB)
l, a, b = cv2.split(lab) # split on 3 different channels
l2 = clahe.apply(l) # apply CLAHE to the L-channel
lab = cv2.merge((l2, a, b)) # merge channels
cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
self.image_pub.publish(cv_image)
class Colorama(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 valid_color(self, _id):
"""
Either returns valid color or None depending if the color is valid or not
"""
if _id not in self.colored:
return None
object_data = self.colored[_id]
print "object_data", object_data
# Maps color => [err1, err2, ..]
potential_colors = {}
for data in object_data:
color, err = data['color'], data['err']
if color not in potential_colors:
potential_colors[color] = []
potential_colors[color].append(err)
potential_colors_avg = {}
for color, errs in potential_colors.iteritems():
if len(errs) > self.spottings_req:
potential_colors_avg[color] = np.average(errs)
print "potential_colors_avg", potential_colors_avg
if len(potential_colors_avg) == 1:
# No debate about the color
color = potential_colors_avg.keys()[0]
err = potential_colors_avg[color]
fprint("Only one color found, error: {} (/ {})".format(
err, self.error_tolerance))
if len(potential_colors[color]
) > self.spottings_req and err <= self.error_tolerance:
return color
elif len(potential_colors_avg) > 1:
# More than one color, see if one of them is still valid
fprint("More than one color detected. Removing all.")
self.colored[_id] = []
return None
def got_request(self, req):
# Threading blah blah something unsafe
color = self.valid_color(req.target_id)
if color is None:
fprint("ID {} is NOT a valid color".format(req.target_id),
msg_color='red')
return VisionRequestResponse(found=False)
return VisionRequestResponse(found=True, color=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 _get_ROI_from_points(self, image_points):
# Probably will return a set of contours
cnt = np.array(image_points).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_color_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
fprint("The colors aren't expressive enough. Rejecting.",
msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'bazinga'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
this_error = np.abs(np.arctan2(sg * c - cg * s, cg * c + sg * s))
if this_error < error:
error = this_error
likely_color = color
if error > self.hue_error_reject:
fprint(
"Closest color was {} with an error of {} rads (> {}) Rejecting."
.format(likely_color, np.round(error, 3),
self.hue_error_reject),
msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(
likely_color, np.round(error, 3)))
return [
np.degrees(self.color_map[likely_color]) / 2.0, likely_color, error
]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
print object_point
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
if np.any([0, 0] > px) or np.any(px > resoultion):
return False
return True
class VampireIdentifier:
def __init__(self):
# Pull constants from config file
self.override = False
self.lower = [0, 0, 0]
self.upper = [0, 0, 0]
self.min_trans = 0
self.max_velocity = 0
self.timeout = 0
self.min_observations = 0
self.camera = rospy.get_param('~camera_topic',
'/camera/down/image_rect_color')
self.goal = None
self.last_config = None
self.reconfigure_server = DynamicReconfigureServer(
VampireIdentifierConfig, self.reconfigure)
# Instantiate remaining variables and objects
self._observations = deque()
self._pose_pairs = deque()
self._times = deque()
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
# Image Subscriber and Camera Information
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
# Ros Services so mission can be toggled and info requested
rospy.Service('~enable', SetBool, self.toggle_search)
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~pose', VisionRequest, self.request_buoy)
self.image_pub = Image_Publisher("drac_vision/debug")
self.point_pub = rospy.Publisher("drac_vision/points",
Point,
queue_size=1)
self.mask_image_pub = rospy.Publisher('drac_vision/mask',
Image,
queue_size=1)
# Debug
self.debug = rospy.get_param('~debug', True)
@staticmethod
def parse_string(threshes):
ret = [float(thresh.strip()) for thresh in threshes.split(',')]
if len(ret) != 3:
raise ValueError('not 3')
return ret
def reconfigure(self, config, level):
try:
self.override = config['override']
self.goal = config['target']
self.lower = self.parse_string(config['dyn_lower'])
self.upper = self.parse_string(config['dyn_upper'])
self.min_trans = config['min_trans']
self.max_velocity = config['max_velocity']
self.timeout = config['timeout']
self.min_observations = config['min_obs']
except ValueError as e:
rospy.logwarn('Invalid dynamic reconfigure: {}'.format(e))
return self.last_config
if self.override:
# Dynamic Values for testing
self.lower = np.array(self.lower)
self.upper = np.array(self.upper)
else:
# Hard Set for use in Competition
if self.goal == 'drac':
self.lower = rospy.get_param('~dracula_low_thresh', [0, 0, 80])
self.upper = rospy.get_param('~dracula_high_thresh',
[0, 0, 80])
else:
raise ValueError('Invalid Target Name')
self.last_config = config
rospy.loginfo('Params succesfully updated via dynamic reconfigure')
return config
def image_cb(self, image):
'''
Run each time an image comes in from ROS.
'''
if not self.enabled:
return
self.last_image = image
if self.last_image_time is not None and \
self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in
# loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.acquire_targets(image)
def toggle_search(self, srv):
'''
Callback for standard ~enable service. If true, start
looking at frames for buoys.
'''
if srv.data:
rospy.loginfo("TARGET ACQUISITION: enabled")
self.enabled = True
else:
rospy.loginfo("TARGET ACQUISITION: disabled")
self.enabled = False
return SetBoolResponse(success=True)
def request_buoy(self, srv):
'''
Callback for 3D vision request. Uses recent observations of target
board specified in target_name to attempt a least-squares position
estimate. Ignoring orientation of board.
'''
if not self.enabled:
return VisionRequestResponse(found=False)
# buoy = self.buoys[srv.target_name]
if self.est is None:
return VisionRequestResponse(found=False)
return VisionRequestResponse(pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(position=Point(*self.est))),
found=True)
def clear_old_observations(self):
'''
Observations older than two seconds are discarded.
'''
time = rospy.Time.now()
i = 0
while i < len(self._times):
if time - self._times[i] > self.timeout:
self._times.popleft()
self._observations.popleft()
self._pose_pairs.popleft()
else:
i += 1
# print('Clearing')
def add_observation(self, obs, pose_pair, time):
'''
Add a new observation associated with an object
'''
self.clear_old_observations()
# print('Adding...')
if slen(self._observations) == 0 or np.linalg.norm(
self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans:
self._observations.append(obs)
self._pose_pairs.append(pose_pair)
self._times.append(time)
def get_observations_and_pose_pairs(self):
'''
Fetch all recent observations + clear old ones
'''
self.clear_old_observations()
return (self._observations, self._pose_pairs)
def detect(self, c):
'''
Verify the shape in the masked image is large enough to be a valid target.
This changes depending on target Vampire, as does the number of targets we want.
'''
target = "unidentified"
peri = cv2.arcLength(c, True)
if peri < self.min_contour_area or peri > self.max_contour_area:
return target
approx = cv2.approxPolyDP(c, 0.04 * peri, True)
target = "Target Aquisition Successful"
return target
def mask_image(self, cv_image, lower, upper):
mask = cv2.inRange(cv_image, lower, upper)
# Remove anything not within the bounds of our mask
output = cv2.bitwise_and(cv_image, cv_image, mask=mask)
print('ree')
if (self.debug):
try:
# print(output)
self.mask_image_pub.publish(
self.bridge.cv2_to_imgmsg(np.array(output), 'bgr8'))
except CvBridgeError as e:
print(e)
return output
def acquire_targets(self, cv_image):
# Take in the data and get its dimensions.
height, width, channels = cv_image.shape
# create NumPy arrays from the boundaries
lower = np.array(self.lower, dtype="uint8")
upper = np.array(self.upper, dtype="uint8")
# Generate a mask based on the constants.
blurred = self.mask_image(cv_image, lower, upper)
blurred = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY)
# Compute contours
cnts = cv2.findContours(blurred.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = cnts[1]
'''
We use OpenCV to compute our contours and then begin processing them
to ensure we are identifying a proper target.
'''
shape = ''
peri_max = 0
max_x = 0
max_y = 0
m_shape = ''
for c in cnts:
# compute the center of the contour, then detect the name of the
# shape using only the contour
M = cv2.moments(c)
if M["m00"] == 0:
M["m00"] = .000001
cX = int((M["m10"] / M["m00"]))
cY = int((M["m01"] / M["m00"]))
self.point_pub.publish(Point(x=cX, y=cY))
shape = self.detect(c)
# multiply the contour (x, y)-coordinates by the resize ratio,
# then draw the contours and the name of the shape on the image
c = c.astype("float")
# c *= ratio
c = c.astype("int")
if shape == "Target Aquisition Successful":
if self.debug:
try:
cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2)
cv2.putText(cv_image, shape, (cX, cY),
cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(255, 255, 255), 2)
self.image_pub.publish(cv_image)
except CvBridgeError as e:
print(e)
# Grab the largest contour. Generally this is a safe bet but... We may need to tweak this for the three different vampires.
peri = cv2.arcLength(c, True)
if peri > peri_max:
peri_max = peri
max_x = cX
max_y = cY
m_shape = shape
'''
Approximate 3D coordinates.
'''
if m_shape == "Target Aquisition Successful":
try:
self.tf_listener.waitForTransform('/map',
self.camera_model.tfFrame(),
self.last_image_time,
rospy.Duration(0.2))
except tf.Exception as e:
rospy.logwarn(
"Could not transform camera to map: {}".format(e))
return False
(t, rot_q) = self.tf_listener.lookupTransform(
'/map', self.camera_model.tfFrame(), self.last_image_time)
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
center = np.array([max_x, max_y])
self.add_observation(center, (np.array(t), R),
self.last_image_time)
observations, pose_pairs = self.get_observations_and_pose_pairs()
if len(observations) > self.min_observations:
self.est = self.multi_obs.lst_sqr_intersection(
observations, pose_pairs)
self.status = 'Pose found'
else:
self.status = '{} observations'.format(len(observations))
class BuoyFinder(object):
'''
Node to find red, green, and yellow buoys in a single camera frame.
Combines several observations and uses a least-squares approach to get a 3D
position estimate of a buoy when requested.
Intended to be modular so other approaches can be tried. Adding more sophistication
to segmentation would increase reliability.
TODO: Use same mask for yellow/green
'''
def __init__(self):
self.tf_listener = tf.TransformListener()
self.enabled = False
self.last_image = None
self.last_image_time = None
self.camera_model = None
self.circle_finder = CircleFinder(
1.0
) # Model radius doesn't matter beacause it's not being used for 3D pose
# Various constants for tuning, debugging. See buoy_finder.yaml for more info
self.min_observations = rospy.get_param('~min_observations')
self.debug_ros = rospy.get_param('~debug/ros', True)
self.debug_cv = rospy.get_param('~debug/cv', False)
self.min_contour_area = rospy.get_param('~min_contour_area')
self.max_circle_error = rospy.get_param('~max_circle_error')
self.max_velocity = rospy.get_param('~max_velocity')
self.roi_y = rospy.get_param('~roi_y')
self.roi_height = rospy.get_param('~roi_height')
camera = rospy.get_param('~camera_topic',
'/camera/front/right/image_rect_color')
self.buoys = {}
for color in ['red', 'yellow', 'green']:
self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
if self.debug_cv:
cv2.waitKey(1)
self.debug_images = {}
# Keep latest odom message for sanity check
self.last_odom = None
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=3)
self.image_sub = Image_Subscriber(camera, self.image_cb)
if self.debug_ros:
self.rviz = rviz.RvizVisualizer(topic='~markers')
self.mask_pub = Image_Publisher('~mask_image')
rospy.Timer(rospy.Duration(1), self.print_status)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~enable', SetBool, self.toggle_search)
rospy.Service('~2D', VisionRequest2D, self.request_buoy)
rospy.Service('~pose', VisionRequest, self.request_buoy3d)
rospy.loginfo("BUOY FINDER: initialized successfully")
def odom_cb(self, odom):
self.last_odom = odom
def toggle_search(self, srv):
'''
Callback for standard ~enable service. If true, start
looking at frames for buoys.
'''
if srv.data:
rospy.loginfo("BUOY FINDER: enabled")
self.enabled = True
else:
rospy.loginfo("BUOY FINDER: disabled")
self.enabled = False
return SetBoolResponse(success=True)
def request_buoy(self, srv):
'''
Callback for 2D vision request. Returns centroid
of buoy found with color specified in target_name
if found.
'''
if not self.enabled or srv.target_name not in self.buoys:
return VisionRequest2DResponse(found=False)
response = self.find_single_buoy(srv.target_name)
if response is False or response is None:
return VisionRequest2DResponse(found=False)
center, radius = response
return VisionRequest2DResponse(header=Header(
stamp=self.last_image_time, frame_id=self.frame_id),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True)
def request_buoy3d(self, srv):
'''
Callback for 3D vision request. Uses recent observations of buoy
specified in target_name to attempt a least-squares position estimate.
As buoys are spheres, orientation is meaningless.
'''
if srv.target_name not in self.buoys or not self.enabled:
return VisionRequestResponse(found=False)
buoy = self.buoys[srv.target_name]
if buoy.est is None:
return VisionRequestResponse(found=False)
return VisionRequestResponse(pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(position=Point(*buoy.est))),
found=True)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find each color buoy.
'''
if not self.enabled:
return
# Crop out some of the top and bottom to exclude the floor and surface reflections
height = image.shape[0]
roi_y = int(self.roi_y * height)
roi_height = height - int(self.roi_height * height)
self.roi = (0, roi_y, roi_height, image.shape[1])
self.last_image = image[self.roi[1]:self.roi[2],
self.roi[0]:self.roi[3]]
self.image_area = self.last_image.shape[0] * self.last_image.shape[1]
if self.debug_ros:
# Create a blacked out debug image for putting masks in
self.mask_image = np.zeros(self.last_image.shape,
dtype=image.dtype)
if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.find_buoys()
if self.debug_ros:
self.mask_pub.publish(self.mask_image)
def print_status(self, _):
'''
Called at 1 second intervals to display the status (not found, n observations, FOUND)
for each buoy.
'''
if self.enabled:
rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'",
self.buoys['red'].status, self.buoys['green'].status,
self.buoys['yellow'].status)
def find_buoys(self):
'''
Run find_single_buoy for each color of buoy
'''
for buoy_name in self.buoys:
self.find_single_buoy(buoy_name)
def is_circular_contour(self, cnt):
'''
Check that a contour is close enough to a circle (using hue invariants)
to be a pottential buoy.
'''
return self.circle_finder.verify_contour(cnt) <= self.max_circle_error
def get_best_contour(self, contours):
'''
Attempts to find a good buoy contour among those found within the
thresholded mask. If a good one is found, it return (contour, centroid, area),
otherwise returns None. Right now the best contour is just the largest.
@param contours Numpy array of contours from a particular buoy's mask
@return A tuple (contour, error) where contour will be the best contour
in an image or None if no contours pass criteria. Error will be a string
describing why no good contour was found, or None if contour is not None.
'''
if len(contours) == 0:
return None, 'no contours in mask'
circular_contours = filter(self.is_circular_contour, contours)
if len(circular_contours) == 0:
return None, 'fails circularity test'
circles_sorted = sorted(circular_contours,
key=cv2.contourArea,
reverse=True)
if cv2.contourArea(
circles_sorted[0]) < self.min_contour_area * self.image_area:
return None, 'fails area test'
return circles_sorted[
0], None # Return the largest contour that pases shape test
def find_single_buoy(self, buoy_type):
'''
Attempt to find one color buoy in the image.
1) Create mask for buoy's color in colorspace specified in paramaters
2) Select the largest contour in this mask
3) Approximate a circle around this contour
4) Store the center of this circle and the current tf between /map and camera
as an observation
5) If observations for this buoy is now >= min_observations, approximate buoy
position using the least squares tool imported
'''
assert buoy_type in self.buoys.keys(
), "Buoys_2d does not know buoy color: {}".format(buoy_type)
buoy = self.buoys[buoy_type]
mask = buoy.get_mask(self.last_image)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.erode(mask, kernel, iterations=2)
mask = cv2.dilate(mask, kernel, iterations=2)
_, contours, _ = cv2.findContours(mask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE,
offset=(self.roi[0], self.roi[1]))
if self.debug_ros:
cv2.add(self.mask_image.copy(),
buoy.cv_colors,
mask=mask,
dst=self.mask_image)
if self.debug_cv:
self.debug_images[buoy_type] = mask.copy()
cnt, err = self.get_best_contour(contours)
if cnt is None:
buoy.clear_old_observations()
buoy.status = '{} w/ {} obs'.format(err, buoy.size())
return
center, radius = cv2.minEnclosingCircle(cnt)
if self.debug_ros:
cv2.circle(
self.mask_image,
(int(center[0] - self.roi[0]), int(center[1]) - self.roi[1]),
int(radius), buoy.cv_colors, 4)
try:
self.tf_listener.waitForTransform('/map', self.frame_id,
self.last_image_time,
rospy.Duration(0.2))
except tf.Exception as e:
rospy.logwarn("Could not transform camera to map: {}".format(e))
return False
if not self.sanity_check(center, self.last_image_time):
buoy.status = 'failed sanity check'
return False
(t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id,
self.last_image_time)
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
buoy.add_observation(center, (np.array(t), R), self.last_image_time)
observations, pose_pairs = buoy.get_observations_and_pose_pairs()
if len(observations) > self.min_observations:
buoy.est = self.multi_obs.lst_sqr_intersection(
observations, pose_pairs)
buoy.status = 'Pose found'
if self.debug_ros:
self.rviz.draw_sphere(buoy.est,
color=buoy.draw_colors,
scaling=(0.2286, 0.2286, 0.2286),
frame='/map',
_id=buoy.visual_id)
else:
buoy.status = '{} observations'.format(len(observations))
return center, radius
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
if self.last_odom is None:
return False
linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
if np.linalg.norm(linear_velocity) > self.max_velocity:
return False
return True
class CLAHE_generator:
def __init__(self):
self.camera = rospy.get_param('~camera_topic',
'/camera/front/left/image_rect_color')
# Instantiate remaining variables and objects
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
# Image Subscriber and Camera Information
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
'''
These variables store the camera information required to perform
the transformations on the coordinates to move from the subs
perspective to our global map perspective. This information is
also necessary to perform the least squares intersection which
will find the 3D coordinates of the torpedo board based on 2D
observations from the Camera. More info on this can be found in
sub8_vision_tools.
'''
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
self.image_pub = Image_Publisher("CLAHE/debug")
# Debug
self.debug = rospy.get_param('~debug', True)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find the torpedo board.
'''
self.last_image = image
if self.last_image_time is not None and \
self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in
# loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.CLAHE(image)
print('published')
def CLAHE(self, cv_image):
'''
CLAHE (Contrast Limited Adaptive Histogram Equalization)
This increases the contrast between color channels and allows us to
better differentiate colors under certain lighting conditions.
'''
clahe = cv2.createCLAHE(clipLimit=9.5, tileGridSize=(4, 4))
# convert from BGR to LAB color space
lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB)
l, a, b = cv2.split(lab) # split on 3 different channels
l2 = clahe.apply(l) # apply CLAHE to the L-channel
lab = cv2.merge((l2, a, b)) # merge channels
cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
self.image_pub.publish(cv_image)
class BuoyFinder(object):
'''
Node to find red, green, and yellow buoys in a single camera frame.
Combines several observations and uses a least-squares approach to get a 3D
position estimate of a buoy when requested.
Intended to be modular so other approaches can be tried. Adding more sophistication
to segmentation would increase reliability.
'''
def __init__(self):
self.tf_listener = tf.TransformListener()
self.enabled = False
self.last_image = None
self.last_image_time = None
self.camera_model = None
# Various constants for tuning, debugging. See buoy_finder.yaml for more info
self.min_observations = rospy.get_param('~min_observations')
self.debug_ros = rospy.get_param('~debug/ros', True)
self.debug_cv = rospy.get_param('~debug/cv', False)
self.min_contour_area = rospy.get_param('~min_contour_area')
self.max_velocity = rospy.get_param('~max_velocity')
camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color')
self.buoys = {}
for color in ['red', 'yellow', 'green']:
self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
self.buoys[color].load_segmentation()
if self.debug_cv:
cv2.waitKey(1)
self.debug_images = {}
# Keep latest odom message for sanity check
self.last_odom = None
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3)
self.image_sub = Image_Subscriber(camera, self.image_cb)
if self.debug_ros:
self.rviz = rviz.RvizVisualizer(topic='~markers')
self.mask_pub = Image_Publisher('~mask_image')
rospy.Timer(rospy.Duration(1), self.print_status)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~enable', SetBool, self.toggle_search)
rospy.Service('~2D', VisionRequest2D, self.request_buoy)
rospy.Service('~pose', VisionRequest, self.request_buoy3d)
rospy.loginfo("BUOY FINDER: initialized successfully")
def odom_cb(self, odom):
self.last_odom = odom
def toggle_search(self, srv):
'''
Callback for standard ~enable service. If true, start
looking at frames for buoys.
'''
if srv.data:
rospy.loginfo("BUOY FINDER: enabled")
self.enabled = True
else:
rospy.loginfo("BUOY FINDER: disabled")
self.enabled = False
return SetBoolResponse(success=True)
def request_buoy(self, srv):
'''
Callback for 2D vision request. Returns centroid
of buoy found with color specified in target_name
if found.
'''
if not self.enabled or srv.target_name not in self.buoys:
return VisionRequest2DResponse(found=False)
response = self.find_single_buoy(srv.target_name)
if response is False or response is None:
return VisionRequest2DResponse(found=False)
center, radius = response
return VisionRequest2DResponse(
header=Header(stamp=self.last_image_time, frame_id=self.frame_id),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True
)
def request_buoy3d(self, srv):
'''
Callback for 3D vision request. Uses recent observations of buoy
specified in target_name to attempt a least-squares position estimate.
As buoys are spheres, orientation is meaningless.
'''
if srv.target_name not in self.buoys or not self.enabled:
return VisionRequestResponse(found=False)
buoy = self.buoys[srv.target_name]
if buoy.est is None:
return VisionRequestResponse(found=False)
return VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(
position=Point(*buoy.est)
)
),
found=True
)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find each color buoy.
'''
if not self.enabled:
return
# Crop out some of the top and bottom to exclude the floor and surface reflections
height = image.shape[0]
roi_y = int(0.2 * height)
roi_height = height - int(0.2 * height)
self.roi = (0, roi_y, roi_height, image.shape[1])
self.last_image = image[self.roi[1]:self.roi[2], self.roi[0]:self.roi[3]]
if self.debug_ros:
# Create a blacked out debug image for putting masks in
self.mask_image = np.zeros(self.last_image.shape, dtype=image.dtype)
if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.find_buoys()
if self.debug_ros:
self.mask_pub.publish(self.mask_image)
def print_status(self, _):
'''
Called at 1 second intervals to display the status (not found, n observations, FOUND)
for each buoy.
'''
if self.enabled:
rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'",
self.buoys['red'].status,
self.buoys['green'].status,
self.buoys['yellow'].status)
def find_buoys(self):
'''
Run find_single_buoy for each color of buoy
'''
for buoy_name in self.buoys:
self.find_single_buoy(buoy_name)
def get_best_contour(self, contours):
'''
Attempts to find a good buoy contour among those found within the
thresholded mask. If a good one is found, it return (contour, centroid, area),
otherwise returns None. Right now the best contour is just the largest.
TODO: Use smarter contour filtering methods, like checking this it is circle like
'''
if len(contours) > 0:
cnt = max(contours, key=cv2.contourArea)
area = cv2.contourArea(cnt)
if area < self.min_contour_area:
return None
M = cv2.moments(cnt)
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
tpl_center = (int(cx), int(cy))
return cnt, tpl_center, area
else:
return None
def find_single_buoy(self, buoy_type):
'''
Attempt to find one color buoy in the image.
1) Create mask for buoy's color in colorspace specified in paramaters
2) Select the largest contour in this mask
3) Approximate a circle around this contour
4) Store the center of this circle and the current tf between /map and camera
as an observation
5) If observations for this buoy is now >= min_observations, approximate buoy
position using the least squares tool imported
'''
assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type)
buoy = self.buoys[buoy_type]
mask = buoy.get_mask(self.last_image)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.erode(mask, kernel, iterations=2)
mask = cv2.dilate(mask, kernel, iterations=2)
_, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE,
offset=(self.roi[0], self.roi[1]))
ret = self.get_best_contour(contours)
if ret is None:
buoy.clear_old_observations()
buoy.status = 'not seen w/ {} obs'.format(buoy.size())
return
contour, tuple_center, area = ret
true_center, rad = cv2.minEnclosingCircle(contour)
if self.debug_ros:
cv2.add(self.mask_image.copy(), buoy.cv_colors, mask=mask, dst=self.mask_image)
cv2.circle(self.mask_image, (int(true_center[0] - self.roi[0]), int(true_center[1]) - self.roi[1]),
int(rad), buoy.cv_colors, 2)
if self.debug_cv:
self.debug_images[buoy_type] = mask.copy()
try:
self.tf_listener.waitForTransform('/map', self.frame_id, self.last_image_time, rospy.Duration(0.2))
except tf.Exception as e:
rospy.logwarn("Could not transform camera to map: {}".format(e))
return False
if not self.sanity_check(tuple_center, self.last_image_time):
buoy.status = 'failed sanity check'
return False
(t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id, self.last_image_time)
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
buoy.add_observation(true_center, (np.array(t), R), self.last_image_time)
observations, pose_pairs = buoy.get_observations_and_pose_pairs()
if len(observations) > self.min_observations:
buoy.est = self.multi_obs.lst_sqr_intersection(observations, pose_pairs)
buoy.status = 'Pose found'
if self.debug_ros:
self.rviz.draw_sphere(buoy.est, color=buoy.draw_colors,
scaling=(0.2286, 0.2286, 0.2286),
frame='/map', _id=buoy.visual_id)
else:
buoy.status = '{} observations'.format(len(observations))
return tuple_center, rad
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
if self.last_odom is None:
return False
linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
if np.linalg.norm(linear_velocity) > self.max_velocity:
return False
return True
class ros_cv_testing_node:
def __init__(self):
# Get information for this particular camera
camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo)
print camera_info
# Set information for the camera
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Subscribe to the camera's image topic and depth image topic
self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw",
Image, self.on_rgb)
self.depth_image_sub = rospy.Subscriber("head_camera/depth_registered/image_raw",
Image, self.on_depth)
# Publish where the button is in the base link frame
self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10)
# Set up connection to CvBridge
self.bridge = CvBridge()
# Open up viewing windows
cv2.namedWindow('depth')
cv2.namedWindow('rgb')
# Set up the class variables
self.rgb_image = None
self.rgb_image_time = None
self.depth_image = None
self.center = None
self.return_point = PointStamped()
self.tf_listener = TransformListener()
def on_rgb(self, image):
# Convert image to something that cv2 can work with
try:
self.rgb_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
except e:
print e
return
self.rgb_image_time = image.header.stamp
# Get height and width of image
h = self.rgb_image.shape[0]
w = self.rgb_image.shape[1]
# Create empty image
color_dst = numpy.empty((h,w), 'uint8')
# Convert picture to grayscale
cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2GRAY, color_dst)
# Find circles given the camera image
dp = 1.1
minDist = 90
circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp, minDist)
# If no circles are detected then exit the program
if circles == None:
print "No circles found using these parameters"
sys.exit()
circles = numpy.uint16(numpy.around(circles))
# Draw the center of the circle closest to the top
ycoord = []
for i in circles[0,:]:
ycoord.append(i[1])
# Find the top circle in the frame
top_circ_y_coor = min(ycoord)
x_coor = 0
y_coor = 0
for i in circles[0,:]:
if i[1] == top_circ_y_coor:
# draw the center of the circle
#cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3)
# draw outer circle
#cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2)
x_coor = i[0]
y_coor = i[1]
cv2.circle(self.rgb_image,(327,247),2,(0,0,255),3)
# Set the coordinates for the center of the circle
self.center = (x_coor, y_coor)
#self.center = (328,248)
cv2.imshow('rgb', self.rgb_image)
cv2.waitKey(1)
def on_depth(self, image):
# Use numpy so that cv2 can use image
self.depth_image = numpy.fromstring(image.data, dtype='float32').reshape((480,640))
nmask = numpy.isnan(self.depth_image)
#Find the minimum and the maximum of the depth image
Dmin = self.depth_image[~nmask].min()
Dmax = self.depth_image[~nmask].max()
# If a circle has been found find its depth and apply that to the ray
if self.center!=None:
ray = self.cam_model.projectPixelTo3dRay(self.center)
depth = self.depth_image[self.center[1]][self.center[0]]
# If the depth is not a number exit
if math.isnan(depth):
print "Incomplete information on depth at this point"
return
# Otherwise mulitply the depth by the unit vector of the ray
else:
print "depth", depth
cam_coor = [depth*ray[0],depth*ray[1],depth*ray[2]]
#print "camera frame coordinate:", cam_coor
else:
return
# Rescale to [0,1]
cv2.imshow('depth', self.depth_image / 2.0)
cv2.waitKey(1)
# Only use depth if the RGB image is running
if self.rgb_image_time is None:
rospy.loginfo('not using depth cause no RGB yet')
return
time_since_rgb = image.header.stamp - self.rgb_image_time
# Only use depth if it is close in time to the RGB image
if time_since_rgb > rospy.Duration(0.5):
rospy.loginfo('not using depth because time since RGB is ' + str(time_since_rgb))
#return
# Find transform from camera frame to world frame
self.tf_listener.waitForTransform(self.cam_model.tfFrame(), "base_link",
image.header.stamp, rospy.Duration(1.0))
# Set up the point to be published
self.return_point.header.stamp = image.header.stamp
self.return_point.header.frame_id = self.cam_model.tfFrame()
self.return_point.point.x = cam_coor[0]
self.return_point.point.y = cam_coor[1]
self.return_point.point.z = cam_coor[2]
# Transform point to the baselink frame
self.return_point = self.tf_listener.transformPoint("base_link", self.return_point)
print "world frame coordinate:", self.return_point.point.x, \
self.return_point.point.y,self.return_point.point.z, "\n"
self.point_pub.publish(self.return_point)
class torp_vision:
def __init__(self):
# Pull constants from config file
self.lower = rospy.get_param('~lower_color_threshold', [0, 0, 80])
self.upper = rospy.get_param(
'~higher_color_threshold', [200, 200, 250])
self.min_contour_area = rospy.get_param('~min_contour_area', .001)
self.max_contour_area = rospy.get_param('~max_contour_area', 400)
self.min_trans = rospy.get_param('~min_trans', .05)
self.max_velocity = rospy.get_param('~max_velocity', 1)
self.timeout = rospy.Duration(
rospy.get_param('~timeout_seconds'), 250000)
self.min_observations = rospy.get_param('~min_observations', 8)
self.camera = rospy.get_param('~camera_topic',
'/camera/front/left/image_rect_color')
# Instantiate remaining variables and objects
self._observations = deque()
self._pose_pairs = deque()
self._times = deque()
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
# Image Subscriber and Camera Information
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
'''
These variables store the camera information required to perform
the transformations on the coordinates to move from the subs
perspective to our global map perspective. This information is
also necessary to perform the least squares intersection which
will find the 3D coordinates of the torpedo board based on 2D
observations from the Camera. More info on this can be found in
sub8_vision_tools.
'''
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
# Ros Services so mission can be toggled and info requested
rospy.Service('~enable', SetBool, self.toggle_search)
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~pose', VisionRequest, self.request_board3d)
self.image_pub = Image_Publisher("torp_vision/debug")
self.point_pub = rospy.Publisher(
"torp_vision/points", Point, queue_size=1)
self.mask_image_pub = rospy.Publisher(
'torp_vison/mask', Image, queue_size=1)
# Debug
self.debug = rospy.get_param('~debug', True)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find the torpedo board.
'''
if not self.enabled:
return
self.last_image = image
if self.last_image_time is not None and \
self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in
# loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.acquire_targets(image)
def toggle_search(self, srv):
'''
Callback for standard ~enable service. If true, start
looking at frames for buoys.
'''
if srv.data:
rospy.loginfo("TARGET ACQUISITION: enabled")
self.enabled = True
else:
rospy.loginfo("TARGET ACQUISITION: disabled")
self.enabled = False
return SetBoolResponse(success=True)
def request_board3d(self, srv):
'''
Callback for 3D vision request. Uses recent observations of target
board specified in target_name to attempt a least-squares position
estimate. Ignoring orientation of board.
'''
if not self.enabled:
return VisionRequestResponse(found=False)
# buoy = self.buoys[srv.target_name]
if self.est is None:
return VisionRequestResponse(found=False)
return VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(position=Point(*self.est))),
found=True)
def clear_old_observations(self):
# Observations older than two seconds are discarded.
time = rospy.Time.now()
i = 0
while i < len(self._times):
if time - self._times[i] > self.timeout:
self._times.popleft()
self._observations.popleft()
self._pose_pairs.popleft()
else:
i += 1
# print('Clearing')
def size(self):
return len(self._observations)
def add_observation(self, obs, pose_pair, time):
self.clear_old_observations()
# print('Adding...')
if self.size() == 0 or np.linalg.norm(
self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans:
self._observations.append(obs)
self._pose_pairs.append(pose_pair)
self._times.append(time)
def get_observations_and_pose_pairs(self):
self.clear_old_observations()
return (self._observations, self._pose_pairs)
def detect(self, c):
# initialize the shape name and approximate the contour
target = "unidentified"
peri = cv2.arcLength(c, True)
if peri < self.min_contour_area or peri > self.max_contour_area:
return target
approx = cv2.approxPolyDP(c, 0.04 * peri, True)
if len(approx) == 4:
target = "Target Aquisition Successful"
elif len(approx) == 3 or len(approx) == 5:
target = "Partial Target Acquisition"
return target
def CLAHE(self, cv_image):
'''
CLAHE (Contrast Limited Adaptive Histogram Equalization)
This increases the contrast between color channels and allows us to
better differentiate colors under certain lighting conditions.
'''
clahe = cv2.createCLAHE(clipLimit=5., tileGridSize=(4, 4))
# convert from BGR to LAB color space
lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB)
l, a, b = cv2.split(lab) # split on 3 different channels
l2 = clahe.apply(l) # apply CLAHE to the L-channel
lab = cv2.merge((l2, a, b)) # merge channels
cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
return cv_image
def mask_image(self, cv_image, lower, upper):
mask = cv2.inRange(cv_image, lower, upper)
# Remove anything not within the bounds of our mask
output = cv2.bitwise_and(cv_image, cv_image, mask=mask)
# Resize to emphasize shapes
# gray = cv2.cvtColor(output, cv2.COLOR_BGR2GRAY)
# Blur image so our contours can better find the full shape.
# blurred = cv2.GaussianBlur(gray, (5, 5), 0)
if (self.debug):
try:
# print(output)
self.mask_image_pub.publish(
self.bridge.cv2_to_imgmsg(np.array(output), 'bgr8'))
except CvBridgeError as e:
print(e)
return output
def acquire_targets(self, cv_image):
# Take in the data and get its dimensions.
height, width, channels = cv_image.shape
# Run CLAHE.
cv_image = self.CLAHE(cv_image)
# Now we generate a color mask to isolate only red in the image. This
# is achieved through the thresholds which can be changed in the above
# constants.
# create NumPy arrays from the boundaries
lower = np.array(self.lower, dtype="uint8")
upper = np.array(self.upper, dtype="uint8")
# Generate a mask based on the constants.
blurred = self.mask_image(cv_image, lower, upper)
blurred = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY)
# Compute contours
cnts = cv2.findContours(blurred.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = cnts[1]
'''
We use OpenCV to compute our contours and then begin processing them
to ensure we are identifying a proper target.
'''
shape = ''
peri_max = 0
max_x = 0
max_y = 0
m_shape = ''
for c in cnts:
# compute the center of the contour, then detect the name of the
# shape using only the contour
M = cv2.moments(c)
if M["m00"] == 0:
M["m00"] = .000001
cX = int((M["m10"] / M["m00"]))
cY = int((M["m01"] / M["m00"]))
shape = self.detect(c)
# multiply the contour (x, y)-coordinates by the resize ratio,
# then draw the contours and the name of the shape on the image
c = c.astype("float")
# c *= ratio
c = c.astype("int")
if shape == "Target Aquisition Successful":
if self.debug:
try:
cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2)
cv2.putText(cv_image, shape, (cX, cY),
cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(255, 255, 255), 2)
self.image_pub.publish(cv_image)
except CvBridgeError as e:
print(e)
peri = cv2.arcLength(c, True)
if peri > peri_max:
peri_max = peri
max_x = cX
max_y = cY
m_shape = shape
'''
This is Kevin's Code, adapted for this project. We are trying to find
the 3D coordinates of the torpedo board/target to give us a better idea
of where we are trying to go and perform more accurate movements
to align with the target. The first thing we need to do is convert from
camera coordinates in pixels to 3D coordinates.
Every time we succesfully get a target aquisition we add it to the
counter. Once we observe it enough times
we can be confident we are looking at the correct target. We then
perform an least squares intersection from multiple angles
to derive the approximate 3D coordinates.
'''
if m_shape == "Target Aquisition Successful":
try:
self.tf_listener.waitForTransform('/map',
self.camera_model.tfFrame(),
self.last_image_time,
rospy.Duration(0.2))
except tf.Exception as e:
rospy.logwarn(
"Could not transform camera to map: {}".format(e))
return False
(t, rot_q) = self.tf_listener.lookupTransform(
'/map', self.camera_model.tfFrame(), self.last_image_time)
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
center = np.array([max_x, max_y])
self.add_observation(center, (np.array(t), R),
self.last_image_time)
observations, pose_pairs = self.get_observations_and_pose_pairs()
if len(observations) > self.min_observations:
self.est = self.multi_obs.lst_sqr_intersection(
observations, pose_pairs)
self.status = 'Pose found'
else:
self.status = '{} observations'.format(len(observations))
class Colorama(object):
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)
def _compute_average_angle(self, angles, weights):
"""
Returns weighted average of angles.
Tends to break down with too many angles 180 degrees of each other - try not to do that.
"""
angles = np.array(angles)
if np.linalg.norm(weights) == 0:
return None
w = weights / np.linalg.norm(weights)
s = np.sum(w * np.sin(angles))
c = np.sum(w * np.cos(angles))
avg_angle = np.arctan2(s, c)
return avg_angle
def _get_quaternion_error(self, q, target_q):
"""
Returns an angluar differnce between q and target_q in radians
"""
dq = trns.quaternion_multiply(np.array(target_q),
trns.quaternion_inverse(np.array(q)))
return 2 * np.arccos(dq[3])
def _get_closest_color(self, hue_angle):
"""
Returns a pair of the most likely color and the radian error associated with that prediction
`hue_angle` := The radian value associated with hue [0, 2*pi]
Colors are found from `self.color_map`
"""
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'undef'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
# We need a signed error for some math later on so no absolute value
this_error = np.arctan2(sg * c - cg * s, cg * c + sg * s)
if np.abs(this_error) < np.abs(error):
error = this_error
likely_color = color
fprint("Likely color: {} with an hue error of {} rads.".format(
likely_color, np.round(error, 3)))
return [likely_color, error]
def do_estimate(self, totem_id):
"""Calculates best color estimate for a given totem id"""
fprint("DOING ESTIMATE", msg_color='blue')
if totem_id not in self.colored:
fprint("Totem ID {} not found!".format(totem_id), msg_color='red')
return None
t_color = self.colored[totem_id]
if len(t_color) < self.min_obs:
fprint("Only {} observations. {} required.".format(
len(t_color), self.min_obs),
msg_color='red')
return None
kwargs = {
'v_u': self.v_u,
'v_sig': self.v_sig,
'dist_sig': self.dist_sig,
'q_factor': self.q_factor,
'q_sig': self.q_sig
}
w, weights = t_color.compute_confidence(
[self.v_factor, self.dist_factor, self.q_factor], True, **kwargs)
fprint("CONF: {}".format(w))
if np.mean(w) < self.conf_reject:
return None
hue_angles = np.radians(np.array(t_color.hues) * 2)
angle = self._compute_average_angle(hue_angles, w)
color = self._get_closest_color(angle)
msg = t_color.as_message
msg.id = totem_id
msg.confidence = w
msg.labels = ["value_errs", "dists", "q_diffs"]
msg.weights = weights
msg.color = colors[0]
msg.est_hues = angle * 2
msg.hues = np.array(t_color.hues) * 2
self.status_pub.publish(msg)
fprint("Color: {}".format(color[0]))
return color
def got_request(self, req):
# Threading blah blah something unsafe
colored_ids = [
_id for _id, color_err in self.colored.iteritems()
if self.valid_color(_id) == req.color
]
fprint("Colored IDs: {}".format(colored_ids), msg_color='blue')
print '\n' * 50
if len(colored_ids) == 0:
return ColorRequestResponse(found=False)
return ColorRequestResponse(found=True, ids=colored_ids)
def do_observe(self, *args):
resp = self.make_request(name='totem')
# If atleast one totem was found start observing
if resp.found:
# Time of the databse request
time_of_marker = resp.objects[0].header.stamp # - ros_t(1)
fprint("Looking for image at {}".format(time_of_marker.to_sec()),
msg_color='yellow')
image_holder = self.image_history.get_around_time(time_of_marker)
if not image_holder.contains_valid_image:
t = self.image_history.newest_image.time
if t is None:
fprint("No images found.")
return
fprint("No valid image found for t={} ({}) dt: {}".format(
time_of_marker.to_sec(), t.to_sec(),
(rospy.Time.now() - t).to_sec()),
msg_color='red')
return
header = navigator_tools.make_header(frame='/enu',
stamp=image_holder.time)
image = image_holder.image
self.debug.image = np.copy(image)
cam_tf = self.camera_model.tfFrame()
try:
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)
except tf.ExtrapolationException as e:
fprint("TF error found and excepted: {}".format(e))
return
for obj in resp.objects:
if len(obj.points) <= 1:
fprint("No points in object")
continue
fprint("{} {}".format(obj.id, "=" * 50))
# Get object position in px coordinates to determine if it's in frame
object_cam = t_mat44.dot(np.append(p2np(obj.position), 1))
object_px = map(
int, self.camera_model.project3dToPixel(object_cam[:3]))
if not self._object_in_frame(object_cam):
fprint("Object not in frame")
continue
# Get enu points associated with this totem and remove ones that are too low
points_np = np.array(map(p2np, obj.points))
height = np.max(points_np[:, 2]) - np.min(points_np[:, 2])
if height < .1:
# If the height of the object is too small, skip (units are meters)
fprint("Object too small")
continue
threshold = np.min(points_np[:,
2]) + self.height_remove * height
points_np = points_np[points_np[:, 2] > threshold]
# Shove ones in there to make homogenous points to get points in image frame
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])
[
cv2.circle(self.debug.image, tuple(map(int, p)), 2,
(255, 255, 255), -1) for p in points_px
]
# Get color information from the points
roi = self._get_ROI_from_points(points_px)
h, s, v = self._get_hsv_from_ROI(roi, image)
# Compute parameters that go into the confidence
boat_q = self.odom[1]
target_q = self._get_solar_angle()
q_err = self._get_quaternion_error(boat_q, target_q)
dist = np.linalg.norm(self.odom[0] - p2np(obj.position))
fprint("H: {}, S: {}, V: {}".format(h, s, v))
fprint("q_err: {}, dist: {}".format(q_err, dist))
# Add to database and setup debug image
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v,
self.value_reject),
msg_color='red')
else:
if obj.id not in self.colored:
self.colored[obj.id] = Observation()
# Add this observation in
self.colored[obj.id] += h, v, dist, q_err
print self.colored[obj.id]
rgb = (0, 0, 0)
color = self.do_estimate(obj.id)
# Do we have a valid color estimate
if color:
fprint("COLOR IS VALID", msg_color='green')
rgb = self.database_color_map[color[0]]
cmd = '{name}={rgb[0]},{rgb[1]},{rgb[2]},{_id}'
self.make_request(
cmd=cmd.format(name=obj.name, _id=obj.id, rgb=rgb))
bgr = rgb[::-1]
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)
def _get_solar_angle(self):
return [0, 0, 0, 1]
def _get_ROI_from_points(self, image_points):
cnt = np.array([image_points]).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_hsv_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
return h, s, v
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
err_msg = "The colors aren't expressive enough s: {} ({}) v: {} ({}). Rejecting."
fprint(err_msg.format(s, self.saturation_reject, v,
self.value_reject),
msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
# Display the current values
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.debug.image,
"h_val: {}".format(np.degrees(hue_angle)), tuple(roi[1]),
font, 1, (255, 255, 255), 2)
likely_color, error = self.get_closest_color(hue_angle)
if error > self.hue_error_reject:
fprint(
"Closest color was {} with an error of {} rads (> {}). Rejecting."
.format(likely_color, np.round(error, 3),
self.hue_error_reject),
msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(
likely_color, np.round(error, 3)))
return [likely_color, error]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
return not (np.any([0, 0] > px) or np.any(px > resoultion))
class Colorama(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 valid_color(self, _id):
"""
Either returns valid color or None depending if the color is valid or not
"""
if _id not in self.colored:
return None
object_data = self.colored[_id]
print "object_data", object_data
# Maps color => [err1, err2, ..]
potential_colors = {}
for data in object_data:
color, err = data['color'], data['err']
if color not in potential_colors:
potential_colors[color] = []
potential_colors[color].append(err)
potential_colors_avg = {}
for color, errs in potential_colors.iteritems():
if len(errs) > self.spottings_req:
potential_colors_avg[color] = np.average(errs)
print "potential_colors_avg", potential_colors_avg
if len(potential_colors_avg) == 1:
# No debate about the color
color = potential_colors_avg.keys()[0]
err = potential_colors_avg[color]
fprint("Only one color found, error: {} (/ {})".format(err, self.error_tolerance))
if len(potential_colors[color]) > self.spottings_req and err <= self.error_tolerance:
return color
elif len(potential_colors_avg) > 1:
# More than one color, see if one of them is still valid
fprint("More than one color detected. Removing all.")
self.colored[_id] = []
return None
def got_request(self, req):
# Threading blah blah something unsafe
color = self.valid_color(req.target_id)
if color is None:
fprint("ID {} is NOT a valid color".format(req.target_id), msg_color='red')
return VisionRequestResponse(found=False)
return VisionRequestResponse(found=True, color=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 _get_ROI_from_points(self, image_points):
# Probably will return a set of contours
cnt = np.array(image_points).astype(np.float32)
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
fprint("Drawing rectangle")
cv2.drawContours(self.debug.image, [box], 0, (0, 0, 255), 2)
return box
def _get_color_from_ROI(self, roi, img):
mask = np.zeros(img.shape[:2], np.uint8)
cv2.drawContours(mask, [roi], 0, 255, -1)
bgr = cv2.mean(img, mask)
# We have to treat that bgr value as a 3d array to get cvtColor to work
bgr = np.array([[bgr]])[:, :3].astype(np.uint8)
h, s, v = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)[0, 0]
# Now check that s and v are in a good range
if s < self.saturation_reject or v < self.value_reject:
fprint("The colors aren't expressive enough. Rejecting.", msg_color='red')
return None
# Compute hue error in SO2
hue_angle = np.radians(h * 2)
c = np.cos(hue_angle)
s = np.sin(hue_angle)
error = np.inf
likely_color = 'bazinga'
for color, h_val in self.color_map.iteritems():
cg = np.cos(h_val)
sg = np.sin(h_val)
this_error = np.abs(np.arctan2(sg*c - cg*s, cg*c + sg*s))
if this_error < error:
error = this_error
likely_color = color
if error > self.hue_error_reject:
fprint("Closest color was {} with an error of {} rads (> {}) Rejecting.".format(likely_color, np.round(error, 3),
self.hue_error_reject), msg_color='red')
return None
fprint("Likely color: {} with an hue error of {} rads.".format(likely_color, np.round(error, 3)))
return [np.degrees(self.color_map[likely_color]) / 2.0, likely_color, error]
def _object_in_frame(self, object_point):
"""
Returns if the object is in frame given the centroid of the object.
`object_point` should be in the camera_model's frame.
"""
print object_point
if object_point[2] < 0:
return False
px = np.array(self.camera_model.project3dToPixel(object_point))
resoultion = self.camera_model.fullResolution()
if np.any([0,0] > px) or np.any(px > resoultion):
return False
return True
class PathMarkerFinder():
"""
Node which finds orange path markers in image frame,
estimates the 2d and 3d position/orientation of marker
and returns this estimate when service is called.
Unit tests for this node is in test_path_marker.py
Finding the marker works as follows:
* blur image
* threshold image mostly for highly saturated, orange/yellow/red objects
* run canny edge detection on thresholded image
* find contours on edge frame
* filter contours to find those that may be contours by:
* checking # of sides in appox polygon
* checking ratio of length/width close to known model
* estimates 3D pose using cv2.solvePnP with known object demensions and camera model
TODO: Implement Kalman filter to smooth pose estimate / eliminate outliers
"""
# Model of four corners of path marker, centered around 0 in meters
PATH_MARKER = np.array([[0.6096, -0.0762, 0], [0.6096, 0.0762, 0],
[-0.6096, 0.0762, 0], [-0.6096, -0.0762, 0]],
dtype=np.float)
# Coordinate axes for debugging image
REFERENCE_POINTS = np.array(
[[0, 0, 0], [0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], dtype=np.float)
def __init__(self):
self.debug_gui = False
self.last2d = None
self.last3d = None
self.enabled = False
self.cam = None
self.last_image = None
self.last_found_time = None
# Constants from launch config file
self.debug_ros = rospy.get_param("debug_ros", True)
self.canny_low = rospy.get_param("canny_low", 100)
self.canny_ratio = rospy.get_param("canny_ratio", 3.0)
self.thresh_hue_high = rospy.get_param("thresh_hue_high", 60)
self.thresh_saturation_low = rospy.get_param("thresh_satuation_low",
100)
self.min_contour_area = rospy.get_param("min_contour_area", 100)
self.length_width_ratio_err = rospy.get_param("length_width_ratio_err",
0.2)
self.approx_polygon_thresh = rospy.get_param("approx_polygon_thresh",
10)
camera = rospy.get_param("marker_camera",
"/camera/down/left/image_rect_color")
if self.debug_ros:
self.debug_pub = Image_Publisher("debug_image")
self.markerPub = rospy.Publisher('path_marker_visualization',
Marker,
queue_size=10)
self.service2D = rospy.Service('/vision/path_marker/2D',
VisionRequest2D, self.cb_2d)
self.service3D = rospy.Service('/vision/path_marker/pose',
VisionRequest, self.cb_3d)
self.toggle = rospy.Service('/vision/path_marker/enable', SetBool,
self.enable_cb)
self.image_sub = Image_Subscriber(camera, self.img_cb)
camera_info = self.image_sub.wait_for_camera_info()
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(camera_info)
def sendDebugMarker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the path marker.
This marker is a scaled cube with the demensions and color of the actual marker.
Only called if debug_ros param == True
'''
m = Marker()
m.header.frame_id = self.cam.tfFrame()
m.header.stamp = self.last_found_time
m.ns = "path_markers"
m.id = 0
m.type = 1
m.action = 0
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
# Real demensions of path marker
m.scale.x = 1.2192
m.scale.y = 0.1524
m.scale.z = 0.05
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(0)
self.markerPub.publish(m)
def enable_cb(self, x):
self.enabled = x.data
return SetBoolResponse(success=True)
def sortRect(self, rect):
'''
Given a contour of 4 points, returns the same 4 points sorted in a known way.
Used so that indicies of contour line up to that in model for cv2.solvePnp
p[0] = Top left corner of marker
p[1] = Top right corner of marker
p[2] = Bottom right corner of marker
p[3] = Bottom left cornet of marker
'''
def sort_contour_y(c):
return c[0][1]
def sort_contour_x(c):
return c[0][0]
sorted_y = np.array(sorted(rect, key=sort_contour_y))
sorted_x = np.array(sorted(rect, key=sort_contour_x))
sorted_rect = None
if (np.linalg.norm(sorted_y[0] - sorted_y[1]) >
np.linalg.norm(sorted_y[0] - sorted_y[2])
or np.linalg.norm(sorted_y[0] - sorted_y[1]) >
np.linalg.norm(sorted_y[0] - sorted_y[3])):
if sorted_x[1][0][1] > sorted_x[0][0][1]:
sorted_x[0], sorted_x[1] = sorted_x[1].copy(
), sorted_x[0].copy()
if sorted_x[2][0][1] > sorted_x[3][0][1]:
sorted_x[2], sorted_x[3] = sorted_x[3].copy(
), sorted_x[2].copy()
sorted_rect = sorted_x
else:
if sorted_y[0][0][0] > sorted_y[1][0][0]:
sorted_y[0], sorted_y[1] = sorted_y[1].copy(
), sorted_y[0].copy()
if sorted_y[3][0][0] > sorted_y[2][0][0]:
sorted_y[3], sorted_y[2] = sorted_y[2].copy(
), sorted_y[3].copy()
sorted_rect = sorted_y
return sorted_rect
def cb_3d(self, req):
res = VisionRequestResponse()
if (self.last2d == None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = self.cam.tfFrame()
res.pose.header.stamp = self.last_found_time
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def cb_2d(self, req):
res = VisionRequest2DResponse()
if (self.last3d == None or not self.enabled):
res.found = False
else:
res.header.frame_id = self.cam.tfFrame()
res.header.stamp = self.last_found_time
res.pose.x = self.last2d[0][0]
res.pose.y = self.last2d[0][1]
res.pose.theta = self.last2d[1]
res.found = True
return res
def get_3d_pose(self, p):
i_points = np.array((p[0][0], p[1][0], p[2][0], p[3][0]),
dtype=np.float)
retval, rvec, tvec = cv2.solvePnP(PathMarkerFinder.PATH_MARKER,
i_points, self.cam.intrinsicMatrix(),
np.zeros((5, 1)))
if tvec[2] < 0.3:
return False
self.last3d = (tvec.copy(),
quaternion_from_euler(0.0, 0.0, self.last2d[1]))
if self.debug_ros:
refs, _ = cv2.projectPoints(PathMarkerFinder.REFERENCE_POINTS,
rvec, tvec, self.cam.intrinsicMatrix(),
np.zeros((5, 1)))
refs = np.array(refs, dtype=np.int)
cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]),
(refs[1][0][0], refs[1][0][1]),
(0, 0, 255)) # X axis refs
cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]),
(refs[2][0][0], refs[2][0][1]), (0, 255, 0)) # Y axis ref
cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]),
(refs[3][0][0], refs[3][0][1]), (255, 0, 0)) # Z axis ref
return True
def get_2d_pose(self, r):
'''
Given a sorted 4 sided polygon, stores the centriod and angle
for the next service call to get 2dpose.
returns False if dx of polygon is 0, otherwise True
'''
top_center = r[0][0] + (r[1][0] - r[0][0]) / 2.0
bot_center = r[2][0] + (r[3][0] - r[2][0]) / 2.0
center = bot_center + (top_center - bot_center) / 2.0
y = top_center[1] - bot_center[1]
x = top_center[0] - bot_center[0]
if x == 0:
rospy.logerr("Contour dx is 0, strange...")
return False
angle = np.arctan(y / x)
self.last2d = (center, angle)
return True
def valid_contour(self, contour):
'''
Does various tests to filter out contours that are clearly not
a valid path marker.
* run approx polygon, check that sides == 4
* find ratio of length to width, check close to known ratio IRL
'''
if cv2.contourArea(contour) < self.min_contour_area:
return False
# Checks that contour is 4 sided
polygon = cv2.approxPolyDP(contour, self.approx_polygon_thresh, True)
if len(polygon) != 4:
return False
rect = self.sortRect(polygon)
for idx, p in enumerate(rect):
cv2.putText(self.last_image, str(idx), (p[0][0], p[0][1]),
cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0, 0, 255))
length = np.linalg.norm(rect[0][0] - rect[3][0])
width = np.linalg.norm(rect[0][0] - rect[1][0])
if width == 0:
rospy.logerr("Width == 0, strange...")
return False
length_width_ratio = length / width
# Checks that ratio of length to width is similar to known demensions (8:1)
if abs(length_width_ratio - 8.0) / 8.0 > self.length_width_ratio_err:
return False
if not self.get_2d_pose(rect):
return False
if not self.get_3d_pose(rect):
return False
return True
def get_edges(self):
'''
Proccesses latest image to find edges by:
blurring and thresholding for highly saturated orangish objects
then runs canny on threshold images and returns canny's edges
'''
blur = cv2.blur(self.last_image, (5, 5))
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
thresh = cv2.inRange(hsv, (0, self.thresh_saturation_low, 0),
(self.thresh_hue_high, 255, 255))
return cv2.Canny(thresh, self.canny_low,
self.canny_low * self.canny_ratio)
def img_cb(self, img):
if not self.enabled or self.cam == None:
return
self.last_image = img
edges = self.get_edges()
_, contours, _ = cv2.findContours(edges, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# Check if each contour is valid
for idx, c in enumerate(contours):
if self.valid_contour(c):
rospy.loginfo("Found path marker")
self.last_found_time = self.image_sub.last_image_time
if self.debug_ros:
self.sendDebugMarker()
cv2.drawContours(self.last_image, contours, idx,
(0, 255, 0), 3)
break
else:
if self.debug_ros:
cv2.drawContours(self.last_image, contours, idx,
(255, 0, 0), 3)
if self.debug_ros:
self.debug_pub.publish(self.last_image)
if self.debug_gui:
cv2.imshow("debug", self.last_image)
cv2.waitKey(5)
def callback_pos(self, msg):
if self.detector_switch == 'on':
x_c = 0.0
y_c = 0.0
z_c = 0.0
x_org = 0.0
y_org = 0.0
z_org = 0.0
if self.img_detection_fnc:
# original drone location:
x_org = msg.position.x
y_org = msg.position.y
z_org = msg.position.z
self.current_x = x_org
self.current_y = y_org
self.current_z = z_org
# need to transform back to the Eular angle
orientation_list = [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]
self.current_yaw = round(
self.RAD_to_DEG *
euler_from_quaternion(orientation_list)[2])
# camera location relative to drone:
x_c = 0.0
y_c = 0.0
z_c = 0.0
else:
pass
if self.pixel_center != []:
pose = Pose()
point_msg = PoseStamped()
# create vectors to scale the normalized coordinates in camera frame
t_1 = np.array([[x_org, y_org, z_org]])
t_2 = np.array([[x_c, y_c, z_c]])
# translation matrix from world to camera
t_W2C = np.add(t_1, t_2)
# transformation from image coordinate to camera coordinate
cam_info = rospy.wait_for_message(
"/firefly/vi_sensor/camera_depth/camera/camera_info",
CameraInfo,
timeout=None)
img_proc = PinholeCameraModel()
img_proc.fromCameraInfo(cam_info)
cam_model_point = img_proc.projectPixelTo3dRay(
self.pixel_center)
cam_model_point = np.round(np.multiply(
np.divide(cam_model_point, cam_model_point[2]), t_W2C[0,
2]),
decimals=0)
# creating a place holder to save the location info
point_msg.pose.position.x = cam_model_point[0]
point_msg.pose.position.y = cam_model_point[1]
point_msg.pose.position.z = cam_model_point[2]
point_msg.pose.orientation.x = 0
point_msg.pose.orientation.y = 0
point_msg.pose.orientation.z = 0
point_msg.pose.orientation.w = 1
point_msg.header.stamp = rospy.Time.now()
point_msg.header.frame_id = img_proc.tfFrame()
# initiate the coordinate (frame) transformation
self.tf_listener.waitForTransform(img_proc.tfFrame(), "world",
rospy.Time.now(),
rospy.Duration(1.0))
# calculate the transformed coordinates
tf_point = self.tf_listener.transformPose("world", point_msg)
x_new = 0.0 # round(tf_point.pose.position.x, 2)
y_new = 0.0 # round(tf_point.pose.position.y, 2)
z_new = msg.position.z - 0.01
# z_new = (1 - 0.01 * self.decent_rate) * msg.position.z
pose.position.x = x_new
pose.position.y = y_new
pose.position.z = z_new
pose.orientation.x = msg.orientation.x
pose.orientation.y = msg.orientation.y
pose.orientation.z = msg.orientation.z
pose.orientation.w = msg.orientation.w
self.pos_pub.publish(pose)
else:
# If there is no landing mark detected
pose = Pose()
# original drone location:
x_org = msg.position.x
y_org = msg.position.y
z_org = msg.position.z
self.current_x = x_org
self.current_y = y_org
self.current_z = z_org
# need to transform back to the Eular angle
orientation_list = [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]
self.current_yaw = round(
self.RAD_to_DEG *
euler_from_quaternion(orientation_list)[2])
pose.position.x = msg.position.x
pose.position.y = msg.position.y
pose.position.z = msg.position.z
pose.orientation.x = msg.orientation.x
pose.orientation.y = msg.orientation.y
pose.orientation.z = msg.orientation.z
pose.orientation.w = msg.orientation.w
self.pos_pub.publish(pose)
else:
# if the detection switch doesn't turn on, skip the coordinate computation
pass
class OrangeRectangleFinder():
"""
Node which finds orange rectangular objects in image frame.
This can be used for the path marker challenge and to detect
the lid of the bins challenge. The node estimates the 2d and 3d
position/orientation of this object and returns this estimate when service is called.
Unit tests for this node is in test_path_marker.py
Finding the marker works as follows:
* blur image
* threshold image mostly for highly saturated, orange/yellow/red objects
* run canny edge detection on thresholded image
* find contours on edge frame
* filter contours to find those that may be contours by:
* checking # of sides in appox polygon
* checking ratio of length/width close to known model
* estimates 3D pose using cv2.solvePnP with known object demensions and camera model
* Use translation vector from PnP and direction vector from 2d contour for pose
* Transform this frames pose into /map frame
* Plug this frames pose in /map into a kalman filter to reduce noise
TODO: Allow for two objects to be identifed at once, both filtered through its own KF
"""
# Coordinate axes for debugging image
REFERENCE_POINTS = np.array(
[[0, 0, 0], [0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], dtype=np.float)
def __init__(self):
self.debug_gui = False
self.enabled = False
self.cam = None
# Constants from launch config file
self.debug_ros = rospy.get_param("~debug_ros", True)
self.canny_low = rospy.get_param("~canny_low", 100)
self.canny_ratio = rospy.get_param("~canny_ratio", 3.0)
self.thresh_hue_high = rospy.get_param("~thresh_hue_high", 60)
self.thresh_saturation_low = rospy.get_param("~thresh_satuation_low",
100)
self.min_contour_area = rospy.get_param("~min_contour_area", 100)
self.epsilon_range = rospy.get_param("~epsilon_range", (0.01, 0.1))
self.epsilon_step = rospy.get_param("~epsilon_step", 0.01)
self.shape_match_thresh = rospy.get_param("~shape_match_thresh", 0.4)
self.min_found_count = rospy.get_param("~min_found_count", 10)
self.timeout_seconds = rospy.get_param("~timeout_seconds", 2.0)
# Default to scale model of path marker. Please use set_geometry service
# to set to correct model of object.
length = rospy.get_param("~length", 1.2192)
width = rospy.get_param("~width", 0.1524)
self.rect_model = RectFinder(length, width)
self.do_3D = rospy.get_param("~do_3D", True)
camera = rospy.get_param("~image_topic",
"/camera/down/left/image_rect_color")
self.tf_listener = tf.TransformListener()
# Create kalman filter to track 3d position and direction vector for marker in /map frame
self.state_size = 5 # X, Y, Z, DY, DX
self.filter = cv2.KalmanFilter(self.state_size, self.state_size)
self.filter.transitionMatrix = 1. * np.eye(self.state_size,
dtype=np.float32)
self.filter.measurementMatrix = 1. * np.eye(self.state_size,
dtype=np.float32)
self.filter.processNoiseCov = 1e-5 * np.eye(self.state_size,
dtype=np.float32)
self.filter.measurementNoiseCov = 1e-4 * np.eye(self.state_size,
dtype=np.float32)
self.filter.errorCovPost = 1. * np.eye(self.state_size,
dtype=np.float32)
self.reset()
self.service_set_geometry = rospy.Service('~set_geometry', SetGeometry,
self._set_geometry_cb)
if self.debug_ros:
self.debug_pub = Image_Publisher("~debug_image")
self.markerPub = rospy.Publisher('~marker', Marker, queue_size=10)
self.service2D = rospy.Service('~2D', VisionRequest2D,
self._vision_cb_2D)
if self.do_3D:
self.service3D = rospy.Service('~pose', VisionRequest,
self._vision_cb_3D)
self.toggle = rospy.Service('~enable', SetBool, self._enable_cb)
self.image_sub = Image_Subscriber(camera, self._img_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
assert self.camera_info is not None
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(self.camera_info)
def _set_geometry_cb(self, req):
self.rect_model = RectFinder.from_polygon(req.model)
self.reset()
rospy.loginfo("Resetting rectangle model to LENGTH=%f, WIDTH=%f",
self.rect_model.length, self.rect_model.width)
return {'success': True}
def _send_debug_marker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the object.
This marker is a scaled cube with the demensions of the model.
Only called if debug_ros param == True
'''
if self.last3d is None or not self.found:
return
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = self.last_found_time_3D
m.ns = "orange_rectangle"
m.id = 0
m.type = 1
m.action = 0
# Real demensions of path marker
m.scale.x = self.rect_model.length
m.scale.y = self.rect_model.width
m.scale.z = 0.05
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(self.timeout_seconds)
self.markerPub.publish(m)
def _enable_cb(self, x):
if x.data != self.enabled:
self.reset()
self.tf_listener.clear()
self.enabled = x.data
return SetBoolResponse(success=True)
def _vision_cb_3D(self, req):
res = VisionRequestResponse()
if self.last_found_time_3D is None or self.image_sub.last_image_time is None:
res.found = False
return res
dt = (self.image_sub.last_image_time -
self.last_found_time_3D).to_sec()
if dt < 0 or dt > self.timeout_seconds:
res.found = False
elif (self.last3d is None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = "/map"
res.pose.header.stamp = self.last_found_time_3D
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def _vision_cb_2D(self, req):
res = VisionRequest2DResponse()
if (self.last2d is None or not self.enabled):
res.found = False
else:
res.header.frame_id = self.cam.tfFrame()
res.header.stamp = self.last_found_time_2D
res.pose.x = self.last2d[0][0]
res.pose.y = self.last2d[0][1]
res.camera_info = self.camera_info
res.max_x = self.camera_info.width
res.max_y = self.camera_info.height
if self.last2d[1][0] < 0:
self.last2d[1][0] = -self.last2d[1][0]
self.last2d[1][1] = -self.last2d[1][1]
angle = np.arctan2(self.last2d[1][1], self.last2d[1][0])
res.pose.theta = angle
res.found = True
return res
def reset(self):
self.last_found_time_2D = None
self.last_found_time_3D = None
self.last2d = None
self.last3d = None
self._clear_filter(None)
def _clear_filter(self, state):
'''
Reset filter and found state. This will ensure that the object
is seen consistently before vision request returns true
'''
rospy.loginfo("MARKER LOST")
self.found_count = 0
self.found = False
self.last3d = None
self.filter.errorCovPre = 1. * np.eye(self.state_size,
dtype=np.float32)
if state is not None:
self.found_count = 1
state = np.array(state, dtype=np.float32)
self.filter.statePost = state
def _update_kf(self, (x, y, z, dy, dx)):
'''
Updates the kalman filter using the pose estimation
from the most recent frame. Also tracks time since last seen and how
often is has been seen to set the boolean "found" for the vision request
'''
if self.last_found_time_3D is None: # First time found, set initial KF pose to this frame
self._clear_filter((x, y, z, dy, dx))
self.last_found_time_3D = self.image_sub.last_image_time
return
dt = (self.image_sub.last_image_time -
self.last_found_time_3D).to_sec()
self.last_found_time_3D = self.image_sub.last_image_time
if dt < 0 or dt > self.timeout_seconds:
rospy.logwarn(
"Timed out since last saw marker, resetting. DT={}".format(dt))
self._clear_filter((x, y, z, dy, dx))
return
self.found_count += 1
measurement = 1. * np.array([x, y, z, dy, dx], dtype=np.float32)
self.filter.predict()
estimated = self.filter.correct(measurement)
if self.found_count > self.min_found_count:
angle = np.arctan2(estimated[3], estimated[4])
self.last3d = ((estimated[0], estimated[1], estimated[2]),
quaternion_from_euler(0.0, 0.0, angle))
if not self.found:
rospy.loginfo("Marker Found")
self.found = True
