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.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 image_cb(self, image):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
self.multi_obs = MultiObservation(self.camera_model)
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):
# 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):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
self.multi_obs = MultiObservation(self.camera_model)
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")
class BuoyFinder(object):
'''
Node to find red, green, and yellow buoys in a single camera frame.
Combines several observations and uses a least-squares approach to get a 3D
position estimate of a buoy when requested.
Intended to be modular so other approaches can be tried. Adding more sophistication
to segmentation would increase reliability.
TODO: Use same mask for yellow/green
'''
def __init__(self):
self.tf_listener = tf.TransformListener()
self.enabled = False
self.last_image = None
self.last_image_time = None
self.camera_model = None
self.circle_finder = CircleFinder(
1.0
) # Model radius doesn't matter beacause it's not being used for 3D pose
# Various constants for tuning, debugging. See buoy_finder.yaml for more info
self.min_observations = rospy.get_param('~min_observations')
self.debug_ros = rospy.get_param('~debug/ros', True)
self.debug_cv = rospy.get_param('~debug/cv', False)
self.min_contour_area = rospy.get_param('~min_contour_area')
self.max_circle_error = rospy.get_param('~max_circle_error')
self.max_velocity = rospy.get_param('~max_velocity')
self.roi_y = rospy.get_param('~roi_y')
self.roi_height = rospy.get_param('~roi_height')
camera = rospy.get_param('~camera_topic',
'/camera/front/right/image_rect_color')
self.buoys = {}
for color in ['red', 'yellow', 'green']:
self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
if self.debug_cv:
cv2.waitKey(1)
self.debug_images = {}
# Keep latest odom message for sanity check
self.last_odom = None
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=3)
self.image_sub = Image_Subscriber(camera, self.image_cb)
if self.debug_ros:
self.rviz = rviz.RvizVisualizer(topic='~markers')
self.mask_pub = Image_Publisher('~mask_image')
rospy.Timer(rospy.Duration(1), self.print_status)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~enable', SetBool, self.toggle_search)
rospy.Service('~2D', VisionRequest2D, self.request_buoy)
rospy.Service('~pose', VisionRequest, self.request_buoy3d)
rospy.loginfo("BUOY FINDER: initialized successfully")
def odom_cb(self, odom):
self.last_odom = odom
def toggle_search(self, srv):
'''
Callback for standard ~enable service. If true, start
looking at frames for buoys.
'''
if srv.data:
rospy.loginfo("BUOY FINDER: enabled")
self.enabled = True
else:
rospy.loginfo("BUOY FINDER: disabled")
self.enabled = False
return SetBoolResponse(success=True)
def request_buoy(self, srv):
'''
Callback for 2D vision request. Returns centroid
of buoy found with color specified in target_name
if found.
'''
if not self.enabled or srv.target_name not in self.buoys:
return VisionRequest2DResponse(found=False)
response = self.find_single_buoy(srv.target_name)
if response is False or response is None:
return VisionRequest2DResponse(found=False)
center, radius = response
return VisionRequest2DResponse(header=Header(
stamp=self.last_image_time, frame_id=self.frame_id),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True)
def request_buoy3d(self, srv):
'''
Callback for 3D vision request. Uses recent observations of buoy
specified in target_name to attempt a least-squares position estimate.
As buoys are spheres, orientation is meaningless.
'''
if srv.target_name not in self.buoys or not self.enabled:
return VisionRequestResponse(found=False)
buoy = self.buoys[srv.target_name]
if buoy.est is None:
return VisionRequestResponse(found=False)
return VisionRequestResponse(pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(position=Point(*buoy.est))),
found=True)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find each color buoy.
'''
if not self.enabled:
return
# Crop out some of the top and bottom to exclude the floor and surface reflections
height = image.shape[0]
roi_y = int(self.roi_y * height)
roi_height = height - int(self.roi_height * height)
self.roi = (0, roi_y, roi_height, image.shape[1])
self.last_image = image[self.roi[1]:self.roi[2],
self.roi[0]:self.roi[3]]
self.image_area = self.last_image.shape[0] * self.last_image.shape[1]
if self.debug_ros:
# Create a blacked out debug image for putting masks in
self.mask_image = np.zeros(self.last_image.shape,
dtype=image.dtype)
if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time:
# Clear tf buffer if time went backwards (nice for playing bags in loop)
self.tf_listener.clear()
self.last_image_time = self.image_sub.last_image_time
self.find_buoys()
if self.debug_ros:
self.mask_pub.publish(self.mask_image)
def print_status(self, _):
'''
Called at 1 second intervals to display the status (not found, n observations, FOUND)
for each buoy.
'''
if self.enabled:
rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'",
self.buoys['red'].status, self.buoys['green'].status,
self.buoys['yellow'].status)
def find_buoys(self):
'''
Run find_single_buoy for each color of buoy
'''
for buoy_name in self.buoys:
self.find_single_buoy(buoy_name)
def is_circular_contour(self, cnt):
'''
Check that a contour is close enough to a circle (using hue invariants)
to be a pottential buoy.
'''
return self.circle_finder.verify_contour(cnt) <= self.max_circle_error
def get_best_contour(self, contours):
'''
Attempts to find a good buoy contour among those found within the
thresholded mask. If a good one is found, it return (contour, centroid, area),
otherwise returns None. Right now the best contour is just the largest.
@param contours Numpy array of contours from a particular buoy's mask
@return A tuple (contour, error) where contour will be the best contour
in an image or None if no contours pass criteria. Error will be a string
describing why no good contour was found, or None if contour is not None.
'''
if len(contours) == 0:
return None, 'no contours in mask'
circular_contours = filter(self.is_circular_contour, contours)
if len(circular_contours) == 0:
return None, 'fails circularity test'
circles_sorted = sorted(circular_contours,
key=cv2.contourArea,
reverse=True)
if cv2.contourArea(
circles_sorted[0]) < self.min_contour_area * self.image_area:
return None, 'fails area test'
return circles_sorted[
0], None # Return the largest contour that pases shape test
def find_single_buoy(self, buoy_type):
'''
Attempt to find one color buoy in the image.
1) Create mask for buoy's color in colorspace specified in paramaters
2) Select the largest contour in this mask
3) Approximate a circle around this contour
4) Store the center of this circle and the current tf between /map and camera
as an observation
5) If observations for this buoy is now >= min_observations, approximate buoy
position using the least squares tool imported
'''
assert buoy_type in self.buoys.keys(
), "Buoys_2d does not know buoy color: {}".format(buoy_type)
buoy = self.buoys[buoy_type]
mask = buoy.get_mask(self.last_image)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.erode(mask, kernel, iterations=2)
mask = cv2.dilate(mask, kernel, iterations=2)
_, contours, _ = cv2.findContours(mask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE,
offset=(self.roi[0], self.roi[1]))
if self.debug_ros:
cv2.add(self.mask_image.copy(),
buoy.cv_colors,
mask=mask,
dst=self.mask_image)
if self.debug_cv:
self.debug_images[buoy_type] = mask.copy()
cnt, err = self.get_best_contour(contours)
if cnt is None:
buoy.clear_old_observations()
buoy.status = '{} w/ {} obs'.format(err, buoy.size())
return
center, radius = cv2.minEnclosingCircle(cnt)
if self.debug_ros:
cv2.circle(
self.mask_image,
(int(center[0] - self.roi[0]), int(center[1]) - self.roi[1]),
int(radius), buoy.cv_colors, 4)
try:
self.tf_listener.waitForTransform('/map', self.frame_id,
self.last_image_time,
rospy.Duration(0.2))
except tf.Exception as e:
rospy.logwarn("Could not transform camera to map: {}".format(e))
return False
if not self.sanity_check(center, self.last_image_time):
buoy.status = 'failed sanity check'
return False
(t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id,
self.last_image_time)
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
buoy.add_observation(center, (np.array(t), R), self.last_image_time)
observations, pose_pairs = buoy.get_observations_and_pose_pairs()
if len(observations) > self.min_observations:
buoy.est = self.multi_obs.lst_sqr_intersection(
observations, pose_pairs)
buoy.status = 'Pose found'
if self.debug_ros:
self.rviz.draw_sphere(buoy.est,
color=buoy.draw_colors,
scaling=(0.2286, 0.2286, 0.2286),
frame='/map',
_id=buoy.visual_id)
else:
buoy.status = '{} observations'.format(len(observations))
return center, radius
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
if self.last_odom is None:
return False
linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
if np.linalg.norm(linear_velocity) > self.max_velocity:
return False
return True
class MultiObs:
def __init__(self):
# Pull constants from config file
self.min_trans = rospy.get_param('~min_trans', .25)
self.max_velocity = rospy.get_param('~max_velocity', 1)
self.min_observations = rospy.get_param('~min_observations', 30)
self.camera = rospy.get_param('~camera_topic',
'/camera/front/left/image_rect_color')
# Instantiate remaining variables and objects
self._observations = deque()
self._pose_pairs = deque()
self._times = deque()
self._observations1 = deque()
self._pose_pairs1 = deque()
self._times1 = deque()
self._observations2 = deque()
self._pose_pairs2 = deque()
self._times2 = deque()
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.est1 = None
self.est2 = None
self.plane = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
self.print_info = FprintFactory(title=MISSION).fprint
# Image Subscriber and Camera Information
self.point_sub = rospy.Subscriber(
'/roi_pub', RegionOfInterest, self.acquire_targets)
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = 'front_left_cam_optical'
self.markerPub0 = rospy.Publisher('estMarker0', Marker, queue_size=1)
self.markerPub1 = rospy.Publisher('estMarker1', Marker, queue_size=1)
self.markerPub2 = rospy.Publisher('estMarker2', Marker, queue_size=1)
# Ros Services so mission can be toggled and info requested
rospy.Service('~enable', SetBool, self.toggle_search)
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~pose', VisionRequest, self.request_board3d)
# Debug
self.debug = rospy.get_param('~debug', True)
def image_cb(self, image):
'''
Run each time an image comes in from ROS. If enabled,
attempt to find the torpedo board.
'''
return None
def toggle_search(self, srv):
'''
Callback for standard ~enable service. If true, start
looking at frames for buoys.
'''
if srv.data:
rospy.loginfo("TARGET ACQUISITION: enabled")
self.enabled = True
else:
rospy.loginfo("TARGET ACQUISITION: disabled")
self.enabled = False
return SetBoolResponse(success=True)
def request_board3d(self, srv):
'''
Callback for 3D vision request. Uses recent observations of target
board specified in target_name to attempt a least-squares position
estimate. Ignoring orientation of board.
'''
if not self.enabled:
print("REEEE")
return VisionRequestResponse(found=False)
#buoy = self.buoys[srv.target_name]
if self.est is None:
self.print_info("NO ESTIMATE!")
return VisionRequestResponse(found=False)
# NOTE: returns normal vec encoded into a quaternion message (so not actually a quaternion)
return VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='/map'),
pose=Pose(position=Point(*((self.est + self.est1 + self.est2) / 3)),
orientation=Quaternion(x=self.plane[0], y=self.plane[1], z=self.plane[2]))),
found=True)
def clear_old_observations(self):
# Observations older than three seconds are discarded.
# print(self._observations)
# print(self._observations1)
# print(self._observations2)
return
def size(self):
return len(self._observations)
def add_observation(self, obs, pose_pair, time):
if len(self._pose_pairs) == 0 or np.linalg.norm(
self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans:
self._observations.append(obs)
self._pose_pairs.append(pose_pair)
self._times.append(time)
def add_observation1(self, obs, pose_pair, time):
if len(self._pose_pairs1) == 0 or np.linalg.norm(
self._pose_pairs1[-1][0] - pose_pair[0]) > self.min_trans:
self._observations1.append(obs)
self._pose_pairs1.append(pose_pair)
self._times1.append(time)
return (self._observations1, self._pose_pairs1)
def add_observation2(self, obs, pose_pair, time):
self.clear_old_observations()
if len(self._pose_pairs2) == 0 or np.linalg.norm(
self._pose_pairs2[-1][0] - pose_pair[0]) > self.min_trans:
self._observations2.append(obs)
self._pose_pairs2.append(pose_pair)
self._times2.append(time)
return (self._observations2, self._pose_pairs2)
def get_observations_and_pose_pairs(self):
self.clear_old_observations()
return (self._observations, self._pose_pairs)
def generate_plane(self):
ab = self.est - self.est1
ac = self.est - self.est2
# print("AB: ", ab)
# print("AC: ", ac)
x = np.cross(ab,ac)
return x / np.linalg.norm(x)
def marker_msg(self, point, point_name):
robotMarker = Marker()
robotMarker.header.frame_id = "/map"
robotMarker.header.stamp = rospy.get_rostime()
robotMarker.ns = point_name
robotMarker.id = 0
robotMarker.type = 2 # sphere
robotMarker.action = 0
robotMarker.pose.position = Point(point[0], point[1], point[2])
robotMarker.pose.orientation.x = 0
robotMarker.pose.orientation.y = 0
robotMarker.pose.orientation.z = 0
robotMarker.pose.orientation.w = 1.0
robotMarker.scale.x = .5
robotMarker.scale.y = .5
robotMarker.scale.z = .5
robotMarker.color.r = 1.0
robotMarker.color.g = 1.0
robotMarker.color.b = 0.0
robotMarker.color.a = 1.0
robotMarker.lifetime = rospy.Duration(0)
return robotMarker
def acquire_targets(self, roi):
if not self.enabled:
return
# NOTE: point.z contains the timestamp of the image when it was processed in the neural net.
x0 = roi.x_offset
y0 = roi.y_offset
height = roi.height
width = roi.width
point0 = np.array([x0, y0])
point1 = np.array([x0+width, y0])
point2 = np.array([x0, y0+height])
point3 = np.array([x0+width, y0+height])
# print("p1: ", point1)
# print("p2: ", point2)
try:
self.tf_listener.waitForTransform('/map',
'/front_left_cam_optical',
self.image_sub.last_image_time - rospy.Time(.2),
rospy.Duration(0.2))
except tf.Exception as e:
rospy.logwarn(
"Could not transform camera to map: {}".format(e))
return False
(t, rot_q) = self.tf_listener.lookupTransform(
'/map', '/front_left_cam_optical', self.image_sub.last_image_time - rospy.Time(.2))
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
self.add_observation(point0, (np.array(t), R),
self.image_sub.last_image_time)
observations1, pose_pairs1 = self.add_observation1(point1, (np.array(t), R),
self.image_sub.last_image_time)
observations2, pose_pairs2 = self.add_observation2(point2, (np.array(t), R),
self.image_sub.last_image_time)
observations, pose_pairs = self.get_observations_and_pose_pairs()
if len(observations) > self.min_observations:
self.est = self.multi_obs.lst_sqr_intersection(
observations, pose_pairs)
self.est1 = self.multi_obs.lst_sqr_intersection(
observations1, pose_pairs1)
self.est2 = self.multi_obs.lst_sqr_intersection(
observations2, pose_pairs2)
# print('est1: ', self.est1)
# print('est2: ', self.est2)
self.status = 'Pose found'
self.print_info("Pose Found!")
self.plane = self.generate_plane()
# print("Plane: ", self.plane)
self.markerPub0.publish(self.marker_msg(self.est, 'est0'))
self.markerPub1.publish(self.marker_msg(self.est1, 'est1'))
self.markerPub2.publish(self.marker_msg(self.est2, 'est2'))
print((self.est + self.est1 + self.est2) / 3)
else:
self.status = '{} observations'.format(len(observations))
class BuoyFinder:
_min_size = 100
'''
TODO:
Check for outliers in observations
'''
def __init__(self):
self.transformer = tf.TransformListener()
rospy.sleep(2.0)
self.search = False
self.last_image = None
self.last_draw_image = None
self.last_image_time = None
self.camera_model = None
self.multi_obs = None
self.max_observations = 20
self._id = 0 # Only for display
self.rviz = rviz.RvizVisualizer()
rospack = rospkg.RosPack()
# TODO: consolidate all these dictionaries into one big one.
self.observations = {
'red':deque(),
'yellow':deque(),
'green':deque()
}
self.pose_pairs = {
'red':deque(),
'yellow':deque(),
'green':deque()
}
if boost_to_the_moon:
self.buoys = {
# Boost classifier paths
'yellow': os.path.join(rospack.get_path('sub8_perception'), 'ml_classifiers/buoys/yellow/' + YELLOW),
'red': os.path.join(rospack.get_path('sub8_perception'), 'ml_classifiers/buoys/red/' + RED),
'green': os.path.join(rospack.get_path('sub8_perception'), 'ml_classifiers/buoys/green/' + GREEN)
}
else:
self.buoys = {
#Threshold paths
'green': '/color/buoy/green',
'red': '/color/buoy/red',
'yellow': '/color/buoy/yellow',
}
self.last_t = {
'green': None,
'red': None,
'yellow': None
}
# For displaying each buoy in rviz
self.draw_colors = {
'green': (0.0, 1.0, 0.0, 1.0),
'red': (1.0, 0.0, 0.0, 1.0),
'yellow': (1.0, 1.0, 0.0, 1.0),
}
self.visual_id = {
'green': 0,
'red': 1,
'yellow': 2,
}
if boost_to_the_moon:
for color in self.buoys.keys():
if self.buoys[color] is None:
rospy.logwarn('Classifier path for {} not found!'.format(color))
continue
path = self.buoys[color]
self.buoys[color] = cv2.Boost()
rospy.loginfo("BUOY - Loading {} boost...".format(color))
self.buoys[color].load(path)
rospy.loginfo("BUOY - Classifier for {} buoy loaded.".format(color))
self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo/left/image_raw', self.image_cb)
self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info')
self.mask_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/mask')
# Occasional status publisher
self.timer = rospy.Timer(rospy.Duration(1), self.publish_target_info)
self.toggle = rospy.Service('vision/buoys/search', SetBool, self.toggle_search)
self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy)
self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d)
print "BUOY - Fueled up, ready to go!"
def toggle_search(self, srv):
if srv.data:
rospy.loginfo("BUOY - Looking for buoys now.")
self.search = True
else:
rospy.loginfo("BUOY - Done looking for buoys.")
self.search = False
return SetBoolResponse(success=srv.data)
def request_buoy(self, srv):
print 'requesting', srv
timestamp = self.last_image_time
response = self.find_single_buoy(np.copy(self.last_image), timestamp, srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='/stereo_front/left'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=timestamp, frame_id='/stereo_front/left'),
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
)
return resp
def request_buoy3d(self, srv):
print "Requesting 3d pose"
if (len(self.observations[srv.target_name]) > 5) and self.multi_obs is not None:
estimated_pose = self.multi_obs.lst_sqr_intersection(self.observations[srv.target_name], self.pose_pairs[srv.target_name])
self.rviz.draw_sphere(estimated_pose, color=self.draw_colors[srv.target_name],
scaling=(0.5, 0.5, 0.5), frame='/map', _id=self.visual_id[srv.target_name])
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(
position=Point(*estimated_pose)
)
),
found=True
)
else:
if len(self.observations[srv.target_name]) <= 5:
rospy.logerr("Did not attempt search because we did not have enough observations ({})".format(self.observations[srv.target_name]))
else:
rospy.logerr("Did not attempt search because buoys_2d was not fully initialized")
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
),
found=False
)
return resp
def publish_target_info(self, *args):
if not self.search or self.last_image is None:
return
self.find_buoys(np.copy(self.last_image), self.last_image_time)
if self.last_draw_image is not None:
self.image_pub.publish(self.last_draw_image)
def image_cb(self, image):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
self.multi_obs = MultiObservation(self.camera_model)
def ncc(self, image, mean_thresh, scale=15):
'''Compute normalized cross correlation w.r.t a shadowed pillbox fcn
The expected scale will vary, so we don't cache it
'''
kernel = np.ones((scale, scale)) * -1
midpoint = (scale // 2, scale // 2)
cv2.circle(kernel, midpoint, midpoint[0], 1, -1)
mean, std_dev = cv2.meanStdDev(image)
# Check if the scene is brighter than our a priori target
if mean > mean_thresh:
kernel = -kernel
normalized_cross_correlation = cv2.filter2D((image - mean) / std_dev, -1, kernel)
renormalized = normalized_cross_correlation
return renormalized
def get_biggest(self, contours):
if len(contours) > 0:
cnt = max(contours, key=cv2.contourArea)
area = cv2.contourArea(cnt)
if area > self._min_size:
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
return None
else:
return None
def find_single_buoy(self, img, timestamp, buoy_type):
assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type)
max_area = 0
best_ret = None
if boost_to_the_moon:
# Segmentation here (machine learning right now)
some_observations = machine_learning.boost.observe(img)
prediction2 = [int(x) for x in [self.buoys[buoy_type].predict(obs) for obs in some_observations]]
mask = np.reshape(prediction2, img[:, :, 2].shape).astype(np.uint8) * 255
else:
#Thresholding here
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
low = np.array(rospy.get_param(self.buoys[buoy_type] + '/hsv_low')).astype(np.int32)
high = np.array(rospy.get_param(self.buoys[buoy_type] + '/hsv_high')).astype(np.int32)
mask = cv2.inRange(hsv, low, high)
rospy.sleep(.5)
kernel = np.ones((13,13),np.uint8)
mask = cv2.dilate(mask, kernel, iterations = 2)
mask = cv2.erode(mask, kernel, iterations = 2)
draw_mask = np.dstack([mask] * 3)
draw_mask[:,:,0] *= 0
if buoy_type == 'red':
draw_mask[:,:,1] *= 0
if buoy_type == 'green':
draw_mask[:,:,2] *= 0
self.mask_pub.publish(draw_mask)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
ret = self.get_biggest(contours)
if ret is None:
print "BUOY {} - No contours found".format(buoy_type)
return
contour, tuple_center, area = ret
if area > max_area:
max_area = area
best_ret = ret
if best_ret is None:
print "BUOY {} - best_ret is None".format(buoy_type)
return False
contour, tuple_center, area = best_ret
true_center, rad = cv2.minEnclosingCircle(contour)
if self.camera_model is not None:
if not self.sanity_check(tuple_center, timestamp):
return False
(t, rot_q) = self.transformer.lookupTransform('/map', '/stereo_front/left', timestamp)
trans = np.array(t)
R = sub8_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
self.rviz.draw_ray_3d(tuple_center, self.camera_model, self.draw_colors[buoy_type], frame='/stereo_front/left',
_id=self._id + 100, timestamp=timestamp)
self._id += 1
if self._id >= self.max_observations * 3:
self._id = 0
if (self.last_t[buoy_type] is None) or (np.linalg.norm(trans - self.last_t[buoy_type]) > 0.3):
self.last_t[buoy_type] = trans
self.observations[buoy_type].append(true_center)
self.pose_pairs[buoy_type].append((t, R))
print "BUOY {} - {} observations".format(buoy_type, len(self.observations[buoy_type]))
if len(self.observations[buoy_type]) > 5:
est = self.multi_obs.lst_sqr_intersection(self.observations[buoy_type], self.pose_pairs[buoy_type])
self.rviz.draw_sphere(est, color=self.draw_colors[buoy_type],
scaling=(0.5, 0.5, 0.5), frame='/map', _id=self.visual_id[buoy_type])
if len(self.observations[buoy_type]) > self.max_observations:
self.observations[buoy_type].popleft()
self.pose_pairs[buoy_type].popleft()
else:
print "BUOY {} - camera_model is None".format(buoy_type)
return tuple_center, rad
def find_buoys(self, img, timestamp):
draw_image = np.copy(img)
# This is only run if buoy_type is not None
for buoy_name in self.buoys.keys():
if self.buoys[buoy_name] is None:
continue
#rospy.loginfo("BUOY - Looking for {}".format(buoy_name))
result = self.find_single_buoy(img, timestamp, buoy_name)
if not result:
continue
center, rad = result
cv2.circle(draw_image, center, int(rad), (255, 255, 0), 2)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(draw_image, '{}'.format(buoy_name), center, font, 0.8, (20, 20, 240), 1)
self.last_draw_image = np.copy(draw_image)
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
sane = True
if np.linalg.norm(self.transformer.lookupTwist('/map', '/stereo_front/left', timestamp, rospy.Duration(.5))) > 1:
rospy.logerr("BUOY - Moving too fast. Not observing buoy.")
sane = False
return sane
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 BuoyFinder:
_min_size = 50
def __init__(self):
self.transformer = tf.TransformListener()
rospy.sleep(1.0)
self.done_once = False
self.last_image = None
self.last_draw_image = None
self.last_poop_image = None
self.last_image_time = None
self.camera_model = None
self.last_t = None
self.observations = deque()
self.pose_pairs = deque()
self.rviz = rviz.RvizVisualizer()
self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy)
self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d)
self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo/right/image_rect_color', self.image_cb)
self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info')
# Occasional status publisher
self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_target_info)
rospack = rospkg.RosPack()
boost_path = os.path.join(
rospack.get_path('sub8_perception'),
'sub8_vision_tools',
'classifiers',
'boost.cv2'
)
self.boost = cv2.Boost()
rospy.loginfo("Loading boost")
self.boost.load(boost_path)
rospy.loginfo("Boost loaded")
self.buoys = {
'green': '/color/buoy/green',
'red': '/color/buoy/red',
'yellow': '/color/buoy/yellow',
}
self.ppf = None
self.multi_obs = None
self.draw_colors = {
'green': (0.0, 1.0, 0.0, 1.0),
'red': (1.0, 0.0, 0.0, 1.0),
'yellow': (1.0, 1.0, 0.0, 1.0),
}
def request_buoy(self, srv):
print 'requesting', srv
response = self.find_single_buoy(np.copy(self.last_image), srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='/stereo_front/right'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=self.last_image_time, frame_id='/stereo_front/right'),
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
)
return resp
def request_buoy3d(self, srv):
print "Requesting 3d pose"
if (len(self.observations) > 5) and self.multi_obs is not None:
estimated_pose = self.multi_obs.multilaterate(self.observations, self.pose_pairs)
self.rviz.draw_sphere(estimated_pose, color=(0.2, 0.8, 0.0, 1.0), scaling=(0.5, 0.5, 0.5), frame='/map')
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(
position=Point(*estimated_pose)
)
),
found=True
)
else:
if len(self.observations) <= 5:
rospy.logerr("Did not attempt search because we did not have enough observations")
else:
rospy.logerr("Did not attempt search because buoys_2d was not fully initialized")
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
),
found=False
)
return resp
def publish_target_info(self, *args):
if self.last_image is None:
return
self.find_buoys(np.copy(self.last_image))
if self.last_draw_image is not None:
self.image_pub.publish(self.last_draw_image)
def image_cb(self, image):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
self.multi_obs = MultiObservation(self.camera_model)
def ncc(self, image, mean_thresh, scale=15):
'''Compute normalized cross correlation w.r.t a shadowed pillbox fcn
The expected scale will vary, so we don't cache it
'''
kernel = np.ones((scale, scale)) * -1
midpoint = (scale // 2, scale // 2)
cv2.circle(kernel, midpoint, midpoint[0], 1, -1)
mean, std_dev = cv2.meanStdDev(image)
# Check if the scene is brighter than our a priori target
if mean > mean_thresh:
kernel = -kernel
normalized_cross_correlation = cv2.filter2D((image - mean) / std_dev, -1, kernel)
renormalized = normalized_cross_correlation
return renormalized
def get_biggest(self, contours):
if len(contours) > 0:
cnt = max(contours, key=cv2.contourArea)
area = cv2.contourArea(cnt)
if area > self._min_size:
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, img, buoy_type):
if buoy_type != 'yellow':
return
assert buoy_type in self.buoys[buoy_type], "Buoys_2d does not know buoy color: {}".format(buoy_type)
max_area = 0
best_ret = None
some_observations = machine_learning.boost.observe(img)
prediction2 = [int(x) for x in [self.boost.predict(obs) for obs in some_observations]]
mask = np.reshape(prediction2, img[:, :, 2].shape).astype(np.uint8)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
ret = self.get_biggest(contours)
if ret is None:
return
contour, tuple_center, area = ret
if area > max_area:
max_area = area
best_ret = ret
if best_ret is None:
return False
contour, tuple_center, area = best_ret
true_center, rad = cv2.minEnclosingCircle(contour)
if self.camera_model is not None and (buoy_type == 'yellow'):
self.rviz.draw_ray_3d(tuple_center, self.camera_model, self.draw_colors[buoy_type])
(t, rot_q) = self.transformer.lookupTransform('/map', '/stereo_front/right', self.last_image_time - rospy.Duration(0.14))
trans = np.array(t)
R = sub8_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
if (self.last_t is None) or (np.linalg.norm(trans - self.last_t) > 0.3):
self.last_t = trans
self.observations.append(true_center)
self.pose_pairs.append((t, R))
if len(self.observations) > 5:
est = self.multi_obs.multilaterate(self.observations, self.pose_pairs)
self.rviz.draw_sphere(est, color=(0.9, 0.1, 0.0, 1.0), scaling=(0.3, 0.3, 0.3), frame='/map')
if len(self.observations) > 10:
self.observations.popleft()
self.pose_pairs.popleft()
return tuple_center, rad
def find_buoys(self, img):
draw_image = np.copy(img)
# This is only run if buoy_type is not None
for buoy_name in self.buoys.keys():
result = self.find_single_buoy(img, buoy_name)
if not result:
continue
center, rad = result
cv2.circle(draw_image, center, int(rad), (255, 255, 0), 2)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(draw_image, '{}'.format(buoy_name), center, font, 0.8, (20, 20, 240), 1)
self.last_draw_image = np.copy(draw_image)
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:
_min_size = 50
'''
TODO:
Check for outliers in observations
'''
def __init__(self):
self.transformer = tf.TransformListener()
rospy.sleep(2.0)
self.search = False
self.last_image = None
self.last_draw_image = None
self.last_image_time = None
self.camera_model = None
self.multi_obs = None
self.max_observations = 200
self._id = 0 # Only for display
self.rviz = rviz.RvizVisualizer()
boosting = rospy.get_param("/buoys/use_boost")
self.buoys = {}
for color in ['red', 'yellow', 'green']:
self.buoys[color] = Buoy(color, boosting)
self.buoys[color].load_segmentation()
self.image_sub = sub8_ros_tools.Image_Subscriber('/camera/front/left/image_raw', self.image_cb)
self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info')
self.mask_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/mask')
# Occasional status publisher
self.timer = rospy.Timer(rospy.Duration(.25), self.publish_target_info)
self.toggle = rospy.Service('vision/buoys/search', SetBool, self.toggle_search)
self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy)
self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d)
print "BUOY - Fueled up, ready to go!"
def toggle_search(self, srv):
if srv.data:
rospy.loginfo("BUOY - Looking for buoys now.")
self.search = True
else:
rospy.loginfo("BUOY - Done looking for buoys.")
self.search = False
return SetBoolResponse(success=srv.data)
def request_buoy(self, srv):
print 'requesting', srv
timestamp = self.last_image_time
response = self.find_single_buoy(np.copy(self.last_image), timestamp, srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='front_left_cam'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=timestamp, frame_id='front_left_cam'),
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
)
return resp
def request_buoy3d(self, srv):
print "Requesting 3d pose"
buoy = self.buoys[srv.target_name]
if (len(buoy.observations) > 5) and self.multi_obs is not None:
estimated_pose = self.multi_obs.lst_sqr_intersection(buoy.observations, buoy.pose_pairs)
rospy.loginfo("===================================")
rospy.loginfo("BUOY: est pose: {}".format(estimated_pose))
rospy.loginfo("===================================")
self.rviz.draw_sphere(estimated_pose, color=buoy.draw_colors,
scaling=(0.5, 0.5, 0.5), frame='/map', _id=buoy.visual_id)
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
pose=Pose(
position=Point(*estimated_pose)
)
),
found=True
)
else:
if len(buoy.observations) <= 5:
rospy.logerr("Did not attempt search because we did not have enough observations ({})".format(len(buoy.observations)))
else:
rospy.logerr("Did not attempt search because buoys_2d was not fully initialized")
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(stamp=self.last_image_time, frame_id='/map'),
),
found=False
)
return resp
def publish_target_info(self, *args):
if not self.search or self.last_image is None:
return
self.find_buoys(np.copy(self.last_image), self.last_image_time)
if self.last_draw_image is not None:
self.image_pub.publish(self.last_draw_image)
def image_cb(self, image):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
self.multi_obs = MultiObservation(self.camera_model)
def ncc(self, image, mean_thresh, scale=15):
'''Compute normalized cross correlation w.r.t a shadowed pillbox fcn
The expected scale will vary, so we don't cache it
'''
kernel = np.ones((scale, scale)) * -1
midpoint = (scale // 2, scale // 2)
cv2.circle(kernel, midpoint, midpoint[0], 1, -1)
mean, std_dev = cv2.meanStdDev(image)
# Check if the scene is brighter than our a priori target
if mean > mean_thresh:
kernel = -kernel
normalized_cross_correlation = cv2.filter2D((image - mean) / std_dev, -1, kernel)
renormalized = normalized_cross_correlation
return renormalized
def get_biggest(self, contours):
if len(contours) > 0:
cnt = max(contours, key=cv2.contourArea)
area = cv2.contourArea(cnt)
if area > self._min_size:
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
return None
else:
return None
def find_single_buoy(self, img, timestamp, buoy_type):
assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type)
max_area = 0
best_ret = None
buoy = self.buoys[buoy_type]
rospy.sleep(.1)
mask = buoy.segment(img)
kernel = np.ones((5,5),np.uint8)
mask = cv2.erode(mask, kernel, iterations = 2)
mask = cv2.dilate(mask, kernel, iterations = 2)
draw_mask = np.dstack([mask] * 3)
draw_mask[:,:,0] *= 0
if buoy_type == 'red':
draw_mask[:,:,1] *= 0
if buoy_type == 'green':
draw_mask[:,:,2] *= 0
self.mask_pub.publish(draw_mask)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
ret = self.get_biggest(contours)
if ret is None:
print "BUOY {} - No contours found".format(buoy_type)
return
contour, tuple_center, area = ret
if area > max_area:
max_area = area
best_ret = ret
if best_ret is None:
print "BUOY {} - best_ret is None".format(buoy_type)
return False
contour, tuple_center, area = best_ret
true_center, rad = cv2.minEnclosingCircle(contour)
if self.camera_model is not None:
if not self.sanity_check(tuple_center, timestamp):
return False
(t, rot_q) = self.transformer.lookupTransform('/map', '/front_left_cam', timestamp)
trans = np.array(t)
R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
self.rviz.draw_ray_3d(tuple_center, self.camera_model, buoy.draw_colors, frame='/front_left_cam',
_id=self._id + 100, timestamp=timestamp)
self._id += 1
if self._id >= self.max_observations * 3:
self._id = 0
if (buoy.last_t is None) or (np.linalg.norm(trans - buoy.last_t) > 0.1):
buoy.last_t = trans
buoy.observations.append(true_center)
buoy.pose_pairs.append((t, R))
print "BUOY {} - {} observations".format(buoy_type, len(buoy.observations))
if len(buoy.observations) > 5:
est = self.multi_obs.lst_sqr_intersection(buoy.observations, buoy.pose_pairs)
rospy.loginfo("===================================")
rospy.loginfo("BUOY: est pose: {}".format(est))
rospy.loginfo("===================================")
self.rviz.draw_sphere(est, color=buoy.draw_colors,
scaling=(0.5, 0.5, 0.5), frame='/map', _id=buoy.visual_id)
if len(buoy.observations) > self.max_observations:
buoy.observations.popleft()
buoy.pose_pairs.popleft()
else:
print "BUOY {} - camera_model is None".format(buoy_type)
return tuple_center, rad
def find_buoys(self, img, timestamp):
draw_image = np.copy(img)
for buoy_name in self.buoys.keys():
if self.buoys[buoy_name] is None:
continue
rospy.loginfo("BUOY - Looking for {}".format(buoy_name))
result = self.find_single_buoy(img, timestamp, buoy_name)
if not result:
continue
center, rad = result
cv2.circle(draw_image, center, int(rad), (255, 255, 0), 2)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(draw_image, '{}'.format(buoy_name), center, font, 0.8, (20, 20, 240), 1)
self.last_draw_image = np.copy(draw_image)
def sanity_check(self, coordinate, timestamp):
'''
Check if the observation is unreasonable. More can go here if we want.
'''
sane = True
if np.linalg.norm(self.transformer.lookupTwist('/map', '/front_left_cam', timestamp, rospy.Duration(.5))) > 1:
rospy.logerr("BUOY - Moving too fast. Not observing buoy.")
sane = False
return sane