def main():
# create ros camera info messages
l_info_manager = camera_info_manager.CameraInfoManager(
cname=LEFT_CAM,
url="file://%s/left.yaml" % CALIBRATION_DATA,
namespace=LEFT_CAM)
l_info_manager.loadCameraInfo()
l_info_message = l_info_manager.getCameraInfo()
r_info_manager = camera_info_manager.CameraInfoManager(
cname=RIGHT_CAM,
url="file://%s/right.yaml" % CALIBRATION_DATA,
namespace=RIGHT_CAM)
r_info_manager.loadCameraInfo()
r_info_message = r_info_manager.getCameraInfo()
# left camera publishers
l_img_pub = rospy.Publisher("%s/image_raw" % LEFT_CAM, ImageMsg, queue_size=QUEUE_SIZE)
l_info_pub = rospy.Publisher("%s/camera_info" % LEFT_CAM, CameraInfo, queue_size=QUEUE_SIZE)
# right camera publishers
r_img_pub = rospy.Publisher("%s/image_raw" % RIGHT_CAM, ImageMsg, queue_size=QUEUE_SIZE)
r_info_pub = rospy.Publisher("%s/camera_info" % RIGHT_CAM, CameraInfo, queue_size=QUEUE_SIZE)
# initialize node
rospy.init_node("stereo_image_stream", anonymous=True)
cap = cv2.VideoCapture(-1)
bridge = CvBridge()
while not rospy.is_shutdown():
# preprocess stereo images
_, raw_image = cap.read()
l_img, r_img = split_image(raw_image)
rospy.loginfo("Read Cv Image: " + str(raw_image.shape))
# create ros image messages
l_img_message = bridge.cv2_to_imgmsg(l_img, encoding="bgr8")
r_img_message = bridge.cv2_to_imgmsg(r_img, encoding="bgr8")
# set headers
l_img_message.header.frame_id = HEADER_ID
r_img_message.header.frame_id = HEADER_ID
l_info_message.header.frame_id = HEADER_ID
r_info_message.header.frame_id = HEADER_ID
# publish messages
l_img_pub.publish(l_img_message)
r_img_pub.publish(r_img_message)
l_info_pub.publish(l_info_message)
r_info_pub.publish(r_info_message)
cap.release()
cv2.destroyAllWindows()
def __init__(self, hostname, username, password, frame_id, camera_info_url,
prefix):
self.hostname = hostname
self.username = username
self.password = password
self.frame_id = frame_id
self.camera_info_url = camera_info_url
self.msg = CompressedImage()
self.msg.header.frame_id = self.frame_id
self.msg.format = "jpeg"
self.valid = False
self.updated = False
# generate a valid camera name based on the hostname
self.cname = camera_info_manager.genCameraName(self.hostname)
self.cinfo = camera_info_manager.CameraInfoManager(
cname=self.cname, url=self.camera_info_url, namespace=prefix)
self.cinfo.loadCameraInfo() # required before getCameraInfo()
self.st = StreamThread(self)
self.st.start()
self.pub = rospy.Publisher(prefix + "/image_raw/compressed",
CompressedImage, self)
self.caminfo_pub = rospy.Publisher(prefix + "/camera_info", CameraInfo,
self)
self.detect_dim()
def __init__(self):
self.folder = rospy.get_param("~folder", "")
self.encoding = rospy.get_param("~encoding", "bgr8")
self.period = rospy.get_param("~update_period", 0.15)
rospy.loginfo('update period {}'.format(self.period))
self.frame = rospy.get_param("~frame", "image_folder")
self.camera_info_pub = None
if True:
self.camera_info_manager = camera_info_manager.CameraInfoManager(
"tbd_camera_name")
self.camera_info_url = rospy.get_param("~camera_info_url", "")
if self.camera_info_url != "":
rv = self.camera_info_manager.setURL(self.camera_info_url)
if not rv:
rospy.logerr("bad url " + self.camera_info_url)
else:
self.camera_info_manager.loadCameraInfo()
self.camera_info_pub = rospy.Publisher("camera_info",
CameraInfo,
queue_size=1)
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("image", Image, queue_size=3)
self.image_list = []
self.image_ind = 1
self.timer = rospy.Timer(rospy.Duration(self.period), self.update)
def __init__(self, hostname, username, password, width, height, frame_id,
camera_info_url, use_encrypted_password, camera):
self.hostname = hostname
self.username = username
self.password = password
self.width = width
self.height = height
self.frame_id = frame_id
self.camera_info_url = camera_info_url
self.use_encrypted_password = use_encrypted_password
self.camera = camera
# generate a valid camera name based on the hostname
self.cname = camera_info_manager.genCameraName(self.hostname)
self.cinfo = camera_info_manager.CameraInfoManager(
cname=self.cname, url=self.camera_info_url)
self.cinfo.loadCameraInfo() # required before getCameraInfo()
self.st = None
self.pub = rospy.Publisher("image_raw/compressed",
CompressedImage,
self,
queue_size=1)
self.caminfo_pub = rospy.Publisher("camera_info",
CameraInfo,
self,
queue_size=1)
def rosSetup(self):
"""
Creates and setups ROS components
"""
self.cinfo = camera_info_manager.CameraInfoManager(
cname=self.camera_model,
url=self.camera_info_url,
namespace=rospy.get_name())
self.cinfo.loadCameraInfo()
# Mirar de cambiar por ImageTransport
self.compressed_image_publisher = rospy.Publisher(
"%scompressed" % rospy.get_namespace(),
CompressedImage,
self,
queue_size=10)
self.caminfo_publisher = rospy.Publisher("%scamera_info" %
rospy.get_namespace(),
CameraInfo,
self,
queue_size=10)
# Sets the url
self.url = 'http://%s/axis-cgi/mjpg/video.cgi?streamprofile=%s&camera=%d&fps=%d&compression=%d' % (
self.hostname, self.profile, self.camera_number, self.fps,
self.compression)
rospy.loginfo('Axis:rosSetup: Camera %s (%s:%d): url = %s, PTZ %s' %
(self.camera_model, self.hostname, self.camera_number,
self.url, self.ptz))
self.encodedstring = base64.encodestring(self.username + ":" +
str(self.password))[:-1]
self.auth = "Basic %s" % self.encodedstring
# Diagnostic Updater
self.diagnostics_updater = diagnostic_updater.Updater()
self.diagnostics_updater.setHardwareID(
"%s-%s:%s" % (self.camera_model, self.camera_id, self.hostname))
self.diagnostics_updater.add("Video stream updater",
self.getStreamDiagnostic)
#self.diagnostics_updater.add("Video stream frequency", self.getStreamFrequencyDiagnostic)
if self.fps == 0:
freq_bounds = {'min': 0.5, 'max': 100}
else:
freq_bounds = {'min': self.fps, 'max': self.fps}
self.image_pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic(
"%scompressed" % rospy.get_namespace(), self.diagnostics_updater,
diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.01, 1))
if self.ptz:
# sets the ptz interface
self.ptz_interface.rosSetup()
self.diagnostics_updater.add("Ptz state updater",
self.ptz_interface.getStateDiagnostic)
# Creates a periodic callback to publish the diagnostics at desired freq
self.diagnostics_timer = rospy.Timer(rospy.Duration(1.0),
self.publishDiagnostics)
def main():
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
image_pub = rospy.Publisher("/pidrone/picamera/image_raw",
Image,
queue_size=1,
tcp_nodelay=False)
camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info",
CameraInfo,
queue_size=1,
tcp_nodelay=False)
cim = camera_info_manager.CameraInfoManager(
"picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
try:
bridge = CvBridge()
with picamera.PiCamera(framerate=90) as camera:
camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
with AnalyzeFlow(camera) as flow_analyzer:
flow_analyzer.setup(camera.resolution)
phase_analyzer = Localizer(camera, bridge)
rospy.Subscriber('/pidrone/reset_transform', Empty,
phase_analyzer.reset_callback)
rospy.Subscriber('/pidrone/state', State,
phase_analyzer.state_callback)
camera.start_recording("/dev/null",
format='h264',
splitter_port=1,
motion_output=flow_analyzer)
print "Starting Flow"
camera.start_recording(phase_analyzer,
format='bgr',
splitter_port=2)
last_time = None
while not rospy.is_shutdown():
camera.wait_recording(1 / 100.0)
if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time:
image_message = bridge.cv2_to_imgmsg(
phase_analyzer.prev_img, encoding="bgr8")
image_message.header.stamp = phase_analyzer.prev_rostime
last_time = phase_analyzer.prev_rostime
image_pub.publish(image_message)
camera_info_pub.publish(cim.getCameraInfo())
camera.stop_recording(splitter_port=1)
camera.stop_recording(splitter_port=2)
print "Shutdown Received"
except Exception:
print "Camera Error!!"
raise
def main():
rospy.init_node('flow_pub')
image_pub = rospy.Publisher("/pidrone/picamera/image_raw",
Image,
queue_size=1,
tcp_nodelay=False)
camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info",
CameraInfo,
queue_size=1,
tcp_nodelay=False)
cim = camera_info_manager.CameraInfoManager(
"picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
try:
velocity = axes_err()
bridge = CvBridge()
with picamera.PiCamera(framerate=90) as camera:
camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
with AnalyzeFlow(camera) as flow_analyzer:
flow_analyzer.setup(camera.resolution)
phase_analyzer = AnalyzePhase(camera, bridge)
camera.start_recording("/dev/null",
format='h264',
splitter_port=1,
motion_output=flow_analyzer)
print "Starting Flow"
camera.start_recording(phase_analyzer,
format='bgr',
splitter_port=2)
last_time = None
while not rospy.is_shutdown():
camera.wait_recording(1 / 100.0)
if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time:
image_message = bridge.cv2_to_imgmsg(
phase_analyzer.prev_img, encoding="bgr8")
image_message.header.stamp = phase_analyzer.prev_rostime
# print "stamp", image_message.header.stamp
last_time = phase_analyzer.prev_rostime
image_pub.publish(image_message)
camera_info_pub.publish(cim.getCameraInfo())
camera.stop_recording(splitter_port=1)
camera.stop_recording(splitter_port=2)
print "Shutdown Received"
sys.exit()
except Exception as e:
raise
def __init__(self):
self.ci = cim.CameraInfoManager(cname='my_camera', namespace='camera')
self.image_sub = rospy.Subscriber("image_topic", Image, self.callback)
rospy.init_node('CameraDriver', anonymous=False)
self.pubInfo = rospy.Publisher('/camera/camera_info',
CameraInfo,
queue_size=10)
self.pubImg = rospy.Publisher('/camera/image_raw',
Image,
queue_size=10)
def __init__(self, _prefix, camera_name, info_prefix, cinfo_url):
self.name = camera_name
self.uri = cinfo_url
self.prefix = _prefix
self.cname = camera_info_manager.genCameraName(self.prefix)
rospy.loginfo("cname = " + self.cname)
self.cinfo = camera_info_manager.CameraInfoManager(
cname=_prefix, url=self.uri, namespace=info_prefix)
self.caminfo_pub = rospy.Publisher(info_prefix + "/camera_info",
CameraInfo, self)
self.cinfo.loadCameraInfo()
rospy.Subscriber(self.prefix + self.name, Image, self.image_callback)
def main():
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
image_pub = rospy.Publisher("/pidrone/picamera/image_raw",
Image,
queue_size=1,
tcp_nodelay=False)
camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info",
CameraInfo,
queue_size=1,
tcp_nodelay=False)
cim = camera_info_manager.CameraInfoManager(
"picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
try:
bridge = CvBridge()
with picamera.PiCamera(resolution=(CAMERA_WIDTH, CAMERA_HEIGHT),
framerate=30) as camera:
phase_analyzer = AnalyzePhase(camera, bridge)
print "Starting Flow"
camera.start_recording(phase_analyzer,
format='bgr',
splitter_port=1)
last_time = None
while not rospy.is_shutdown():
camera.wait_recording(1 / 30.0)
if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time:
image_message = bridge.cv2_to_imgmsg(
phase_analyzer.prev_img, encoding="bgr8")
image_message.header.stamp = phase_analyzer.prev_rostime
# print "stamp", image_message.header.stamp
last_time = phase_analyzer.prev_rostime
image_pub.publish(image_message)
camera_info_pub.publish(cim.getCameraInfo())
camera.stop_recording(splitter_port=1)
print "Shutdown Recieved"
sys.exit()
except Exception as e:
raise
def __init__(self, url):
try:
self.stream = urllib.urlopen(url)
except:
rospy.logerr('Unable to open camera stream: ' + str(url))
sys.exit() #'Unable to open camera stream')
self.bytes = ''
self.width = 640
self.height = 480
self.frame_id = 'camera'
self.image_pub = rospy.Publisher("camera/image_raw", Image)
self.cinfo = camera_info_manager.CameraInfoManager(
cname='camera', url='PACKAGE://ipcam/config/calibration.yaml')
self.cinfo.loadCameraInfo() # required before getCameraInfo()
self.caminfo_pub = rospy.Publisher("camera/camera_info", CameraInfo)
self.bridge = CvBridge()
def __init__(self):
self.freq = rospy.get_param("~freq", DEFAULT_FREQUENCY)
self.cname = rospy.get_param("~cname", DEFAULT_CNAME)
self.url = rospy.get_param("~url", DEFAULT_URL)
self.namespace = rospy.get_param("~namespace", DEFAULT_NAMESPACE)
self.cam_manager = cim.CameraInfoManager(cname=self.cname,
url=self.url,
namespace=self.namespace)
try:
self.cam_manager.loadCameraInfo()
rospy.loginfo("Camera name = %s", self.cam_manager.getCameraName())
except IOError as e:
rospy.logerr(
"Tried to load camera calibration data, but it is unreadable. Error = %s",
e.message)
rospy.signal_shutdown("Could not read calibration info")
except cim.CameraInfoMissingError as e:
rospy.logerr("Camera info missing. Error = %s", e.message)
rospy.logerr(
"Node will continue to run, but you will need to call set_camera_info service to fill out camera data"
)
try:
if not self.cam_manager.isCalibrated():
rospy.logwarn(
"No exceptions during startup, but camera calibration data is using default values!"
)
rospy.loginfo("Calibration info: ")
print(self.cam_manager.getCameraInfo())
else:
rospy.loginfo(
"No errors during the loading of the camera data -- should be publishing okay."
)
except cim.CameraInfoMissingError as e:
rospy.logerr(
"Even after trying to load camera info, we still don't have calibration information available"
)
rospy.signal_shutdown(
"Can't even provide default calibration data... shutting down")
self.pub = rospy.Publisher("camera_info", cim.CameraInfo, queue_size=1)
self.timer = rospy.Timer(rospy.Duration(1.0 / self.freq), self.timercb)
return
def camera_driver():
pub = rospy.Publisher('camera_pi/image_raw', Image, queue_size=10)
pub2 = rospy.Publisher('camera_pi/camera_info', CameraInfo, queue_size=10)
rospy.init_node('camera_driver', anonymous=True)
rate = rospy.Rate(24)
picam = camera_info_manager.CameraInfoManager(
cname='camera_pi',
url='file:///home/p2p/.ros/camera_info/camera_pi.yaml',
namespace='camera_pi')
picam.loadCameraInfo()
while not rospy.is_shutdown():
image = np.empty((h[1], h[0], 3), dtype=np.uint8).flatten()
camera.capture(image, format='bgr', use_video_port=True)
image = image.reshape([h[1], h[0], 3])
image = bridge.cv2_to_imgmsg(image, 'bgr8')
pub.publish(image)
pub2.publish(picam.getCameraInfo())
rate.sleep()
def __init__(self, url, config):
# Logging if the camera is opened or not
try:
self.stream = urllib.urlopen(url)
rospy.loginfo('Opened camera stream: ' + str(url))
except:
rospy.logerr('Unable to open camera stream: ' + str(url))
sys.exit() #'Unable to open camera stream')
# Initialize camera variables
self.bytes = ''
self.width = 640
self.height = 480
self.frame_id = 'camera'
self.image_pub = rospy.Publisher("/camera/image_raw", Image)
self.cinfo = camera_info_manager.CameraInfoManager(cname='camera',
url=config)
self.cinfo.loadCameraInfo() # required before getCameraInfo()
self.caminfo_pub = rospy.Publisher("/camera/camera_info", CameraInfo)
self.bridge = CvBridge()
def __init__(self, filename, frame_id, camera_info_url, prefix):
self.filename = filename
self.frame_id = frame_id
self.camera_info_url = camera_info_url
self.msg = CompressedImage()
self.msg.header.frame_id = self.frame_id
self.msg.format = "jpeg"
im = cv2.imread(filename)
self.width=im.shape[1]
self.height=im.shape[0]
f=open(filename, 'r+')
self.msg.data = f.read()
self.valid = True;
self.updated = True;
# generate a valid camera name based on the hostname
self.cname = camera_info_manager.genCameraName(filename)
self.cinfo = camera_info_manager.CameraInfoManager(cname = self.cname,
url = self.camera_info_url, namespace = prefix)
self.cinfo.loadCameraInfo() # required before getCameraInfo()
self.st = None
self.pub = rospy.Publisher(prefix + "/image_raw/compressed", CompressedImage, self)
self.caminfo_pub = rospy.Publisher(prefix + "/camera_info", CameraInfo, self)
def main():
parser = argparse.ArgumentParser(description='Estimate the drone\'s pose with SLAM or localization')
# Arguments to determine if the throttle command is being used. E.g.:
# rosrun topic_tools throttle messages /pidrone/infrared 40.0
parser.add_argument('--SLAM', action='store_true',
help=('Do you want to do SLAM?'))
parser.add_argument('--offline', action='store_true',
help=('Run offline?'))
args = parser.parse_args()
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False)
camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False)
cim = camera_info_manager.CameraInfoManager("picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
try:
bridge = CvBridge()
with picamera.PiCamera(framerate=90) as camera:
camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
with AnalyzeFlow(camera) as flow_analyzer:
flow_analyzer.setup(camera.resolution)
if args.SLAM:
if args.offline:
phase_analyzer = MATL(camera, bridge)
rospy.Subscriber('/pidrone/map', Empty, phase_analyzer.map_callback)
else:
phase_analyzer = SLAM(camera, bridge)
else:
phase_analyzer = Localizer(camera, bridge)
rospy.Subscriber('/pidrone/reset_transform', Empty, phase_analyzer.reset_callback)
rospy.Subscriber('/pidrone/state', State, phase_analyzer.state_callback)
camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer)
print "Starting Flow"
camera.start_recording(phase_analyzer, format='bgr', splitter_port=2)
last_time = None
while not rospy.is_shutdown():
camera.wait_recording(1 / 100.0)
if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time:
image_message = bridge.cv2_to_imgmsg(phase_analyzer.prev_img, encoding="bgr8")
image_message.header.stamp = phase_analyzer.prev_rostime
last_time = phase_analyzer.prev_rostime
image_pub.publish(image_message)
camera_info_pub.publish(cim.getCameraInfo())
camera.stop_recording(splitter_port=1)
camera.stop_recording(splitter_port=2)
print "Shutdown Received"
except Exception:
print "Camera Error!!"
raise
def reconfigure(self, new_config, level):
"""
Reconfigure the camera
"""
rospy.loginfo('reconfigure changed')
if self.pub_img_.get_num_connections() == 0:
rospy.loginfo(
'Changes recorded but not applied as nobody is subscribed to the ROS topics.'
)
self.config.update(new_config)
return self.config
# check if we are even subscribed to a camera
is_camera_new = self.nameId is None
if is_camera_new:
rospy.loginfo(
'subscribed to camera proxy, since this is the first camera')
self.nameId = self.camProxy.subscribeCamera(
"rospy_gvm", new_config['source'], new_config['resolution'],
new_config['color_space'], new_config['frame_rate'])
if self.config['source'] != new_config['source'] or is_camera_new:
rospy.loginfo('updating camera source information')
if new_config['source'] == kTopCamera:
self.frame_id = self.config['camera_top_frame']
elif new_config['source'] == kBottomCamera:
self.frame_id = self.config['camera_bottom_frame']
elif new_config['source'] == kDepthCamera:
self.frame_id = self.config['camera_depth_frame']
else:
rospy.logerr('Invalid source. Must be 0, 1 or 2')
exit(1)
# check if the camera changed
if self.config['camera_info_url'] == "" or \
( self.config['camera_info_url'] != new_config['camera_info_url'] and \
new_config['camera_info_url'] not in self.camera_infos ):
self.load_camera_info()
if 'cim' not in self.__dict__:
self.cim = camera_info_manager.CameraInfoManager(
cname='nao_camera')
if not self.cim.setURL(new_config['camera_info_url']):
rospy.logerr('malformed URL for calibration file')
else:
try:
self.cim.loadCameraInfo()
except IOExcept:
rospy.logerr(
'Could not read from existing calibration file')
if self.cim.isCalibrated():
rospy.loginfo('Successfully loaded camera info')
self.camera_infos[
new_config['camera_info_url']] = self.cim.getCameraInfo()
else:
rospy.logerr(
'There was a problem loading the calibration file. Check the URL!'
)
# set params
camParams = self.extractParams(new_config)
self.setParams(camParams)
key_methods = [('resolution', 'setResolution'),
('color_space', 'setColorSpace'),
('frame_rate', 'setFrameRate')]
if self.get_version() >= LooseVersion('2.0'):
key_methods.append(('source', 'setActiveCamera'))
for key, method in key_methods:
if self.config[key] != new_config[key] or is_camera_new:
self.camProxy.__getattribute__(method)(self.nameId,
new_config[key])
self.config.update(new_config)
return self.config
def main():
parser = argparse.ArgumentParser(
description='Display Image Matching Regions')
args = parser.parse_args()
# [x, y, z, yaw]
pos = [0, 0, 0, 0]
angle_x = 0.0
angle_y = 0.0
# localization does not estimate z
z = 0.33
first_locate = True
#MS Local test, set to locate position immediately without waiting for web interface
# locate_position = False
locate_position = True
map_counter = 0
max_map_counter = 0
# constant
alpha_yaw = 0.1 # perceived yaw smoothing alpha
hybrid_alpha = 0.7 # blend position with first frame and int # MS was 0.3
node_name = os.path.splitext(os.path.basename(__file__))[0]
cim = camera_info_manager.CameraInfoManager(
"picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
try:
bridge = CvBridge()
# Create the in-memory stream
stream = io.BytesIO()
with picamera.PiCamera(framerate=90) as camera:
camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
with picamera.array.PiRGBArray(camera) as stream:
detector = cv2.ORB_create(nfeatures=NUM_FEATURES,
scoreType=cv2.ORB_FAST_SCORE)
map_kp, map_des, map_image = create_map('map.jpg')
estimator = Single_Map_Localization(map_kp, map_des)
estimator.set_map_image(map_image)
print "Starting Single Map Localization"
while True: # Do forever
camera.capture(stream, format='bgr')
# At this point the image is available as stream.array
image = stream.array # Get a single image
stream.seek(0) # Rewind the stream
estimator.set_cur_image(image)
curr_kp, curr_des = detector.detectAndCompute(image, None)
img_keypoints = np.empty(
(image.shape[0], image.shape[1], 3), dtype=np.uint8)
cv2.drawKeypoints(image, curr_kp, img_keypoints)
cv2.namedWindow('Current image', cv2.WINDOW_NORMAL)
cv2.imshow('Current image', img_keypoints)
cv2.waitKey(50)
if curr_kp is not None and curr_des is not None:
new_pos, weight = estimator.update(
z, angle_x, angle_y, curr_kp, curr_des)
# update position
if new_pos is not None:
pos = [
hybrid_alpha * new_pos[0] +
(1.0 - hybrid_alpha) * pos[0],
hybrid_alpha * new_pos[1] +
(1.0 - hybrid_alpha) * pos[1], z,
alpha_yaw * new_pos[3] +
(1.0 - alpha_yaw) * pos[3]
]
print '--pose', 'x ', pos[0], 'y ', pos[
1], 'z', pos[2], 'yaw ', pos[3]
map_counter = min(map_counter + 1, -MAX_BAD_COUNT)
else:
map_counter = map_counter - 1
# if it's been a while without a significant average weight
if map_counter < MAX_BAD_COUNT:
locate_position = False
map_counter = 0
print 'Lost, leaving position control'
print 'count', map_counter
else:
print "CANNOT FIND ANY FEATURES !!!!!"
print "Shutdown Received"
except Exception:
print "Cancel Received"
raise
self._frame = new_frame
return Gst.FlowReturn.OK
if __name__ == '__main__':
rospy.init_node("bluerov_video_node")
# Create the video object
# Add port= if is necessary to use a different one
video = Video(4777)
bridge = cv_bridge.CvBridge()
img_pub = rospy.Publisher('/camera/image', Image, queue_size=10)
if pub_info:
info_pub = rospy.Publisher('/camera/camera_info', CameraInfo, queue_size=10)
info_manger = camera_info_manager.CameraInfoManager(cname='camera', namespace='camera')
info_manger.loadCameraInfo()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Wait for the next frame
if not video.frame_available():
continue
frame = video.frame()
img_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
img_msg.header.frame_id = 'camera'
img_msg.header.stamp = rospy.Time.now()
img_pub.publish(img_msg)
def publish_image():
rospy.init_node('image_publisher')
provider = rospy.get_param('~video_stream_provider')
cap = cv2.VideoCapture(provider)
if not cap.isOpened():
rospy.logerr('Failed to open video stream: {}'.format(provider))
return
flip_horz = rospy.get_param('~flip_horizontal', default=False)
flip_vert = rospy.get_param('~flip_vertical', default=False)
if flip_horz and flip_vert:
flip_code = -1
elif flip_horz:
flip_code = 1
elif flip_vert:
flip_code = 0
else:
flip_code = None
queue_size = rospy.get_param('~queue_size', default=10)
ts = np.load(rospy.get_param('~ts_file'))
ts += float(rospy.get_param('/trial_start_time', default=0.))
try:
queue = ImageQueue(cap, ts, queue_size, flip_code)
except IOError as e:
rospy.logerr('Failed to initialize queue: {}'.format(e))
return
camera_name = rospy.get_param('~camera_name')
pub = rospy.Publisher(camera_name + '/image_raw', sensor_msgs.msg.Image, queue_size=1)
bridge = cv_bridge.CvBridge()
# Set up time information: either drawn directly from the /clock topic or offset from the current time
use_manual_sim = rospy.get_param('~use_manual_sim', default=False)
offset_from_start = rospy.get_param('~offset_from_start', default=False)
if use_manual_sim:
sim_clock = SimClockDelay()
get_time = sim_clock.get_time
elif offset_from_start:
time_offset = rospy.Time.from_sec(ts[0]) - rospy.get_rostime()
get_time = lambda: rospy.get_rostime() + time_offset
else:
get_time = rospy.get_rostime
# Set up the camera calibration info
camera_info_url = rospy.get_param('~camera_info_url', default='')
camera_manager = camera_info_manager.CameraInfoManager(camera_name, camera_info_url)
camera_manager.loadCameraInfo()
# set up a camera_info publisher manually because image_transport doesn't exist in python :(
# TODO: make this use SubscribeListener instead of a loop
camera_info_pub = rospy.Publisher(camera_name + '/camera_info', sensor_msgs.msg.CameraInfo, queue_size=1)
while not rospy.is_shutdown() and not queue.is_finished:
if camera_info_pub.get_num_connections() > 0:
camera_info_pub.publish(camera_manager.getCameraInfo())
try:
t, img_raw = queue.get(block=True, timeout=1.)
tm = rospy.Time.from_sec(t)
cur_tm = get_time()
if tm < cur_tm:
rospy.logdebug("Video frame time has passed already (offset={}), discarding".format( (cur_tm - tm).to_sec()))
continue
# rospy.loginfo("got frame for time: {}".format(tm))
img = bridge.cv2_to_imgmsg(img_raw, encoding="bgr8")
img.header.stamp = tm
img.header.frame_id = camera_name
if use_manual_sim:
done = False
while not done:
try:
sim_clock.wait(tm, timeout=1.)
done = True
except RuntimeError:
pass
else:
while cur_tm < tm:
sleep_len = min(tm - cur_tm, rospy.Duration(1))
# rospy.loginfo("Waiting {} to publish frame (total: {})".format(sleep_len.to_sec(), (tm - cur_tm).to_sec()))
rospy.sleep(sleep_len)
cur_tm = rospy.get_rostime()
pub.publish(img)
except Queue.Empty:
pass
except rospy.exceptions.ROSTimeMovedBackwardsException:
rospy.loginfo('ROS time move backwards, assuming the bag restarted and reloading the queue')
queue.stop()
# rospy.loginfo('Queue stopped')
cap = cv2.VideoCapture(provider)
queue = ImageQueue(cap, ts, queue_size, flip_code)
def reconfigure(self, new_config, level):
""" Reconfigure the camera
"""
if self.pub_img_.get_num_connections() == 0:
rospy.loginfo(
'Changes recorded but not applied as nobody is subscribed to the ROS topics.'
)
self.config.update(new_config)
return self.config
# check if we are even subscribed to a camera
is_camera_new = self.nameId is None
if self.config['source'] != new_config['source'] or is_camera_new:
# unsubscribe for all zombie subscribers
self.camProxy.unsubscribeAllInstances("rospy_gvm")
# subscribe
self.nameId = self.camProxy.subscribe("rospy_gvm",
new_config['resolution'],
new_config['color_space'],
new_config['frame_rate'])
rospy.loginfo('Using camera: %d . Subscriber name is %s .' %
(new_config['source'], self.nameId))
if new_config['source'] == kTopCamera:
self.frame_id = "/CameraTop_frame"
elif new_config['source'] == kBottomCamera:
self.frame_id = "/CameraBottom_frame"
elif new_config['source'] == kDepthCamera:
self.frame_id = new_config['camera3d_frame']
else:
rospy.logerr('Invalid source. Must be 0, 1 or 2')
exit(1)
# check if the camera changed
if self.config['camera_info_url'] != new_config['camera_info_url'] and \
new_config['camera_info_url'] and new_config['camera_info_url'] not in self.camera_infos:
if 'cim' not in self.__dict__:
self.cim = camera_info_manager.CameraInfoManager(
cname='nao_camera')
if not self.cim.setURL(new_config['camera_info_url']):
rospy.logerr('malformed URL for calibration file')
else:
try:
self.cim.loadCameraInfo()
except IOExcept:
rospy.logerr(
'Could not read from existing calibration file')
if self.cim.isCalibrated():
rospy.loginfo('Successfully loaded camera info')
self.camera_infos[
new_config['camera_info_url']] = self.cim.getCameraInfo()
else:
rospy.logerr(
'There was a problem loading the calibration file. Check the URL!'
)
# set params
for key, naoqi_key in [('source', kCameraSelectID),
('auto_exposition', kCameraAutoExpositionID),
('auto_exposure_algo', kCameraAecAlgorithmID),
('contrast', kCameraContrastID),
('saturation', kCameraSaturationID),
('hue', kCameraHueID),
('sharpness', kCameraSharpnessID),
('auto_white_balance',
kCameraAutoWhiteBalanceID)]:
if self.config[key] != new_config[key] or is_camera_new:
self.camProxy.setParam(naoqi_key, new_config[key])
for key, naoqi_key, auto_exp_val in [
('exposure', kCameraExposureID, 0), ('gain', kCameraGainID, 0),
('brightness', kCameraBrightnessID, 1)
]:
if self.config[key] != new_config[key] or is_camera_new:
self.camProxy.setParam(kCameraAutoExpositionID, auto_exp_val)
self.camProxy.setParam(naoqi_key, new_config[key])
if self.config['white_balance'] != new_config[
'white_balance'] or is_camera_new:
self.camProxy.setParam(kCameraAutoWhiteBalanceID, 0)
self.camProxy.setParam(kCameraWhiteBalanceID,
new_config['white_balance'])
for key, method in [('resolution', 'setResolution'),
('color_space', 'setColorSpace'),
('frame_rate', 'setFrameRate')]:
if self.config[key] != new_config[key] or is_camera_new:
self.camProxy.__getattribute__(method)(self.nameId,
new_config[key])
self.config.update(new_config)
return self.config
def main():
rospy.init_node('flow_pub')
<<<<<<< HEAD
=======
>>>>>>> 1d6eb550eab108dfc7c3e3b3b6fcca8777ced052
first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True)
image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False)
#image_mono_pub = rospy.Publisher("/pidrone/picamera/image_mono",Image, queue_size=1, tcp_nodelay=False)
camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False)
cim = camera_info_manager.CameraInfoManager("picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
global current_mode
global phase_started
global camera
global phase_analyzer
try:
velocity = axes_err()
bridge = CvBridge()
with AnalyzeFlow(camera) as flow_analyzer:
flow_analyzer.bridge = bridge
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# check if camera_switch is admissible
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
rospy.loginfo('using camera: %d', self.camera_switch)
# Parameters for ALVideoDevice
self.resolution = rospy.get_param('~resolution', kVGA)
self.colorSpace = rospy.get_param('~color_space', kBGRColorSpace)
self.fps = rospy.get_param('~fps', 30)
# ROS publishers
self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=10)
self.pub_info_ = rospy.Publisher('~camera_info',
CameraInfo,
queue_size=10)
# initialize camera info manager
self.cname = "nao_camera" # maybe it's a good idea to add _top and _bottom here ...
self.cim = camera_info_manager.CameraInfoManager(cname=self.cname)
self.calibration_file_bottom = rospy.get_param(
'~calibration_file_bottom', None)
self.calibration_file_top = rospy.get_param('~calibration_file_top',
None)
if self.camera_switch == 0:
calibration_file = self.calibration_file_top
else:
calibration_file = self.calibration_file_bottom
if not self.cim.setURL(calibration_file):
rospy.logerr('malformed URL for calibration file')
sys.exit(1)
else:
try:
self.cim.loadCameraInfo()
except IOExcept:
rospy.logerr('Could not read from existing calibration file')
exit(1)
if self.cim.isCalibrated():
rospy.loginfo('Successfully loaded camera info')
else:
rospy.logerr(
'There was a problem loading the calibration file. Check the URL!'
)
exit(1)
# subscription to camProxy
self.nameId = ''
self.subscribe()
# subscription to requests to start/stop publishing
self.sub_publish = rospy.Subscriber(PUBLISH_STATUS_TOPIC, Bool,
self.setPublishingStatus)
# capture a dummy image
self.dummyImage = self.camProxy.getImageRemote(self.nameId)
def main():
def reset_callback(data):
"""start localization when '/pidrone/reset_transform' is published to (press 'r')"""
print "Start localization"
global locate_position
global map_counter
global max_map_counter
global first_locate
locate_position = True
first_locate = True
map_counter = 0
max_map_counter = 0
first_image_pub.publish(map_message)
parser = argparse.ArgumentParser(
description='Display Image Matching Regions')
args = parser.parse_args()
# [x, y, z, yaw]
pos = [0, 0, 0, 0]
angle_x = 0.0
angle_y = 0.0
# localization does not estimate z
z = 0.33
global first_locate
#MS Local test, set to locate position immediately without waiting for web interface
# locate_position = False
global locate_position
global map_counter
global max_map_counter
# constant
alpha_yaw = 0.1 # perceived yaw smoothing alpha
hybrid_alpha = 0.7 # blend position with first frame and int # MS was 0.3
node_name = os.path.splitext(os.path.basename(__file__))[0]
rospy.init_node(node_name)
image_pub = rospy.Publisher("/pidrone/picamera/image_raw",
Image,
queue_size=1,
tcp_nodelay=False)
camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info",
CameraInfo,
queue_size=1,
tcp_nodelay=False)
posepub = rospy.Publisher('/pidrone/picamera/pose',
PoseStamped,
queue_size=1)
posemsg = PoseStamped()
first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
rospy.Subscriber('/pidrone/reset_transform', Empty, reset_callback)
position_control_pub = rospy.Publisher('/pidrone/position_control',
Bool,
queue_size=1)
cim = camera_info_manager.CameraInfoManager(
"picamera", "package://pidrone_pkg/params/picamera.yaml")
cim.loadCameraInfo()
if not cim.isCalibrated():
rospy.logerr("warning, could not find calibration for the camera.")
try:
bridge = CvBridge()
# Create the in-memory stream
stream = io.BytesIO()
with picamera.PiCamera(framerate=90) as camera:
camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
with picamera.array.PiRGBArray(camera) as stream:
detector = cv2.ORB_create(nfeatures=NUM_FEATURES,
scoreType=cv2.ORB_FAST_SCORE)
map_kp, map_des, map_image = create_map('map.jpg')
map_message = bridge.cv2_to_imgmsg(map_image, encoding="bgr8")
first_image_pub.publish(map_message)
estimator = Single_Map_Localization(map_kp, map_des)
print "Starting Single Map Localization"
while not rospy.is_shutdown(): # Do until shutdown received
camera.capture(stream, format='bgr')
# At this point the image is available as stream.array
image = stream.array # Get a single image
stream.seek(0) # Rewind the stream
image_message = bridge.cv2_to_imgmsg(image,
encoding="bgr8")
image_message.header.stamp = rospy.Time.now()
# publish image for view in web interface
image_pub.publish(image_message)
curr_kp, curr_des = detector.detectAndCompute(image, None)
if curr_kp is not None and curr_des is not None:
new_pos, weight = estimator.update(
z, angle_x, angle_y, curr_kp, curr_des)
# update position
if new_pos is not None:
pos = [
hybrid_alpha * new_pos[0] +
(1.0 - hybrid_alpha) * pos[0],
hybrid_alpha * new_pos[1] +
(1.0 - hybrid_alpha) * pos[1], z,
alpha_yaw * new_pos[3] +
(1.0 - alpha_yaw) * pos[3]
]
print '--pose', 'x ', pos[0], 'y ', pos[
1], 'z', pos[2], 'yaw ', pos[3]
map_counter = min(map_counter + 1, -MAX_BAD_COUNT)
# Publish the pose message to ROS
posemsg.header.stamp = rospy.Time.now()
posemsg.pose.position.x = pos[0]
posemsg.pose.position.y = pos[1]
posemsg.pose.position.z = pos[2]
x, y, z, w = tf.transformations.quaternion_from_euler(
0, 0, pos[3])
posemsg.pose.orientation.x = x
posemsg.pose.orientation.y = y
posemsg.pose.orientation.z = z
posemsg.pose.orientation.w = w
posepub.publish(posemsg)
else:
map_counter = map_counter - 1
# if it's been a while without a significant average weight
if map_counter < MAX_BAD_COUNT:
locate_position = False
map_counter = 0
position_control_pub.publish(False)
print 'Lost, leaving position control, must Reset positon hold with "R"'
print 'count', map_counter
else:
print "CANNOT FIND ANY FEATURES !!!!!"
map_counter = map_counter - 1
print 'count', map_counter
if map_counter < MAX_BAD_COUNT:
locate_position = False
map_counter = 0
position_control_pub.publish(False)
print 'Lost, leaving position control, must Reset positon hold with "R"'
print "Shutdown Received"
except Exception:
print "Cancel Received"
raise
def reconfigure(self, new_config, level):
"""
Reconfigure the camera
"""
rospy.loginfo('reconfigure changed')
if self.pub_img_.get_num_connections() == 0:
rospy.loginfo(
'Changes recorded but not applied as nobody is subscribed to the ROS topics.'
)
self.config.update(new_config)
return self.config
# check if we are even subscribed to a camera
is_camera_new = self.nameId is None
new_config['frame_rate'] = int(new_config['frame_rate'])
if is_camera_new:
rospy.loginfo(
'subscribed to camera proxy, since this is the first camera')
if self.get_version() < LooseVersion('2.0'):
# self.nameId = self.camProxy.subscribe("rospy_gvm", new_config['source'],
# new_config['resolution'], new_config['color_space'],
# int(new_config['frame_rate']))
self.nameId = self.camProxy.subscribe(
"rospy_gvm", new_config['resolution'],
new_config['color_space'], new_config['frame_rate'])
else:
self.nameId = self.camProxy.subscribeCamera(
"rospy_gvm", new_config['source'],
new_config['resolution'], new_config['color_space'],
int(new_config['frame_rate']))
if self.config['source'] != new_config['source'] or is_camera_new:
rospy.loginfo('updating camera source information')
if new_config['source'] == kTopCamera:
self.frame_id = "/CameraTop_frame"
elif new_config['source'] == kBottomCamera:
self.frame_id = "/CameraBottom_frame"
elif new_config['source'] == kDepthCamera:
self.frame_id = new_config['camera3d_frame']
else:
rospy.logerr('Invalid source. Must be 0, 1 or 2')
exit(1)
# check if the camera changed
if self.config['camera_info_url'] != new_config['camera_info_url'] and \
new_config['camera_info_url'] and new_config['camera_info_url'] not in self.camera_infos:
if 'cim' not in self.__dict__:
self.cim = camera_info_manager.CameraInfoManager(
cname='nao_robot/camera/top')
if not self.cim.setURL(new_config['camera_info_url']):
rospy.logerr('malformed URL for calibration file')
else:
try:
self.cim.loadCameraInfo()
except IOExcept:
rospy.logerr(
'Could not read from existing calibration file')
if self.cim.isCalibrated():
rospy.loginfo('Successfully loaded camera info')
self.camera_infos[
new_config['camera_info_url']] = self.cim.getCameraInfo()
else:
rospy.logerr(
'There was a problem loading the calibration file. Check the URL!'
)
# set params
key_naoqi_keys = [('auto_exposition', kCameraAutoExpositionID),
('auto_exposure_algo', kCameraAecAlgorithmID),
('contrast', kCameraContrastID),
('saturation', kCameraSaturationID),
('hue', kCameraHueID),
('sharpness', kCameraSharpnessID),
('auto_white_balance', kCameraAutoWhiteBalanceID)]
#if self.get_version() < LooseVersion('2.0'):
# key_method.append(('source', 'setActiveCamera'))
for key, naoqi_key in key_naoqi_keys:
if self.config[key] != new_config[key] or is_camera_new:
if self.get_version() < LooseVersion('2.0'):
self.camProxy.setParam(naoqi_key, new_config[key])
else:
self.camProxy.setCamerasParameter(self.nameId, naoqi_key,
new_config[key])
for key, naoqi_key, auto_exp_val in [
('exposure', kCameraExposureID, 0), ('gain', kCameraGainID, 0),
('brightness', kCameraBrightnessID, 1)
]:
if self.config[key] != new_config[key] or is_camera_new:
if self.get_version() < LooseVersion('2.0'):
self.camProxy.setParam(kCameraAutoExpositionID,
auto_exp_val)
self.camProxy.setParam(naoqi_key, new_config[key])
else:
self.camProxy.setCamerasParameter(self.nameId,
kCameraAutoExpositionID,
auto_exp_val)
self.camProxy.setCamerasParameter(self.nameId, naoqi_key,
new_config[key])
if self.config['white_balance'] != new_config[
'white_balance'] or is_camera_new:
if self.get_version() < LooseVersion('2.0'):
self.camProxy.setParam(kCameraAutoWhiteBalanceID, 0)
#self.camProxy.setParam(kCameraWhiteBalanceID, new_config['white_balance'])
else:
self.camProxy.setCamerasParameter(self.nameId,
kCameraAutoWhiteBalanceID, 0)
self.camProxy.setCamerasParameter(self.nameId,
kCameraWhiteBalanceID,
new_config['white_balance'])
key_methods = [('resolution', 'setResolution'),
('color_space', 'setColorSpace'),
('frame_rate', 'setFrameRate')]
if self.get_version() >= LooseVersion('2.0'):
key_methods.append(('source', 'setActiveCamera'))
for key, method in key_methods:
if self.config[key] != new_config[key] or is_camera_new:
self.camProxy.__getattribute__(method)(self.nameId,
new_config[key])
self.config.update(new_config)
return self.config
def main():
"""Publish a video as ROS messages.
"""
# Patse arguments.
parser = argparse.ArgumentParser(
description="Convert video into a rosbag.")
parser.add_argument("video_file", help="Input video.")
parser.add_argument("-c",
"--camera",
default="camera",
help="Camera name.")
parser.add_argument("-f",
"--frame_id",
default="camera",
help="tf frame_id.")
parser.add_argument("--width",
type=np.int32,
default="640",
help="Image width.")
parser.add_argument("--height",
type=np.int32,
default="480",
help="Image height.")
# parser.add_argument("--info_url", default="file:///eye_tracker_calib.yml",
# help="Camera calibration url.")
parser.add_argument("--info_url",
default="file:///nao_bottom.yml",
help="Camera calibration url.")
args = parser.parse_args()
print "Publishing %s." % (args.video_file)
# Set up node.
rospy.init_node("video_publisher", anonymous=True)
img_pub = rospy.Publisher("/" + args.camera + "/image_raw",
Image,
queue_size=10)
info_pub = rospy.Publisher("/" + args.camera + "/camera_info",
CameraInfo,
queue_size=10)
info_manager = camera_info_manager.CameraInfoManager(cname=args.camera,
url=args.info_url,
namespace=args.camera)
info_manager.loadCameraInfo()
# Open video.
video = cv2.VideoCapture(args.video_file)
# Get frame rate.
fps = video.get(cv2.CAP_PROP_FPS)
rate = rospy.Rate(fps)
# Loop through video frames.
while not rospy.is_shutdown() and video.grab():
tmp, img = video.retrieve()
if not tmp:
print "Could not grab frame."
break
img_out = np.empty((args.height, args.width, img.shape[2]))
# Compute input/output aspect ratios.
aspect_ratio_in = np.float(img.shape[1]) / np.float(img.shape[0])
aspect_ratio_out = np.float(args.width) / np.float(args.height)
if aspect_ratio_in > aspect_ratio_out:
# Output is narrower than input -> crop left/right.
rsz_factor = np.float(args.height) / np.float(img.shape[0])
img_rsz = cv2.resize(img, (0, 0),
fx=rsz_factor,
fy=rsz_factor,
interpolation=cv2.INTER_AREA)
diff = (img_rsz.shape[1] - args.width) / 2
img_out = img_rsz[:, diff:-diff - 1, :]
elif aspect_ratio_in < aspect_ratio_out:
# Output is wider than input -> crop top/bottom.
rsz_factor = np.float(args.width) / np.float(img.shape[1])
img_rsz = cv2.resize(img, (0, 0),
fx=rsz_factor,
fy=rsz_factor,
interpolation=cv2.INTER_AREA)
diff = (img_rsz.shape[0] - args.height) / 2
img_out = img_rsz[diff:-diff - 1, :, :]
else:
# Resize image.
img_out = cv2.resize(img, (args.width, args.height))
#print args.height
#print args.width
#print aspect_ratio_in
#print aspect_ratio_out
#print img_out.shape[0]
#print img_out.shape[1]
#assert img_out.shape[0:2] == (args.height, args.width)
try:
# Publish image.
img_msg = bridge.cv2_to_imgmsg(img_out, "bgr8")
img_msg.header.stamp = rospy.Time.now()
img_msg.header.frame_id = args.frame_id
img_pub.publish(img_msg)
# Publish camera info.
info_msg = info_manager.getCameraInfo()
info_msg.header = img_msg.header
info_pub.publish(info_msg)
except CvBridgeError as err:
print err
rate.sleep()
return
