class VisionNode(object): ''' Base class to be used unify the interfacing for MIL's computer vision scripts. Handles the bootstrap of image subscription, enable/disable, etc. Provides a callback for new images which is expected to return ''' __metaclass__ = abc.ABCMeta def __init__(self): self._objects_pub = rospy.Publisher("~identified_objects", ObjectsInImage, queue_size=3) self._camera_info = None self.camera_model = None self._enabled = False self._image_sub = Image_Subscriber("image", callback=self._img_cb) if rospy.get_param("~autostart", default=False): self._enable() else: self._disable() self._enable_srv = rospy.Service("~enable", SetBool, self._enable_cb) def _enable_cb(self, req): if req.data and not self._enabled: self._enable() elif not req.data and self._enabled: self._disable() return {'success': True} def _enable(self): if self._camera_info is None: self._camera_info = self._image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self._camera_info) self._enabled = True rospy.loginfo("Enabled.") def _disable(self): self._enabled = False rospy.loginfo("Disabled.") def _img_cb(self, img): if not self._enabled: return msg = ObjectsInImage() msg.header = self._image_sub.last_image_header msg.objects = self.find_objects(img) if not isinstance(msg.objects, list) or (len(msg.objects) and not isinstance(msg.objects[0], ObjectInImage)): rospy.logwarn("find_objects did not return a list of mil_msgs/ObjectInImage message. Ignoring.") self._objects_pub.publish(msg) @abc.abstractmethod def find_objects(self, img): pass
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 MultiObs: def __init__(self): # Pull constants from config file self.min_trans = rospy.get_param('~min_trans', .25) self.max_velocity = rospy.get_param('~max_velocity', 1) self.min_observations = rospy.get_param('~min_observations', 30) 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._observations1 = deque() self._pose_pairs1 = deque() self._times1 = deque() self._observations2 = deque() self._pose_pairs2 = deque() self._times2 = deque() self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.est1 = None self.est2 = None self.plane = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() self.print_info = FprintFactory(title=MISSION).fprint # Image Subscriber and Camera Information self.point_sub = rospy.Subscriber( '/roi_pub', RegionOfInterest, self.acquire_targets) self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = 'front_left_cam_optical' self.markerPub0 = rospy.Publisher('estMarker0', Marker, queue_size=1) self.markerPub1 = rospy.Publisher('estMarker1', Marker, queue_size=1) self.markerPub2 = rospy.Publisher('estMarker2', Marker, queue_size=1) # 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) # 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. ''' return None 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: print("REEEE") return VisionRequestResponse(found=False) #buoy = self.buoys[srv.target_name] if self.est is None: self.print_info("NO ESTIMATE!") return VisionRequestResponse(found=False) # NOTE: returns normal vec encoded into a quaternion message (so not actually a quaternion) return VisionRequestResponse( pose=PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='/map'), pose=Pose(position=Point(*((self.est + self.est1 + self.est2) / 3)), orientation=Quaternion(x=self.plane[0], y=self.plane[1], z=self.plane[2]))), found=True) def clear_old_observations(self): # Observations older than three seconds are discarded. # print(self._observations) # print(self._observations1) # print(self._observations2) return def size(self): return len(self._observations) def add_observation(self, obs, pose_pair, time): if len(self._pose_pairs) == 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 add_observation1(self, obs, pose_pair, time): if len(self._pose_pairs1) == 0 or np.linalg.norm( self._pose_pairs1[-1][0] - pose_pair[0]) > self.min_trans: self._observations1.append(obs) self._pose_pairs1.append(pose_pair) self._times1.append(time) return (self._observations1, self._pose_pairs1) def add_observation2(self, obs, pose_pair, time): self.clear_old_observations() if len(self._pose_pairs2) == 0 or np.linalg.norm( self._pose_pairs2[-1][0] - pose_pair[0]) > self.min_trans: self._observations2.append(obs) self._pose_pairs2.append(pose_pair) self._times2.append(time) return (self._observations2, self._pose_pairs2) def get_observations_and_pose_pairs(self): self.clear_old_observations() return (self._observations, self._pose_pairs) def generate_plane(self): ab = self.est - self.est1 ac = self.est - self.est2 # print("AB: ", ab) # print("AC: ", ac) x = np.cross(ab,ac) return x / np.linalg.norm(x) def marker_msg(self, point, point_name): robotMarker = Marker() robotMarker.header.frame_id = "/map" robotMarker.header.stamp = rospy.get_rostime() robotMarker.ns = point_name robotMarker.id = 0 robotMarker.type = 2 # sphere robotMarker.action = 0 robotMarker.pose.position = Point(point[0], point[1], point[2]) robotMarker.pose.orientation.x = 0 robotMarker.pose.orientation.y = 0 robotMarker.pose.orientation.z = 0 robotMarker.pose.orientation.w = 1.0 robotMarker.scale.x = .5 robotMarker.scale.y = .5 robotMarker.scale.z = .5 robotMarker.color.r = 1.0 robotMarker.color.g = 1.0 robotMarker.color.b = 0.0 robotMarker.color.a = 1.0 robotMarker.lifetime = rospy.Duration(0) return robotMarker def acquire_targets(self, roi): if not self.enabled: return # NOTE: point.z contains the timestamp of the image when it was processed in the neural net. x0 = roi.x_offset y0 = roi.y_offset height = roi.height width = roi.width point0 = np.array([x0, y0]) point1 = np.array([x0+width, y0]) point2 = np.array([x0, y0+height]) point3 = np.array([x0+width, y0+height]) # print("p1: ", point1) # print("p2: ", point2) try: self.tf_listener.waitForTransform('/map', '/front_left_cam_optical', self.image_sub.last_image_time - rospy.Time(.2), 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', '/front_left_cam_optical', self.image_sub.last_image_time - rospy.Time(.2)) R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q) self.add_observation(point0, (np.array(t), R), self.image_sub.last_image_time) observations1, pose_pairs1 = self.add_observation1(point1, (np.array(t), R), self.image_sub.last_image_time) observations2, pose_pairs2 = self.add_observation2(point2, (np.array(t), R), self.image_sub.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.est1 = self.multi_obs.lst_sqr_intersection( observations1, pose_pairs1) self.est2 = self.multi_obs.lst_sqr_intersection( observations2, pose_pairs2) # print('est1: ', self.est1) # print('est2: ', self.est2) self.status = 'Pose found' self.print_info("Pose Found!") self.plane = self.generate_plane() # print("Plane: ", self.plane) self.markerPub0.publish(self.marker_msg(self.est, 'est0')) self.markerPub1.publish(self.marker_msg(self.est1, 'est1')) self.markerPub2.publish(self.marker_msg(self.est2, 'est2')) print((self.est + self.est1 + self.est2) / 3) else: self.status = '{} observations'.format(len(observations))
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 ScanTheCodePerception(object): def __init__(self): self.enabled = False self.pattern_pub = rospy.Publisher("/scan_the_code", ScanTheCode, queue_size=3) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.get_params() self.last_panel_points_msg = None self.sub = Image_Subscriber(self.image_topic, self.img_cb) info = self.sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(info) if self.debug: self.image_mux = ImageMux(size=(info.height, info.width), shape=(1, 2), labels=['Result', 'Mask']) self.debug_pub = Image_Publisher('~debug_image') self.bbox_sub = rospy.Subscriber("stc_led_pts_marshall", PointCloud2, self.panel_points_cb) self.classification_list = deque() self.enabled = True def get_params(self): ''' Set several constants used for image processing and classification from ROS params for runtime configurability. ''' self.debug = rospy.get_param('~debug', True) self.image_topic = rospy.get_param('~image_topic', '/camera/starboard/image_rect_color') self.classifier = ScanTheCodeClassifier() self.classifier.train_from_csv() def panel_points_cb(self, pc2): rospy.loginfo_throttle(10., 'BBox received') self.last_panel_points_msg = pc2 def get_panel_bbox(self): if self.last_panel_points_msg is None: return None try: transform = self.tf_buffer.lookup_transform_full( self.sub.last_image_header.frame_id, self.sub.last_image_header.stamp, self.last_panel_points_msg.header.frame_id, self.last_panel_points_msg.header.stamp, "enu", timeout=rospy.Duration(1)) except tf2_ros.TransformException as e: rospy.logwarn(e) return None transformed_cloud = do_transform_cloud(self.last_panel_points_msg, transform) points = np.array(list(sensor_msgs.point_cloud2.read_points(transformed_cloud, skip_nans=True))) if len(points) < 4: rospy.logwarn('less than 4 points') return None if self.camera_model is None: rospy.logwarn('no camera model') return None roi = roi_enclosing_points(self.camera_model, points) if roi is None: rospy.logwarn('No points project into camera.') return None rect = rect_from_roi(roi) ul, br = rect xmin, ymin = ul xmax, ymax = br x_shrink_pixels = 15 y_shrink_pixels = x_shrink_pixels xmin += x_shrink_pixels xmax -= x_shrink_pixels ymin += y_shrink_pixels ymax -= y_shrink_pixels new_rect = ((xmin, ymin), (xmax, ymax)) bbox_contour = bbox_countour_from_rectangle(new_rect) return bbox_contour def img_cb(self, img): if not self.enabled: return if self.camera_model is None: return bbox = self.get_panel_bbox() if bbox is not None: bbox = np.array(bbox, dtype=int) debug = contour_mask(bbox, img_shape=img.shape) prediction = self.classifier.classify(img, debug)[0] label = self.classifier.CLASSES[prediction] symbol = label[10] if len(self.classification_list) == 0 or self.classification_list[-1] != symbol: if len(self.classification_list) >= 5: self.classification_list.popleft() self.classification_list.append(symbol) text = label + ' | ' + ''.join(self.classification_list) scale = 3 thickness = 2 putText_ul(debug, text, (0, 0), fontScale=scale, thickness=thickness) rospy.loginfo('saw {}, running {}'.format(symbol, text)) debug = cv2.bitwise_or(img, img, mask=debug) self.image_mux[1] = debug if len(self.classification_list) == 5 and self.classification_list[0] == 'o' \ and self.classification_list[4] == 'o' and self.classification_list[1] != 'o' \ and self.classification_list[2] != 'o' and self.classification_list[3] != 'o': pattern = \ (self.classification_list[1] + self.classification_list[2] + self.classification_list[3]).upper() rospy.loginfo('SAW PATTERN {}!!!!!!!!!!!'.format(pattern)) self.pattern_pub.publish(ScanTheCode(color_pattern=pattern)) self.image_mux[0] = img if self.debug: self.debug_pub.publish(self.image_mux())
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)
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 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 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 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 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 VrxClassifier(object): # Handle buoys / black totem specially, discrminating on volume as they have the same color # The black objects that we have trained the color classifier on BLACK_OBJECT_CLASSES = ['buoy', 'black_totem'] # All the black objects in VRX POSSIBLE_BLACK_OBJECTS = ['polyform_a3', 'polyform_a5', 'polyform_a7'] # The average perceceived PCODAR volume of each above object BLACK_OBJECT_VOLUMES = [0.3, 0.6, 1.9] BLACK_OBJECT_AREA = [0., 0.5, 0., 0.] TOTEM_MIN_HEIGHT = 0.9 def __init__(self): self.enabled = False # Maps ID to running class probabilities self.object_map = {} # Maps ID to mean volume, used to discriminate buoys / black totem self.volume_means = {} self.area_means = {} self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.get_params() self.last_panel_points_msg = None self.database_client = rospy.ServiceProxy('/database/requests', ObjectDBQuery) self.sub = Image_Subscriber(self.image_topic, self.img_cb) self.camera_info = self.sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) if self.debug: self.image_mux = ImageMux(size=(self.camera_info.height, self.camera_info.width), shape=(1, 2), labels=['Result', 'Mask']) self.debug_pub = Image_Publisher('~debug_image') self.last_objects = None self.last_update_time = rospy.Time.now() self.objects_sub = rospy.Subscriber('/pcodar/objects', PerceptionObjectArray, self.process_objects, queue_size=2) self.enabled_srv = rospy.Service('~set_enabled', SetBool, self.set_enable_srv) if self.is_training: self.enabled = True @thread_lock(lock) def set_enable_srv(self, req): self.enabled = req.data return {'success': True} def in_frame(self, pixel): # TODO: < or <= ??? return pixel[0] > 0 and pixel[0] < self.camera_info.width and pixel[ 1] > 0 and pixel[1] < self.camera_info.height @thread_lock(lock) def process_objects(self, msg): self.last_objects = msg def get_params(self): ''' Set several constants used for image processing and classification from ROS params for runtime configurability. ''' self.is_training = rospy.get_param('~train', False) self.debug = rospy.get_param('~debug', True) self.image_topic = rospy.get_param( '~image_topic', '/camera/starboard/image_rect_color') self.update_period = rospy.Duration(1.0 / rospy.get_param('~update_hz', 1)) self.classifier = VrxColorClassifier() self.classifier.train_from_csv() def get_box_roi(self, corners): roi = roi_enclosing_points(self.camera_model, corners, border=(-10, 0)) if roi is None: rospy.logwarn('No points project into camera.') return None rect = rect_from_roi(roi) bbox_contour = bbox_countour_from_rectangle(rect) return bbox_contour def get_bbox(self, p, q_mat, obj_msg): points = np.zeros((len(obj_msg.points), 3), dtype=np.float) for i in range(len(obj_msg.points)): points[i, :] = p + q_mat.dot(rosmsg_to_numpy(obj_msg.points[i])) return points def get_object_roi(self, p, q_mat, obj_msg): box_corners = self.get_bbox(p, q_mat, obj_msg) return self.get_box_roi(box_corners) def update_object(self, object_msg, class_probabilities): object_id = object_msg.id # Update the total class probabilities if object_id in self.object_map: self.object_map[object_id] += class_probabilities else: self.object_map[object_id] = class_probabilities total_probabilities = self.object_map[object_id] # Guess the type of object based most_likely_index = np.argmax(total_probabilities) most_likely_name = self.classifier.CLASSES[most_likely_index] # Unforuntely this doesn't really work' if most_likely_name in self.BLACK_OBJECT_CLASSES: object_scale = rosmsg_to_numpy(object_msg.scale) object_volume = object_scale.dot(object_scale) object_area = object_scale[:2].dot(object_scale[:2]) height = object_scale[2] if object_id in self.volume_means: self.volume_means[object_id].add_value(object_volume) self.area_means[object_id].add_value(object_area) else: self.volume_means[object_id] = RunningMean(object_volume) self.area_means[object_id] = RunningMean(object_area) running_mean_volume = self.volume_means[object_id].mean running_mean_area = self.area_means[object_id].mean if height > self.TOTEM_MIN_HEIGHT: black_guess = 'black_totem' else: black_guess_index = np.argmin( np.abs(self.BLACK_OBJECT_VOLUMES - running_mean_volume)) black_guess = self.POSSIBLE_BLACK_OBJECTS[black_guess_index] most_likely_name = black_guess rospy.loginfo( '{} current/running volume={}/{} area={}/{} height={}-> {}'. format(object_id, object_volume, running_mean_volume, object_area, running_mean_area, height, black_guess)) obj_title = object_msg.labeled_classification probability = class_probabilities[most_likely_index] rospy.loginfo('Object {} {} classified as {} ({}%)'.format( object_id, object_msg.labeled_classification, most_likely_name, probability * 100.)) if obj_title != most_likely_name: cmd = '{}={}'.format(object_id, most_likely_name) rospy.loginfo('Updating object {} to {}'.format( object_id, most_likely_name)) if not self.is_training: self.database_client(ObjectDBQueryRequest(cmd=cmd)) return most_likely_name @thread_lock(lock) def img_cb(self, img): if not self.enabled: return if self.camera_model is None: return if self.last_objects is None or len(self.last_objects.objects) == 0: return now = rospy.Time.now() if now - self.last_update_time < self.update_period: return self.last_update_time = now # Get Transform from ENU to optical at the time of this image transform = self.tf_buffer.lookup_transform( self.sub.last_image_header.frame_id, "enu", self.sub.last_image_header.stamp, timeout=rospy.Duration(1)) translation = rosmsg_to_numpy(transform.transform.translation) rotation = rosmsg_to_numpy(transform.transform.rotation) rotation_mat = quaternion_matrix(rotation)[:3, :3] # Transform the center of each object into optical frame positions_camera = [ translation + rotation_mat.dot(rosmsg_to_numpy(obj.pose.position)) for obj in self.last_objects.objects ] pixel_centers = [ self.camera_model.project3dToPixel(point) for point in positions_camera ] distances = np.linalg.norm(positions_camera, axis=1) CUTOFF_METERS = 15 # Get a list of indicies of objects who are sufficiently close and can be seen by camera met_criteria = [] for i in xrange(len(self.last_objects.objects)): distance = distances[i] if self.in_frame( pixel_centers[i] ) and distance < CUTOFF_METERS and positions_camera[i][2] > 0: met_criteria.append(i) # print 'Keeping {} of {}'.format(len(met_criteria), len(self.last_objects.objects)) rois = [ self.get_object_roi(translation, rotation_mat, self.last_objects.objects[i]) for i in met_criteria ] debug = np.zeros(img.shape, dtype=img.dtype) if self.is_training: training = [] for i in xrange(len(rois)): # The index in self.last_objects that this object is index = met_criteria[i] # The actual message object we are looking at object_msg = self.last_objects.objects[index] object_id = object_msg.id # Exit early if we could not get a valid roi if rois[i] is None: rospy.logwarn( 'Object {} had no points, skipping'.format(object_id)) continue # Form a contour from the ROI contour = np.array(rois[i], dtype=int) # Create image mask from the contour mask = contour_mask(contour, img_shape=img.shape) # get the color mean features features = np.array(self.classifier.get_features(img, mask)).reshape( 1, 9) # Predict class probabilites based on color means class_probabilities = self.classifier.feature_probabilities( features)[0] # Use this and previous probabilities to guess at which object this is guess = self.update_object(object_msg, class_probabilities) # If training, save this if self.is_training and obj_title != 'UNKNOWN': classification_index = self.classifier.CLASSES.index(obj_title) training.append(np.append(classification_index, features)) # Draw debug info colorful = cv2.bitwise_or(img, img, mask=mask) debug = cv2.bitwise_or(debug, colorful) scale = 3 thickness = 2 center = np.array(pixel_centers[index], dtype=int) text = str(object_id) putText_ul(debug, text, center, fontScale=scale, thickness=thickness) if self.is_training and len(training) != 0: training = np.array(training) try: previous_data = pandas.DataFrame.from_csv( self.classifier.training_file).values data = np.vstack((previous_data, training)) except Exception as e: data = training self.classifier.save_csv(data[:, 1:], data[:, 0]) rospy.signal_shutdown('fdfd') raise Exception('did something, kev') self.image_mux[0] = img self.image_mux[1] = debug self.debug_pub.publish(self.image_mux()) return
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 VrxClassifier(object): # Handle buoys / black totem specially, discrminating on volume as they have the same color # The black objects that we have trained the color classifier on BLACK_OBJECT_CLASSES = ['buoy', 'black_totem'] # All the black objects in VRX POSSIBLE_BLACK_OBJECTS = ['polyform_a3', 'polyform_a5', 'polyform_a7'] # The average perceceived PCODAR volume of each above object BLACK_OBJECT_VOLUMES = [0.3, 0.6, 1.9] BLACK_OBJECT_AREA = [0., 0.5, 0., 0.] TOTEM_MIN_HEIGHT = 0.9 CLASSES = [ "mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange" ] Votes = {} def __init__(self): self.enabled = False # Maps ID to running class probabilities self.object_map = {} # Maps ID to mean volume, used to discriminate buoys / black totem self.volume_means = {} self.area_means = {} self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.get_params() self.last_panel_points_msg = None self.database_client = rospy.ServiceProxy('/database/requests', ObjectDBQuery) self.task_info_sub = rospy.Subscriber("/vrx/task/info", Task, self.taskinfoSubscriber) self.is_perception_task = False self.sub = Image_Subscriber(self.image_topic, self.image_cb) self.camera_info = self.sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) if self.debug: self.image_mux = ImageMux(size=(self.camera_info.height, self.camera_info.width), shape=(1, 2), labels=['Result', 'Mask']) self.debug_pub = Image_Publisher('~debug_image') self.last_objects = None self.last_update_time = rospy.Time.now() self.objects_sub = rospy.Subscriber('/pcodar/objects', PerceptionObjectArray, self.process_objects, queue_size=2) self.boxes_sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.process_boxes) self.enabled_srv = rospy.Service('~set_enabled', SetBool, self.set_enable_srv) self.last_image = None if self.is_training: self.enabled = True self.queue = [] @thread_lock(lock) def set_enable_srv(self, req): self.enabled = req.data return {'success': True} def image_cb(self, msg): self.last_image = msg return def taskinfoSubscriber(self, msg): self.is_perception_task = msg.name == "perception" def in_frame(self, pixel): # TODO: < or <= ??? return pixel[0] > 0 and pixel[0] < self.camera_info.width and pixel[ 1] > 0 and pixel[1] < self.camera_info.height @thread_lock(lock) def process_objects(self, msg): self.last_objects = msg def in_rect(self, point, bbox): if point[0] >= bbox.xmin and point[1] >= bbox.ymin and point[ 0] <= bbox.xmax and point[1] <= bbox.ymax: return True else: return False def distance(self, first, second): x_diff = second[0] - first[0] y_diff = second[1] - first[1] return math.sqrt(x_diff * x_diff + y_diff * y_diff) @thread_lock(lock) def process_boxes(self, msg): if not self.enabled: return if self.camera_model is None: return if self.last_objects is None or len(self.last_objects.objects) == 0: return now = rospy.Time.now() if now - self.last_update_time < self.update_period: return self.last_update_time = now # Get Transform from ENU to optical at the time of this image transform = self.tf_buffer.lookup_transform( self.sub.last_image_header.frame_id, "enu", self.sub.last_image_header.stamp, timeout=rospy.Duration(1)) translation = rosmsg_to_numpy(transform.transform.translation) rotation = rosmsg_to_numpy(transform.transform.rotation) rotation_mat = quaternion_matrix(rotation)[:3, :3] # Transform the center of each object into optical frame positions_camera = [ translation + rotation_mat.dot(rosmsg_to_numpy(obj.pose.position)) for obj in self.last_objects.objects ] pixel_centers = [ self.camera_model.project3dToPixel(point) for point in positions_camera ] distances = np.linalg.norm(positions_camera, axis=1) CUTOFF_METERS = 30 if self.is_perception_task: CUTOFF_METERS = 100 # Get a list of indicies of objects who are sufficiently close and can be seen by camera met_criteria = [] for i in xrange(len(self.last_objects.objects)): distance = distances[i] if self.in_frame( pixel_centers[i] ) and distance < CUTOFF_METERS and positions_camera[i][2] > 0: met_criteria.append(i) # print 'Keeping {} of {}'.format(len(met_criteria), len(self.last_objects.objects)) classified = set() #for each bounding box,check which buoy is closest to boat within pixel range of bounding box for a in msg.bounding_boxes: buoys = [] for i in met_criteria: if self.in_rect(pixel_centers[i], a): buoys.append(i) if len(buoys) > 0: closest_to_box = buoys[0] closest_to_boat = buoys[0] for i in buoys[1:]: if distances[i] < distances[closest_to_boat]: closest_to_box = i closest_to_boat = i classified.add(self.last_objects.objects[closest_to_box].id) print('Object {} classified as {}'.format( self.last_objects.objects[closest_to_box].id, a.Class)) cmd = '{}={}'.format( self.last_objects.objects[closest_to_box].id, a.Class) self.database_client(ObjectDBQueryRequest(cmd=cmd)) if not self.is_perception_task: return for a in met_criteria: if self.last_objects.objects[a].id in classified: continue height = self.last_objects.objects[a].scale.z #if pixel_centers[i][0] > 1280 or pixel_centers[i][0] > 720: # return if height > 0.45: print('Reclassified as white') print('Object {} classified as {}'.format( self.last_objects.objects[a].id, "mb_marker_buoy_white")) cmd = '{}={}'.format(self.last_objects.objects[a].id, "mb_marker_buoy_white") self.database_client(ObjectDBQueryRequest(cmd=cmd)) else: print('Object {} classified as {}'.format( self.last_objects.objects[a].id, "mb_round_buoy_black")) cmd = '{}={}'.format(self.last_objects.objects[a].id, "mb_round_buoy_black") self.database_client(ObjectDBQueryRequest(cmd=cmd)) def get_params(self): ''' Set several constants used for image processing and classification from ROS params for runtime configurability. ''' self.is_training = rospy.get_param('~train', False) self.debug = rospy.get_param('~debug', True) self.image_topic = rospy.get_param( '~image_topic', '/camera/starboard/image_rect_color') self.model_loc = rospy.get_param('~model_location', 'config/model') self.update_period = rospy.Duration(1.0 / rospy.get_param('~update_hz', 5)) def get_box_roi(self, corners): roi = roi_enclosing_points(self.camera_model, corners, border=(-10, 0)) if roi is None: rospy.logwarn('No points project into camera.') return None rect = rect_from_roi(roi) bbox_contour = bbox_countour_from_rectangle(rect) return bbox_contour def get_bbox(self, p, q_mat, obj_msg): points = np.zeros((len(obj_msg.points), 3), dtype=np.float) for i in range(len(obj_msg.points)): points[i, :] = p + q_mat.dot(rosmsg_to_numpy(obj_msg.points[i])) return points def get_object_roi(self, p, q_mat, obj_msg): box_corners = self.get_bbox(p, q_mat, obj_msg) return self.get_box_roi(box_corners)