def __init__(self, topic=None):
self.topic = topic
self.lock = threading.Lock()
self.bridge = CvBridge()
self.blobFinder = BlobFinder()
self.blobFinder.threshold = 150
self.blobFinder.filter_by_area = True
self.blobFinder.min_area = 0
self.blobFinder.max_area = 200
self.topic_type = mct_introspection.get_topic_type(topic)
rospy.init_node('blob_finder')
self.ready = False
if self.topic_type == 'sensor_msgs/Image':
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_callback)
else:
self.image_sub = rospy.Subscriber(self.topic, SeqAndImage,
self.seq_and_image_callback)
self.image_pub = rospy.Publisher('image_blobs', Image)
self.blob_data_pub = rospy.Publisher('blob_data', BlobData)
node_name = rospy.get_name()
self.set_param_srv = rospy.Service('{0}/set_param'.format(node_name),
BlobFinderSetParam,
self.handle_set_param_srv)
self.get_param_srv = rospy.Service('{0}/get_param'.format(node_name),
BlobFinderGetParam,
self.handle_get_param_srv)
self.ready = True
def __init__(self, topic, frame_rate=30.0):
self.topic = topic
self.frame_rate = frame_rate
self.start_t = 0.0
self.current_t = 0.0
self.progress_t = 0.0
self.frame_count = 0
self.recording_message = 'stopped'
self.writer = None
self.done = True
self.cv_image_size = None
self.filename = os.path.join(os.environ['HOME'], 'default.avi')
self.lock = threading.Lock()
self.bridge = CvBridge()
rospy.init_node('avi_writer')
self.node_name = rospy.get_name()
# Set up publications
self.progress_msg = RecordingProgressMsg()
self.progress_pub = rospy.Publisher('progress', RecordingProgressMsg)
# Subscribe to messages
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_handler)
# Set up services
self.recording_srv = rospy.Service(
'{0}/recording_cmd'.format(self.node_name), RecordingCmd,
self.handle_recording_cmd)
def __init__(self,topic=None):
self.topic = topic
self.lock = threading.Lock()
self.bridge = CvBridge()
rospy.init_node('zoom_tool')
node_name = rospy.get_name()
self.blobFinder = BlobFinder()
self.blobFinder.threshold = rospy.get_param('/zoom_tool_params/threshold', 200)
self.blobFinder.filter_by_area = rospy.get_param('/zoom_tool_params/filter_by_area', False)
self.blobFinder.min_area = rospy.get_param('/zoom_tool_params/min_area', 0)
self.blobFinder.max_area = rospy.get_param('/zoom_tool_params/max_area', 200)
self.circle_color = (0,0,255)
self.text_color = (0,0,255)
self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.8, 0.8,thickness=1)
self.image_sub = rospy.Subscriber(self.topic,Image,self.image_callback)
self.image_pub = rospy.Publisher('image_zoom_tool', Image)
#self.devel_pub = rospy.Publisher('develop', Float32)
self.set_param_srv = rospy.Service(
'{0}/set_param'.format(node_name),
BlobFinderSetParam,
self.handle_set_param_srv
)
self.get_param_srv = rospy.Service(
'{0}/get_param'.format(node_name),
BlobFinderGetParam,
self.handle_get_param_srv
)
def __init__(self, topic_name):
self.bridge = CvBridge()
def message_extractor(ros_img):
try:
cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
return cv_image, ros_img
except CvBridgeError, e:
return None
def __init__(self):
self.bridge = CvBridge()
file_dir, file_name = os.path.split(__file__)
font_path = '%s/../fonts/FreeMono.ttf'%(file_dir,)
self.font = PILImageFont.truetype(font_path, 20)
self.fill = (0,0,0)
rospy.init_node('progress_info')
self.pub = rospy.Publisher('image_progress_message', Image)
self.sub = rospy.Subscriber('progress',ProgressMsg, self.handle_progress_msg)
def __init__(self, frame_rate, max_allowed_error=10.0e-3, max_seq_age=200):
self.frame_rate = frame_rate
self.lock = threading.Lock()
self.max_allowed_error = max_allowed_error
self.max_seq_age = max_seq_age
self.seq_to_camera_stamps = {}
self.stamp_last = {}
self.max_error_by_camera = {}
self.max_error = 0.0
self.camera_fail = ''
self.most_recent_seq = None
self.last_checked_seq = None
self.ok = True
self.seq_fail = 0
self.error_msg = ''
self.bridge = CvBridge()
self.info_image_size = (400, 90)
self.font = PILImageFont.truetype(
"/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16)
self.ready = False
rospy.init_node('time_stamp_watchdog')
# Subscribe to camera info topics
self.wait_for_camera_info_topics()
camera_info_topics = mct_introspection.find_camera_info_topics()
self.camera_info_sub = {}
for topic in camera_info_topics:
camera = get_camera_from_topic(topic)
handler = functools.partial(self.camera_info_handler, camera)
self.camera_info_sub[camera] = rospy.Subscriber(
topic, CameraInfo, handler)
self.number_of_cameras = len(camera_info_topics)
# Create reset service
self.reset_srv = rospy.Service('time_stamp_watchdog_reset', Empty,
self.reset_handler)
# Create time stamp watchdog publication
self.watchdog_pub = rospy.Publisher('time_stamp_watchdog',
TimeStampWatchDog)
# Create watchdog info image
self.image_watchdog_pub = rospy.Publisher('image_time_stamp_watchdog',
Image)
self.ready = True
class ROSStereoListener:
def __init__(self, topics, rate=30.0, name='stereo_listener'):
self.listener = ru.GenericListener(name, [Image, Image], topics, rate)
self.lbridge = CvBridge()
self.rbridge = CvBridge()
def next(self):
lros, rros = self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8'))
rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8'))
return lcv, rcv
def __init__(self):
self.bridge = CvBridge()
self.font = PILImageFont.truetype("/usr/share/fonts/truetype/freefont/FreeMono.ttf", 20)
self.fill = (0,00,0)
self.bg_color = (255,255,255)
#self.bg_color = (200,200,255)
self.image_size = (105,28)
self.text_pos = (0,4)
rospy.init_node('fps_info_image')
self.pub = rospy.Publisher('image_fps', Image)
self.sub = rospy.Subscriber('fps_info',FPSInfoMsg, self.handle_fps_msg)
def find_image_features(bagname, topic):
features_list = []
bridge = CvBridge()
i = 0
for topic, msg, t in ru.bag_iter(bagname, [topic]):
t = msg.header.stamp.to_time()
image = bridge.imgmsg_to_cv(msg, 'bgr8')
image_gray = fea.grayscale(image)
surf_keypoints, surf_descriptors = fea.surf(image_gray)
features_list.append((t, (surf_keypoints, surf_descriptors)))
rospy.loginfo("%.3f frame %d found %d points" % (t, i, len(surf_keypoints)))
i = i + 1
return features_list
class ROSStereoListener:
def __init__(self, topics, rate=30.0, name='stereo_listener'):
self.listener = ru.GenericListener(name, [Image, Image], topics, rate)
self.lbridge = CvBridge()
self.rbridge = CvBridge()
def next(self):
lros, rros = self.listener.read(allow_duplication=False,
willing_to_wait=True,
warn=False,
quiet=True)
lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8'))
rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8'))
return lcv, rcv
def __init__(self,
threshold=200,
filter_by_area=False,
min_area=0,
max_area=200):
self.threshold = threshold
self.filter_by_area = filter_by_area
self.min_area = min_area
self.max_area = max_area
self.bridge = CvBridge()
#self.blob_mask = cvblob.CV_BLOB_RENDER_CENTROID
#self.blob_mask |= cvblob.CV_BLOB_RENDER_COLOR
self.blobs_image = None
self.label_image = None
self.thresh_image = None
class FPS_Image(object):
def __init__(self):
self.bridge = CvBridge()
self.font = PILImageFont.truetype("/usr/share/fonts/truetype/freefont/FreeMono.ttf", 20)
self.fill = (0,00,0)
self.bg_color = (255,255,255)
#self.bg_color = (200,200,255)
self.image_size = (105,28)
self.text_pos = (0,4)
rospy.init_node('fps_info_image')
self.pub = rospy.Publisher('image_fps', Image)
self.sub = rospy.Subscriber('fps_info',FPSInfoMsg, self.handle_fps_msg)
def handle_fps_msg(self,data):
# Extract keyword arguments
fps = data.rate
# Create PIL image and write text to it
pil_image = PILImage.new('RGB',self.image_size,self.bg_color)
draw = PILImageDraw.Draw(pil_image)
fps_text = 'fps %2.1f'%(fps,)
draw.text(self.text_pos,fps_text,font=self.font,fill=self.fill)
# Convert to opencv image, then to ROS image and publish
cv_image = cv.CreateImageHeader(pil_image.size, cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_image, pil_image.tostring())
rosimage = self.bridge.cv_to_imgmsg(cv_image,'rgb8')
self.pub.publish(rosimage)
def run(self):
rospy.spin()
def __init__(self,topic,frame_rate=30.0):
self.topic = topic
self.frame_rate = frame_rate
self.start_t = 0.0
self.current_t = 0.0
self.progress_t = 0.0
self.frame_count = 0
self.recording_message = 'stopped'
self.writer = None
self.done = True
self.cv_image_size = None
self.filename = os.path.join(os.environ['HOME'],'default.avi')
self.lock = threading.Lock()
self.bridge = CvBridge()
rospy.init_node('avi_writer')
self.node_name = rospy.get_name()
# Set up publications
self.progress_msg = RecordingProgressMsg()
self.progress_pub = rospy.Publisher('progress',RecordingProgressMsg)
# Subscribe to messages
self.image_sub = rospy.Subscriber(self.topic,Image,self.image_handler)
# Set up services
self.recording_srv = rospy.Service(
'{0}/recording_cmd'.format(self.node_name),
RecordingCmd,
self.handle_recording_cmd
)
def __init__(self):
self.image_shape = (15, 640, 3)
self.empty_color = (230, 230, 230)
self.fill_color = (0, 0, 200)
self.base_array = 255 * numpy.ones(self.image_shape, dtype=numpy.uint8)
for i in range(0, 3):
self.base_array[:, :, i] = self.empty_color[i]
self.bridge = CvBridge()
rospy.init_node('progress_bar')
# Pulications
self.pub = rospy.Publisher('image_progress_bar', Image)
# Subscriptions
self.sub = rospy.Subscriber('progress', ProgressMsg,
self.handle_progress_msg)
class Progress_Bar(object):
def __init__(self):
self.image_shape = (15, 640, 3)
self.empty_color = (230, 230, 230)
self.fill_color = (0, 0, 200)
self.base_array = 255 * numpy.ones(self.image_shape, dtype=numpy.uint8)
for i in range(0, 3):
self.base_array[:, :, i] = self.empty_color[i]
self.bridge = CvBridge()
rospy.init_node('progress_bar')
# Pulications
self.pub = rospy.Publisher('image_progress_bar', Image)
# Subscriptions
self.sub = rospy.Subscriber('progress', ProgressMsg,
self.handle_progress_msg)
def handle_progress_msg(self, data):
frame_count = data.frame_count
progress_t = data.progress_t
record_t = data.record_t
image_array = numpy.array(self.base_array)
if record_t > 0:
fill_ind = int(self.image_shape[1] * progress_t / record_t)
else:
fill_ind = self.image_shape[1]
for i in range(0, 3):
image_array[:, :fill_ind, i] = self.fill_color[i]
cv_image = cv.fromarray(image_array)
rosimage = self.bridge.cv_to_imgmsg(cv_image, 'rgb8')
self.pub.publish(rosimage)
def run(self):
rospy.spin()
def __init__(self, topic_name):
self.bridge = CvBridge()
def message_extractor(ros_img):
try:
cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
return cv_image, ros_img
except CvBridgeError, e:
return None
def find_contact_images(bag_name, contact_times, all_times, topic_name):
print 'finding closest images for ', topic_name
times_tree = sp.KDTree(np.matrix(all_times).T)
closest_times = [all_times[times_tree.query([a_time])[1]] for a_time in contact_times]
pdb.set_trace()
print 'getting & saving images, expecting', len(set(closest_times)), 'images'
bridge = CvBridge()
cleaned_topic_name = topic_name.replace('/', '')
i = 0
for ros_msg in get_closest_msgs(bag_name, [topic_name], closest_times):
i = i + 1
msg_time = ros_msg.header.stamp.to_time() - all_times[0]
cv_image = bridge.imgmsg_to_cv(ros_msg, 'bgr8')
img_name = "%s_%.3f_touched.png" % (cleaned_topic_name, msg_time)
print 'writing', img_name
cv.SaveImage(img_name, cv_image)
print 'got', i, 'images'
def __init__(self, image_topic, filename):
self.image_topic = image_topic
self.filename = filename
self.frame_count = 0
self.recording = False
self.writer = None
self.cv_image_size = None
self.framerate = None
self.last_stamp = None
self.lock = threading.Lock()
self.bridge = CvBridge()
rospy.init_node('avi_writer')
rospy.on_shutdown(self.stop_video_writer)
# Subscribe to image topic
self.image_sub = rospy.Subscriber(self.image_topic, Image,
self.image_handler)
def __init__(self, topic):
# Video recording parameters
self.topic = topic
self.record_t = 10.0
self.start_t = 0.0
self.current_t = 0.0
self.progress_t = 0.0
self.frame_count = 0
self.recording_message = 'stopped'
self.frame_rate = 24.0 # This is a kludge - get from image topic
self.writer = None
self.done = True
self.cv_image_size = None
self.filename = os.path.join(os.environ['HOME'], 'default.avi')
# Current pulse parameters
#self.pulse_channel = 'a'
self.pulse_channel = 'c' # Should probalby move this to a configuration file
self.pulse_controller = None
self.timing_fid = None
self.lock = threading.Lock()
self.redis_db = redis.Redis('localhost', db=lia_config.redis_db)
self.bridge = CvBridge()
rospy.init_node('avi_writer')
# Set up publications
self.progress_msg = ProgressMsg()
self.progress_pub = rospy.Publisher('progress', ProgressMsg)
# Subscribe to messages
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_handler)
# Set up services
self.recording_srv = rospy.Service('recording_cmd', RecordingCmd,
self.handle_recording_cmd)
# Set up proxy for set current service
rospy.wait_for_service('set_current')
self.set_current_proxy = rospy.ServiceProxy('set_current',
SetCurrentCmd)
class ROSImageClient:
def __init__(self, topic_name):
self.bridge = CvBridge()
def message_extractor(ros_img):
try:
cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
return cv_image, ros_img
except CvBridgeError, e:
return None
self.listener = ru.GenericListener('ROSImageClient', Image, topic_name,
.1, message_extractor)
def find_contact_images(bag_name, contact_times, all_times, topic_name):
print 'finding closest images for ', topic_name
times_tree = sp.KDTree(np.matrix(all_times).T)
closest_times = [
all_times[times_tree.query([a_time])[1]] for a_time in contact_times
]
pdb.set_trace()
print 'getting & saving images, expecting', len(
set(closest_times)), 'images'
bridge = CvBridge()
cleaned_topic_name = topic_name.replace('/', '')
i = 0
for ros_msg in get_closest_msgs(bag_name, [topic_name], closest_times):
i = i + 1
msg_time = ros_msg.header.stamp.to_time() - all_times[0]
cv_image = bridge.imgmsg_to_cv(ros_msg, 'bgr8')
img_name = "%s_%.3f_touched.png" % (cleaned_topic_name, msg_time)
print 'writing', img_name
cv.SaveImage(img_name, cv_image)
print 'got', i, 'images'
def __init__(self, surf_name, contacts_name):
forearm_cam_l = '/l_forearm_cam/image_rect_color'
self.bridge = CvBridge()
rospy.loginfo('loading %s' % surf_name)
self.surf_data = ut.load_pickle(surf_name)
rospy.loginfo('loading %s' % contacts_name)
self.scene, self.contact_points = ut.load_pickle(contacts_name)
self.surf_idx = None
cv.NamedWindow('surf', 1)
self.tflistener = tf.TransformListener()
self.camera_geo = ROSCameraCalibration('/l_forearm_cam/camera_info')
self.lmat0 = None
self.rmat0 = None
self.contact = False
self.contact_stopped = False
#rospy.Subscriber('/pressure/l_gripper_motor', pm.PressureState, self.lpress_cb)
rospy.Subscriber(forearm_cam_l, sm.Image, self.image_cb)
print 'ready'
def __init__(self, topic=None):
self.topic = topic
self.lock = threading.Lock()
self.bridge = CvBridge()
self.camera = get_camera_from_topic(self.topic)
camera_calibration = file_tools.read_camera_calibration(self.camera)
self.camera_matrix = get_camera_matrix(camera_calibration)
# Blob finder parameters
self.blobFinder = BlobFinder()
self.blobFinder.threshold = 150
self.blobFinder.filter_by_area = False
self.blobFinder.min_area = 0
self.blobFinder.max_area = 200
# Tracking parameters
self.tracking_pts_dist = (0.0, 0.04774, 0.07019)
self.tracking_pts_colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255)]
rospy.init_node('blob_finder')
self.ready = False
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_callback)
self.image_tracking_pts_pub = rospy.Publisher('image_tracking_pts',
Image)
self.image_blobs_pub = rospy.Publisher('image_blobs', Image)
self.blob_data_pub = rospy.Publisher('blob_data', BlobData)
self.devel_pub = rospy.Publisher('devel_data', Float32)
node_name = rospy.get_name()
self.set_param_srv = rospy.Service('{0}/set_param'.format(node_name),
BlobFinderSetParam,
self.handle_set_param_srv)
self.get_param_srv = rospy.Service('{0}/get_param'.format(node_name),
BlobFinderGetParam,
self.handle_get_param_srv)
self.ready = True
def __init__(self, max_seq_age=150):
rospy.init_node('frame_drop_watchdog')
self.max_seq_age = max_seq_age
self.lock = threading.Lock()
self.frames_dropped = {}
self.latest_seq = None
self.ready = False
camera_assignment = file_tools.read_camera_assignment()
self.number_of_cameras = len(camera_assignment)
self.bridge = CvBridge()
self.info_image_size = (400, 90)
self.font = PILImageFont.truetype(
"/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16)
# Subscribe to camera info topics
self.frames_dropped_sub = {}
frames_dropped_topics = self.wait_for_topics()
for topic in frames_dropped_topics:
camera = get_camera_from_topic(topic)
handler = functools.partial(self.frames_dropped_handler, camera)
self.frames_dropped_sub[camera] = rospy.Subscriber(
topic, FramesDropped, handler)
# Setup total frames dropped service
self.total_dropped_pub = rospy.Publisher('total_frames_dropped',
FramesDropped)
# Setup reset service
self.reset_srv = rospy.Service('frame_drop_watchdog_reset', Empty,
self.reset_handler)
# Create watchdog info image
self.image_watchdog_pub = rospy.Publisher('image_frame_drop_watchdog',
Image)
self.ready = True
class ROSImageClient:
def __init__(self, topic_name):
self.bridge = CvBridge()
def message_extractor(ros_img):
try:
cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
return cv_image, ros_img
except CvBridgeError, e:
return None
self.listener = ru.GenericListener('ROSImageClient', Image, topic_name,
.1, message_extractor)
def __init__(self,
stitched_image_topic,
tracking_pts_topic,
max_seq_age=150):
self.lock = threading.Lock()
self.bridge = CvBridge()
self.stitching_params = file_tools.read_tracking_2d_stitching_params()
self.stitched_image_topic = stitched_image_topic
self.tracking_pts_topic = tracking_pts_topic
self.max_seq_age = max_seq_age
self.latest_seq = None
self.seq_to_stitched_image = {}
self.seq_to_tracking_pts = {}
self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,
0.8,
0.8,
thickness=1)
self.magenta = (255, 255, 0)
self.tracking_pts_colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255)]
self.labeled_image = None
self.ready = False
rospy.init_node('stitched_image_labeler')
# Subscribe to stitched image and tracking pts topics
self.image_sub = rospy.Subscriber(self.stitched_image_topic,
SeqAndImage,
self.stitched_image_handler)
self.tracking_pts_sub = rospy.Subscriber(self.tracking_pts_topic,
ThreePointTracker,
self.tracking_pts_handler)
# Create labeled image publication
self.labeled_image_pub = rospy.Publisher('image_stitched_labeled',
Image)
self.ready = True
def __init__(self, *args, **kwargs):
self.lock = threading.Lock()
self.calibrate = False
super(MCT_CalibrationNode, self).__init__(*args, **kwargs)
self.bridge = CvBridge()
self.cal_img_pub = rospy.Publisher('image_calibrator',
sensor_msgs.msg.Image)
self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2)
self.font_color_red = cv.CV_RGB(255, 0, 0)
self.font_color_green = cv.CV_RGB(0, 255, 0)
self.font_color_blue = cv.CV_RGB(80, 80, 255)
self.x_start = 10
self.y_start = 30
self.x_step = 140
self.y_step = 32
# Set up sevices
node_name = rospy.get_name()
self.good_enough_srv = rospy.Service(
'{0}/good_enough'.format(node_name),
GetBool,
self.handle_good_enough_srv,
)
self.calibrate_srv = rospy.Service(
'{0}/calibrate'.format(node_name),
GetBool,
self.handle_calibrate_srv,
)
self.get_calibration_srv = rospy.Service(
'{0}/get_calibration'.format(node_name),
GetString,
self.handle_get_calibration_srv,
)
def __init__(self,topic):
self.topic = topic
self.record_t = 10.0
self.start_t = 0.0
self.current_t = 0.0
self.progress_t = 0.0
self.frame_count = 0
self.recording_message = 'stopped'
self.frame_rate = 24.0 # This is a kludge - get from image topic
self.writer = None
self.done = True
self.cv_image_size = None
self.filename = os.path.join(os.environ['HOME'],'default.avi')
#self.progress_message = Progress_Message()
self.lock = threading.Lock()
self.redis_db = redis.Redis('localhost', db=lia_config.redis_db)
self.bridge = CvBridge()
rospy.init_node('avi_writer')
# Set up publications
self.progress_msg = ProgressMsg()
self.progress_pub = rospy.Publisher('progress',ProgressMsg)
# Subscribe to messages
self.image_sub = rospy.Subscriber(self.topic,Image,self.image_handler)
# Set up services
self.recording_srv = rospy.Service(
'recording_cmd',
RecordingCmd,
self.handle_recording_cmd
)
class Progress_Info(object):
def __init__(self):
self.bridge = CvBridge()
file_dir, file_name = os.path.split(__file__)
font_path = '%s/../fonts/FreeMono.ttf'%(file_dir,)
self.font = PILImageFont.truetype(font_path, 20)
self.fill = (0,0,0)
rospy.init_node('progress_info')
self.pub = rospy.Publisher('image_progress_message', Image)
self.sub = rospy.Subscriber('progress',ProgressMsg, self.handle_progress_msg)
def handle_progress_msg(self,data):
# Extract keyword arguments
message = data.recording_message
frame_count = data.frame_count
record_t = data.record_t
progress_t = data.progress_t
# Get minutes and seconds
record_hr, record_min, record_sec = get_hr_min_sec(record_t)
progress_hr, progress_min, progress_sec = get_hr_min_sec(progress_t)
# Create PIL image and write text to it
pil_image = PILImage.new('RGB',(640,30),(255,255,255))
draw = PILImageDraw.Draw(pil_image)
message_text = '%s,'%(message,)
draw.text((0,10),message_text,font=self.font,fill=self.fill)
frame_text = 'frame %d'%(frame_count,)
draw.text((160,10),frame_text,font=self.font,fill=self.fill)
time_data = (progress_hr,progress_min,progress_sec,record_hr,record_min,record_sec)
time_text = '%02d:%02d:%02d/%02d:%02d:%02d'%time_data
draw.text((430,10),time_text,font=self.font,fill=self.fill)
# Convert to opencv image, then to ROS image and publish
cv_image = cv.CreateImageHeader(pil_image.size, cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_image, pil_image.tostring())
rosimage = self.bridge.cv_to_imgmsg(cv_image,'rgb8')
self.pub.publish(rosimage)
def run(self):
rospy.spin()
class ImageProcess:
def __init__(self, surf_name, contacts_name):
forearm_cam_l = '/l_forearm_cam/image_rect_color'
self.bridge = CvBridge()
rospy.loginfo('loading %s' % surf_name)
self.surf_data = ut.load_pickle(surf_name)
rospy.loginfo('loading %s' % contacts_name)
self.scene, self.contact_points = ut.load_pickle(contacts_name)
self.surf_idx = None
cv.NamedWindow('surf', 1)
self.tflistener = tf.TransformListener()
self.camera_geo = ROSCameraCalibration('/l_forearm_cam/camera_info')
self.lmat0 = None
self.rmat0 = None
self.contact = False
self.contact_stopped = False
#rospy.Subscriber('/pressure/l_gripper_motor', pm.PressureState, self.lpress_cb)
rospy.Subscriber(forearm_cam_l, sm.Image, self.image_cb)
print 'ready'
#def lpress_cb(self, msg):
#def lpress_cb(self, pmsg):
# #conv to mat
# lmat = np.matrix((pmsg.l_finger_tip)).T
# rmat = np.matrix((pmsg.r_finger_tip)).T
# if self.lmat0 == None:
# self.lmat0 = lmat
# self.rmat0 = rmat
# return
# #zero
# lmat = lmat - self.lmat0
# rmat = rmat - self.rmat0
# #touch detected
# if np.any(np.abs(lmat) > 250) or np.any(np.abs(rmat) > 250): #TODO: replace this with something more sound
# self.contact = True
# else:
# if self.contact == True:
# self.contact_stopped = True
# self.contact = False
# #Contact has been made!! look up gripper tip location
# #to_frame = 'base_link'
# #def frame_loc(from_frame):
# # p_base = tfu.transform('base_footprint', from_frame, self.tflistener) \
# # * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
# # #* tfu.tf_as_matrix((p.A1.tolist(), tr.quaternion_from_euler(0,0,0)))
# # return tfu.matrix_as_tf(p_base)
# #tip_locs = [frame_loc(n)[0] for n in self.ftip_frames]
# #t = pmsg.header.stamp.to_time()
# #rospy.loginfo("contact detected at %.3f" % t)
# #self.contact_locs.append([t, tip_locs])
# #self.last_msg = time.time()
# rospy.loginfo('lpress_cb ' + str(np.max(rmat)) + ' ' + str(np.max(lmat)))
def image_cb(self, msg):
image_time = msg.header.stamp.to_time()
image = self.bridge.imgmsg_to_cv(msg, 'bgr8')
if self.surf_idx == None:
for i, d in enumerate(self.surf_data):
t, sdata = d
if image_time == t:
self.surf_idx = i
break
else:
self.surf_idx = self.surf_idx + 1
stime, sdata = self.surf_data[self.surf_idx]
if stime != image_time:
print 'surf time != image_time'
surf_keypoints, surf_descriptors = sdata
nimage = fea.draw_surf(image, surf_keypoints, (255,0,0))
cv.ShowImage('surf', nimage)
cv.WaitKey(10)
# Track and give 3D location to features.
## Project 3D points into this frame (need access to tf => must do online or from cache)
## camera_T_3dframe at ti
scene2d = self.camera_geo.project(self.scene, self.tflistener, 'base_footprint')
scene2d_tree = sp.KDTree(np.array(scene2d.T))
## Find features close to these 2d points
for loc, lap, size, d, hess in surf_keypoints:
idx = scene2d_tree.query(np.array(loc))[1]
orig3d = self.scene[:, idx]
## Get features closest to the contact point
## stop running if contact has stopped
if self.contact:
dest='headless',
help='headless mode')
opt, args = p.parse_args()
cameras = [
opt.cam + '/left/image_rect_color', opt.cam + '/right/image_rect_color'
]
stereo_listener = rc.ROSStereoListener(cameras)
if not opt.headless:
#cv.NamedWindow('left', 0)
#cv.NamedWindow('right', 0)
cv.NamedWindow('stereo-anaglyph', 0)
cv.ResizeWindow('stereo-anaglyph', 640, 480)
cv.WaitKey(10)
else:
bridge = CvBridge()
image_pub = rospy.Publisher('stereo_anaglyph', Image)
anaglyph_cyan_image_distance_correction = rospy.get_param(
'anaglyph_dist', opt.dist)
left = 1113937 # 65361
right = 1113939 #65363
escape = 1048603 #27
while not rospy.is_shutdown():
l, r = stereo_listener.next()
red_blue = anaglyph(l, r, anaglyph_cyan_image_distance_correction)
if not opt.headless:
#cv.ShowImage('left', l)
#cv.ShowImage('right', r)
cv.ShowImage('stereo-anaglyph', red_blue)
class FrameDropWatchdog(object):
"""
Frame drop watchdog monitors the number of frames dropped by the system.
"""
def __init__(self, max_seq_age=150):
rospy.init_node('frame_drop_watchdog')
self.max_seq_age = max_seq_age
self.lock = threading.Lock()
self.frames_dropped = {}
self.latest_seq = None
self.ready = False
camera_assignment = file_tools.read_camera_assignment()
self.number_of_cameras = len(camera_assignment)
self.bridge = CvBridge()
self.info_image_size = (400, 90)
self.font = PILImageFont.truetype(
"/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16)
# Subscribe to camera info topics
self.frames_dropped_sub = {}
frames_dropped_topics = self.wait_for_topics()
for topic in frames_dropped_topics:
camera = get_camera_from_topic(topic)
handler = functools.partial(self.frames_dropped_handler, camera)
self.frames_dropped_sub[camera] = rospy.Subscriber(
topic, FramesDropped, handler)
# Setup total frames dropped service
self.total_dropped_pub = rospy.Publisher('total_frames_dropped',
FramesDropped)
# Setup reset service
self.reset_srv = rospy.Service('frame_drop_watchdog_reset', Empty,
self.reset_handler)
# Create watchdog info image
self.image_watchdog_pub = rospy.Publisher('image_frame_drop_watchdog',
Image)
self.ready = True
def wait_for_topics(self):
"""
Wait for the frames_dropped topics to be published.
"""
while 1:
frames_dropped_topics = mct_introspection.find_topics_w_ending(
'frames_dropped')
if len(frames_dropped_topics) == self.number_of_cameras:
break
rospy.sleep(0.25)
return frames_dropped_topics
def reset_handler(self, req):
"""
Handler for the nodes reset service - empties the frames_dropped buffer.
"""
with self.lock:
self.frames_dropped = {}
self.latest_seq = None
return EmptyResponse()
def frames_dropped_handler(self, camera, data):
if not self.ready:
return
with self.lock:
try:
self.frames_dropped[data.seq][camera] = data.frames_dropped
except KeyError:
self.frames_dropped[data.seq] = {camera: data.frames_dropped}
self.update_latest_seq(data.seq)
def update_latest_seq(self, seq):
if self.latest_seq is None:
self.latest_seq = seq
else:
self.latest_seq = max([seq, self.latest_seq])
def publish_watchdog_image(self, seq, total_frames_dropped,
cameras_w_drops):
"""
Publish image for GUI w/ seq #, total frames dropped, other info?
"""
pil_info_image = PILImage.new('RGB', self.info_image_size,
(255, 255, 255))
draw = PILImageDraw.Draw(pil_info_image)
info_items = [
('seq', seq),
('dropped', total_frames_dropped),
('cameras', cameras_w_drops),
]
text_x, text_y, step_y = 10, 10, 20
for i, item in enumerate(info_items):
label, value = item
label_text = '{0}:'.format(label)
if type(value) == float:
value_text = '{0:<1.6f}'.format(value)
else:
value_text = '{0}'.format(value)
draw.text((text_x, text_y + step_y * i),
label_text,
font=self.font,
fill=(0, 0, 0))
draw.text((text_x + 100, text_y + step_y * i),
value_text,
font=self.font,
fill=(0, 0, 0))
cv_info_image = cv.CreateImageHeader(pil_info_image.size,
cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_info_image, pil_info_image.tostring())
# Convert to a rosimage and publish
info_rosimage = self.bridge.cv_to_imgmsg(cv_info_image, 'rgb8')
self.image_watchdog_pub.publish(info_rosimage)
def run(self):
"""
Node, main loop. While the node
"""
while not rospy.is_shutdown():
with self.lock:
for seq, data in sorted(self.frames_dropped.items()):
if len(data) == self.number_of_cameras:
total_frames_dropped = sum(data.values())
self.total_dropped_pub.publish(seq,
total_frames_dropped)
cameras_w_drops = [
c for c, n in data.iteritems() if n > 0
]
cameras_w_drops = [
int(c.split('_')[1]) for c in cameras_w_drops
]
del self.frames_dropped[seq]
self.publish_watchdog_image(seq, total_frames_dropped,
cameras_w_drops)
else:
if self.latest_seq - seq > self.max_seq_age:
del self.frames_dropped[seq]
class MCT_CalibrationNode(CalibrationNode):
def __init__(self, *args, **kwargs):
self.lock = threading.Lock()
self.calibrate = False
super(MCT_CalibrationNode, self).__init__(*args, **kwargs)
self.bridge = CvBridge()
self.cal_img_pub = rospy.Publisher('image_calibrator',
sensor_msgs.msg.Image)
self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2)
self.font_color_red = cv.CV_RGB(255, 0, 0)
self.font_color_green = cv.CV_RGB(0, 255, 0)
self.font_color_blue = cv.CV_RGB(80, 80, 255)
self.x_start = 10
self.y_start = 30
self.x_step = 140
self.y_step = 32
# Set up sevices
node_name = rospy.get_name()
self.good_enough_srv = rospy.Service(
'{0}/good_enough'.format(node_name),
GetBool,
self.handle_good_enough_srv,
)
self.calibrate_srv = rospy.Service(
'{0}/calibrate'.format(node_name),
GetBool,
self.handle_calibrate_srv,
)
self.get_calibration_srv = rospy.Service(
'{0}/get_calibration'.format(node_name),
GetString,
self.handle_get_calibration_srv,
)
def handle_good_enough_srv(self, req):
"""
Handles requests for the calibrators good_enough flag which indicates whether
or not sufficient data has been aquired for calibrating the camera.
"""
with self.lock:
good_enough = self.c.goodenough
return GetBoolResponse(good_enough)
def handle_calibrate_srv(self, req):
"""
Handles requests for calibrating the cameras associated with the calibrator.
"""
with self.lock:
if self.c.goodenough:
self.calibrate = True
flag = True
else:
flag = False
return GetBoolResponse(flag)
def handle_get_calibration_srv(self, req):
"""
Handles requests for the camera calibration parameters.
"""
with self.lock:
if self.c.calibrated:
ost_txt = self.c.ost()
else:
ost_txt = ''
if ost_txt:
# Add actuall camera name to calibration
camera_topic = rospy.remap_name('camera')
camera_name = camera_topic.split('/')[2]
ost_txt = ost_txt.replace('narrow_stereo/left', camera_name)
return GetStringResponse(ost_txt)
def redraw_monocular(self, drawable):
"""
Redraw calibratoin image callback.
"""
if not self.c.calibrated:
with self.lock:
if self.calibrate and self.c.goodenough:
self.c.do_calibration()
if self.c.goodenough:
text_data = [('Qty', '{0}'.format(len(self.c.db))),
('Good Enough', )]
self.add_progress_text(drawable.scrib, text_data,
self.font_color_green)
else:
if drawable.params:
text_data = [('Qty', '{0}'.format(len(self.c.db)))] + list(
drawable.params)
self.add_progress_text(drawable.scrib, text_data,
self.font_color_red)
else:
text_data = [('No Data', )]
self.add_progress_text(drawable.scrib, text_data,
self.font_color_red)
else:
text_data = [('Calibrated', )]
self.add_progress_text(drawable.scrib, text_data,
self.font_color_green)
#print self.c.ost()
rosimage = self.bridge.cv_to_imgmsg(drawable.scrib, 'bgr8')
self.cal_img_pub.publish(rosimage)
def add_progress_text(self, img, text_data, font_color):
"""
Adds progress text to calibration image, img. The text to be added
consists of a list of tuples where each tuple is a row of text to be
added.
"""
for i, values in enumerate(text_data):
for j, item in enumerate(values):
if type(item) == float:
msg = '{0:1.2f}'.format(item)
else:
msg = '{0}'.format(item)
pos = (self.x_start + j * self.x_step,
self.y_start + i * self.y_step)
cv.PutText(img, msg, pos, self.font, font_color)
class AVI_Writer(object):
def __init__(self,topic,frame_rate=30.0):
self.topic = topic
self.frame_rate = frame_rate
self.start_t = 0.0
self.current_t = 0.0
self.progress_t = 0.0
self.frame_count = 0
self.recording_message = 'stopped'
self.writer = None
self.done = True
self.cv_image_size = None
self.filename = os.path.join(os.environ['HOME'],'default.avi')
self.lock = threading.Lock()
self.bridge = CvBridge()
rospy.init_node('avi_writer')
self.node_name = rospy.get_name()
# Set up publications
self.progress_msg = RecordingProgressMsg()
self.progress_pub = rospy.Publisher('progress',RecordingProgressMsg)
# Subscribe to messages
self.image_sub = rospy.Subscriber(self.topic,Image,self.image_handler)
# Set up services
self.recording_srv = rospy.Service(
'{0}/recording_cmd'.format(self.node_name),
RecordingCmd,
self.handle_recording_cmd
)
def run(self):
rospy.spin()
def handle_recording_cmd(self,req):
"""
Handles avi recording commands - starts and stops avi recording.
"""
with self.lock:
if self.cv_image_size is None:
# If we don't have and image yet - we can't get started, return fail.
return RecordingCmdResponse(False)
self.filename = req.filename
self.frame_rate = req.frame_rate
command = req.command.lower()
if command == 'start':
# Get start time and create video writer
self.writer = cv.CreateVideoWriter(
self.filename,
cv.CV_FOURCC('D','I','V','X'),
self.frame_rate,
self.cv_image_size,
)
if self.writer is None:
response = False
else:
response = True
self.start_t = rospy.get_time()
self.frame_count = 0
self.done = False
self.recording_message = 'recording'
elif command == 'stop':
self.done = True
del self.writer
self.writer = None
self.recording_message = 'stopped'
response = True
return RecordingCmdResponse(response)
def image_handler(self,data):
"""
Writes frames to avi file.
"""
self.current_t = rospy.get_time()
# Convert to opencv image and then to ipl_image
cv_image = self.bridge.imgmsg_to_cv(data,'bgr8')
ipl_image = cv.GetImage(cv_image)
with self.lock:
if self.cv_image_size == None:
self.cv_image_size = cv.GetSize(cv_image)
if not self.done:
# Write video frame
cv.WriteFrame(self.writer,ipl_image)
# Update times and frame count - these are used elsewhere so we
# need the lock
with self.lock:
self.frame_count += 1
self.progress_t = self.current_t - self.start_t
# Publish progress message
with self.lock:
self.progress_msg.frame_count = self.frame_count
self.progress_msg.progress_t = self.progress_t
self.progress_msg.recording_message = self.recording_message
self.progress_pub.publish(self.progress_msg)
print 'This utility is used for publishing'
print 'OpenCV accessible camera images over'
print 'ROS.\n'
print 'Usage ./ros_camera OPENCV_ID'
print 'where OPENCV_ID is a number >= 0'
print 'representing opencv\'s index for '
print 'the particular device'
camera_id = int(sys.argv[1])
topic_name = 'cvcamera' + str(camera_id)
image_pub = rospy.Publisher(topic_name, Image)
rospy.init_node('cvcamera', anonymous=True)
capture = cv.CaptureFromCAM(camera_id)
bridge = CvBridge()
print 'Opening OpenCV camera with ID', camera_id
print 'Publishing on topic', topic_name
while not rospy.is_shutdown():
try:
cv_image = cv.CloneImage(cv.QueryFrame(capture))
rosimage = bridge.cv_to_imgmsg(cv_image, "bgr8")
image_pub.publish(rosimage)
except rospy.exceptions.ROSSerializationException, e:
print 'serialization exception'
except CvBridgeError, e:
print e
break
except KeyboardInterrupt:
print "Shutting down."
class HomographyCalibratorNode(object):
def __init__(self, topic):
self.ready = False
self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED
self.topic = topic
self.bridge = CvBridge()
self.lock = threading.Lock()
rospy.init_node('homography_calibrator')
self.node_name = rospy.get_name()
# Initialize data lists
self.blobs_list = []
self.image_points = []
self.world_points = []
# Set active target information and turn off leds
target_info = mct_active_target.active_target_info()
self.led_n_max = target_info[0]
self.led_m_max = target_info[1]
self.number_of_leds = self.led_n_max * self.led_m_max
self.led_max_power = target_info[2]
self.led_space_x = target_info[3]
self.led_space_y = target_info[3]
mct_active_target.off()
# Current led indices
self.led_n = 0
self.led_m = 0
# Led power setting
params_namespace = '/homography_calibrator_params'
self.led_power = rospy.get_param(
'{0}/target/led_power'.format(params_namespace), 10)
# Wait count for image acquisition
self.image_wait_number = rospy.get_param(
'{0}/image_wait_number'.format(params_namespace), 4)
self.image_wait_cnt = 0
# Sleep periods for idle and wait count loops
self.idle_sleep_dt = 0.25
self.wait_sleep_dt = 0.005
# Initialize blob finder
self.blobFinder = BlobFinder()
self.blobFinder.threshold = rospy.get_param(
'{0}/blob_finder/threshold'.format(params_namespace), 200)
self.blobFinder.filter_by_area = rospy.get_param(
'{0}/blob_finder/filter_by_area'.format(params_namespace), False)
self.blobFinder.min_area = rospy.get_param(
'{0}/blob_finder/min_area'.format(params_namespace), 0)
self.blobFinder.max_area = rospy.get_param(
'{0}/blob_finder/max_area'.format(params_namespace), 200)
# Initialize homography matrix and number of points required to solve for it
self.homography_matrix = None
self.num_points_required = rospy.get_param(
'{0}/num_points_required'.format(params_namespace), 10)
# Set font and initial image information
self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,
0.8,
0.8,
thickness=1)
self.image_info = 'no data'
# Subscription to image topic
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_callback)
# Publications - blobs image and calibration progress image
self.image_blobs_pub = rospy.Publisher('image_homography_blobs', Image)
self.image_calib_pub = rospy.Publisher('image_homography_calibration',
Image)
# Services
self.start_srv = rospy.Service('{0}/start'.format(self.node_name),
GetFlagAndMessage,
self.handle_start_srv)
self.get_matrix_srv = rospy.Service(
'{0}/get_matrix'.format(self.node_name), GetMatrix,
self.handle_get_matrix_srv)
self.is_calibrated_srv = rospy.Service(
'{0}/is_calibrated'.format(self.node_name), GetBool,
self.handle_is_calibrated_srv)
self.ready = True
def handle_start_srv(self, req):
"""
Handles to start/restart the homography calibration procedure.
"""
flag = True
message = ''
with self.lock:
flag, message = mct_active_target.lock(self.node_name)
if flag:
self.homography_matrix = None
self.image_info = ''
self.image_points = []
self.world_points = []
self.blobs_list = []
self.state = WORKING
self.led_n = 0
self.led_m = 0
return GetFlagAndMessageResponse(flag, message)
def handle_get_matrix_srv(self, req):
"""
Returns the homography matrix. If the homography matrix has not yet be computed the
identity matirx is returned.
"""
if self.homography_matrix is None:
data = [1, 0, 0, 0, 1, 0, 0, 0, 1]
else:
data = self.homography_matrix.reshape((9, ))
return GetMatrixResponse(3, 3, data)
def handle_is_calibrated_srv(self, req):
"""
Handles requests for whether or not the homography calibration has been completed.
"""
if self.homography_matrix is not None:
value = True
else:
value = False
return GetBoolResponse(value)
def image_callback(self, data):
"""
Image topic callback function. Uses the blobFinder to find the blobs
(leds) in the image. Publishes images showing the blobs and the
calibration progress.
"""
# Find blobs
with self.lock:
if not self.ready:
return
if self.state == WORKING:
self.blobs_list, blobs_image = self.blobFinder.findBlobs(data)
blobs_rosimage = self.bridge.cv_to_imgmsg(
blobs_image, encoding="passthrough")
else:
self.blobs_list = []
blobs_rosimage = data
if self.image_wait_cnt < self.image_wait_number:
self.image_wait_cnt += 1
self.image_blobs_pub.publish(blobs_rosimage)
# Create calibration data image
cv_image = self.bridge.imgmsg_to_cv(data,
desired_encoding="passthrough")
ipl_image = cv.GetImage(cv_image)
calib_image = cv.CreateImage(cv.GetSize(ipl_image), cv.IPL_DEPTH_8U, 3)
cv.CvtColor(ipl_image, calib_image, cv.CV_GRAY2BGR)
# Add calibration markers to image
color = STATE_COLOR[self.state]
for x, y in self.image_points:
cv.Circle(calib_image, (int(x), int(y)), 3, color)
## Add text to image
message = [STATE_MESSAGE[self.state]]
if self.state == WORKING or self.state == FINISHED:
#message.append('{0}/{1} pts'.format(len(self.image_points),self.number_of_leds))
message.append('{0}/{1} pts'.format(len(self.image_points),
self.get_led_count()))
if self.image_info:
message.append('- {0}'.format(self.image_info))
message = ' '.join(message)
cv.PutText(calib_image, message, (10, 25), self.cv_text_font, color)
# Publish calibration progress image
calib_rosimage = self.bridge.cv_to_imgmsg(calib_image,
encoding="passthrough")
self.image_calib_pub.publish(calib_rosimage)
def wait_for_images(self):
"""
Wait for 'image_wait_number' of images to be acquired.
"""
with self.lock:
self.image_wait_cnt = 0
done = False
while not done:
with self.lock:
image_wait_cnt = self.image_wait_cnt
if image_wait_cnt >= self.image_wait_number:
done = True
rospy.sleep(self.wait_sleep_dt)
def increment_led(self):
"""
Increments the led array indices. When the last led is reached check to see if
sufficient points have been captured for the homography calibratoin. If so, then
the state is changed to FINISHED. If insufficient points have been gathered then
the state is changed back to WAITING.
"""
if self.led_m < self.led_m_max - 1 or self.led_n < self.led_n_max - 1:
if self.led_n < self.led_n_max - 1:
self.led_n += 1
else:
self.led_n = 0
self.led_m += 1
else:
# Turn off led and unlock active target
mct_active_target.off()
mct_active_target.unlock(self.node_name)
# Need to check that we got enough done otherwise return to idle.
if len(self.image_points) >= self.num_points_required:
self.state = FINISHED
self.image_info = ''
else:
self.state = WAITING
self.image_info = 'not enough data'
self.homography_matrix = None
def get_led_count(self):
return self.led_n + self.led_m * self.led_n_max + 1
def run(self):
"""
Node main function. When the state is WORKING this function activates the leds on the
active calibration target one at a time. When all of the leds have been activated and
if sufficient number of points have been collected the homography matrix is calculated.
"""
while not rospy.is_shutdown():
if self.state == WORKING:
# Turn on current led and wait for images
mct_active_target.set_led(self.led_n, self.led_m,
self.led_power)
self.wait_for_images()
if len(self.blobs_list) == 1:
# Get centroid of blob and add to image point list
blob = self.blobs_list[0]
image_x = blob['centroid_x']
image_y = blob['centroid_y']
self.image_points.append((image_x, image_y))
# Compute coordinates of the led in the world plane
world_x = self.led_n * self.led_space_x
world_y = self.led_m * self.led_space_y
self.world_points.append((world_x, world_y))
self.increment_led()
elif self.state == FINISHED and self.homography_matrix is None:
# Find the homography transformation
image_points = numpy.array(self.image_points)
world_points = numpy.array(self.world_points)
result = cv2.findHomography(image_points, world_points,
cv.CV_RANSAC)
self.homography_matrix, mask = result
# Compute the mean reprojection error
image_points_hg = numpy.ones((image_points.shape[0], 3))
image_points_hg[:, :2] = image_points
world_points_hg = numpy.ones((world_points.shape[0], 3))
world_points_hg[:, :2] = world_points
homography_matrix_t = self.homography_matrix.transpose()
world_points_hg_pred = numpy.dot(image_points_hg,
homography_matrix_t)
denom = numpy.zeros((world_points.shape[0], 2))
denom[:, 0] = world_points_hg_pred[:, 2]
denom[:, 1] = world_points_hg_pred[:, 2]
world_points_pred = world_points_hg_pred[:, :2] / denom
error = (world_points - world_points_pred)**2
error = error.sum(axis=1)
error = numpy.sqrt(error)
error = error.mean()
self.image_info = 'error {0:1.2f} mm'.format(1e3 * error)
else:
rospy.sleep(self.idle_sleep_dt)
initial_centers = fmat[:, center_indices]
kresults = vq.kmeans(np.array(fmat.T), np.array(initial_centers.T))
return np.matrix(kresults[0]).T
if __name__ == '__main__':
import sys
import pdb
features_file = sys.argv[1]
images_file = sys.argv[2]
features_db = np.column_stack([ut.load_pickle(p[0]) for p in csv_bag_names(features_file)])
features_db_reduced = features_mat_compress(features_db, 500)
#Generate a random color for each feature
colors = np.matrix(np.random.randint(0, 255, (3, features_db_reduced.shape[1])))
features_tree = sp.KDTree(np.array(features_db_reduced.T))
bridge = CvBridge()
forearm_cam_l = '/l_forearm_cam/image_rect_color'
cv.NamedWindow('surf', 1)
#import pdb
#while not rospy.is_shutdown():
i = 0
for topic, msg, t in ru.bag_iter(images_file, [forearm_cam_l]):
image = bridge.imgmsg_to_cv(msg, 'bgr8')
#image = camera.get_frame()
image_gray = fea.grayscale(image)
surf_keypoints, surf_descriptors = fea.surf(image_gray)
#print len(surf_keypoints)
#pdb.set_trace()
p.add_option('-c', action='store', default='/wide_stereo', type='string', dest='cam', help='which camera to listen to')
p.add_option('-d', action='store', default=30, type='int', dest='dist', help='separation distance')
p.add_option('-s', action='store_true', dest='headless', help='headless mode')
opt, args = p.parse_args()
cameras = [opt.cam + '/left/image_rect_color',
opt.cam + '/right/image_rect_color']
stereo_listener = rc.ROSStereoListener(cameras)
if not opt.headless:
#cv.NamedWindow('left', 0)
#cv.NamedWindow('right', 0)
cv.NamedWindow('stereo-anaglyph', 0)
cv.ResizeWindow('stereo-anaglyph', 640, 480)
cv.WaitKey(10)
else:
bridge = CvBridge()
image_pub = rospy.Publisher('stereo_anaglyph', Image)
anaglyph_cyan_image_distance_correction = rospy.get_param('anaglyph_dist', opt.dist)
left = 1113937# 65361
right = 1113939#65363
escape = 1048603#27
while not rospy.is_shutdown():
l, r = stereo_listener.next()
red_blue = anaglyph(l, r, anaglyph_cyan_image_distance_correction)
if not opt.headless:
#cv.ShowImage('left', l)
#cv.ShowImage('right', r)
cv.ShowImage('stereo-anaglyph', red_blue)
k = cv.WaitKey(10)
def __init__(self, topic):
self.ready = False
self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED
self.topic = topic
self.bridge = CvBridge()
self.lock = threading.Lock()
rospy.init_node('homography_calibrator')
self.node_name = rospy.get_name()
# Initialize data lists
self.blobs_list = []
self.image_points = []
self.world_points = []
# Set active target information and turn off leds
target_info = mct_active_target.active_target_info()
self.led_n_max = target_info[0]
self.led_m_max = target_info[1]
self.number_of_leds = self.led_n_max * self.led_m_max
self.led_max_power = target_info[2]
self.led_space_x = target_info[3]
self.led_space_y = target_info[3]
mct_active_target.off()
# Current led indices
self.led_n = 0
self.led_m = 0
# Led power setting
params_namespace = '/homography_calibrator_params'
self.led_power = rospy.get_param(
'{0}/target/led_power'.format(params_namespace), 10)
# Wait count for image acquisition
self.image_wait_number = rospy.get_param(
'{0}/image_wait_number'.format(params_namespace), 4)
self.image_wait_cnt = 0
# Sleep periods for idle and wait count loops
self.idle_sleep_dt = 0.25
self.wait_sleep_dt = 0.005
# Initialize blob finder
self.blobFinder = BlobFinder()
self.blobFinder.threshold = rospy.get_param(
'{0}/blob_finder/threshold'.format(params_namespace), 200)
self.blobFinder.filter_by_area = rospy.get_param(
'{0}/blob_finder/filter_by_area'.format(params_namespace), False)
self.blobFinder.min_area = rospy.get_param(
'{0}/blob_finder/min_area'.format(params_namespace), 0)
self.blobFinder.max_area = rospy.get_param(
'{0}/blob_finder/max_area'.format(params_namespace), 200)
# Initialize homography matrix and number of points required to solve for it
self.homography_matrix = None
self.num_points_required = rospy.get_param(
'{0}/num_points_required'.format(params_namespace), 10)
# Set font and initial image information
self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,
0.8,
0.8,
thickness=1)
self.image_info = 'no data'
# Subscription to image topic
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_callback)
# Publications - blobs image and calibration progress image
self.image_blobs_pub = rospy.Publisher('image_homography_blobs', Image)
self.image_calib_pub = rospy.Publisher('image_homography_calibration',
Image)
# Services
self.start_srv = rospy.Service('{0}/start'.format(self.node_name),
GetFlagAndMessage,
self.handle_start_srv)
self.get_matrix_srv = rospy.Service(
'{0}/get_matrix'.format(self.node_name), GetMatrix,
self.handle_get_matrix_srv)
self.is_calibrated_srv = rospy.Service(
'{0}/is_calibrated'.format(self.node_name), GetBool,
self.handle_is_calibrated_srv)
self.ready = True
def __init__(self, topics, rate=30.0, name='stereo_listener'):
self.listener = ru.GenericListener(name, [Image, Image], topics, rate)
self.lbridge = CvBridge()
self.rbridge = CvBridge()
