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
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 __init__(self):
self._objects_pub = rospy.Publisher("~identified_objects", ObjectsInImage, queue_size=3)
self._camera_info = 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 __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)
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 __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 __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 __init__(self):
rospy.sleep(1.0)
self.last_image = None
self.last_draw_image = None
self.last_image_time = None
self.camera_model = None
self.white_list = None
self.ocr_service = rospy.Service('vision/ocr', OcrRequest,
self.request_characters)
self.image_sub = Image_Subscriber(
'/camera/front/right/image_rect_color', self.image_cb)
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 __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)
self.shape_match_thresh = rospy.get_param("shape_match_thresh", 0.4)
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)
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
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 __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 __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 __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)
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 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 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 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 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 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)
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)
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 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 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 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 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)