def get_camera_info(camera_info, camera):
with open(camera_info, 'r') as stream:
try:
data = yaml.load(stream)
camera_info = CameraInfo()
T=[0,0,0]
camera_info.width = data[camera]['resolution'][0]
camera_info.height = data[camera]['resolution'][1]
if data[camera]['distortion_model'] == "radtan":
camera_info.distortion_model = "plumb_bob"
else:
camera_info.distortion_model = data[camera]['distortion_model']
fx,fy,cx,cy = data[camera]['intrinsics']
camera_info.K[0:3] = [fx, 0, cx]
camera_info.K[3:6] = [0, fy, cy]
camera_info.K[6:9] = [0, 0, 1]
k1,k2,t1,t2 = data[camera]['distortion_coeffs']
camera_info.D = [k1,k2,t1,t2,0]
#if cam0 then it's left camera, so R = identity and T = [0 0 0]
if camera == "cam0":
camera_info.R[0:3] = [1, 0, 0]
camera_info.R[3:6] = [0, 1, 0]
camera_info.R[6:9] = [0, 0, 1]
else:
camera_info.R[0:3] = data[camera]['T_cn_cnm1'][0][:3]
camera_info.R[3:6] = data[camera]['T_cn_cnm1'][1][:3]
camera_info.R[6:9] = data[camera]['T_cn_cnm1'][2][:3]
T[0:3] = [data[camera]['T_cn_cnm1'][0][3], data[camera]['T_cn_cnm1'][1][3], data[camera]['T_cn_cnm1'][2][3]]
except yaml.YAMLError as exc:
print(exc)
return camera_info, T
def run(self):
# left_cam_info = self.yaml_to_camera_info(self.left_yaml_file)
# right_cam_info = self.yaml_to_camera_info(self.right_yaml_file)
left_cam_info = CameraInfo()
left_cam_info.width = 640
left_cam_info.height = 480
left_cam_info.K = [742.858255, 0.000000, 342.003337, 0.000000, 752.915532, 205.211518, 0.000000, 0.000000, 1.000000]
left_cam_info.D = [0.098441, 0.046414, -0.039316, 0.011202, 0.000000]
left_cam_info.R = [0.983638, 0.047847, -0.173686, -0.048987, 0.998797, -0.002280, 0.173368, 0.010751, 0.984798]
left_cam_info.P = [905.027641, 0.000000, 500.770557, 0.000000, 0.000000, 905.027641, 180.388927, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
left_cam_info.distortion_model = 'plumb_bob'
right_cam_info = CameraInfo()
right_cam_info.width = 640
right_cam_info.height = 480
right_cam_info.K = [747.026840, 0.000000, 356.331849, 0.000000, 757.336986, 191.248883, 0.000000, 0.000000, 1.000000]
right_cam_info.D = [0.127580, -0.050428, -0.035857, 0.018986, 0.000000]
right_cam_info.R = [0.987138, 0.047749, -0.152571, -0.046746, 0.998855, 0.010153, 0.152881, -0.002890, 0.988240]
right_cam_info.P = [905.027641, 0.000000, 500.770557, -42.839515, 0.000000, 905.027641, 180.388927, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
right_cam_info.distortion_model = 'plumb_bob'
rate = rospy.Rate(30)
while not rospy.is_shutdown():
ret,frame=self.cam.read()
if not ret:
print('[ERROR]: frame error')
break
expand_frame=cv2.resize(frame,None,fx=2,fy=1)
left_image = expand_frame[0:480,0:640]
right_image = expand_frame[0:480,640:1280]
self.msg_header.frame_id = 'stereo_image'
self.msg_header.stamp = rospy.Time.now()
left_cam_info.header = self.msg_header
right_cam_info.header = self.msg_header
self.left_image_info_pub.publish(left_cam_info)
self.right_image_info_pub.publish(right_cam_info)
# self.pub_image(self.left_image_pub, left_image, self.msg_header )
# self.pub_image(self.right_image_pub, right_image, self.msg_header )
try:
thread.start_new_thread( self.pub_image, (self.left_image_pub, left_image, self.msg_header, ))
thread.start_new_thread( self.pub_image, (self.right_image_pub, right_image, self.msg_header, ))
except:
print("[ERROR]: unable to start thread")
rate.sleep()
def genRosCameraInfo(K, P, resolution):
'''
默认参数
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
height: 0
width: 0
distortion_model: ''
D: []
K: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
R: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
P: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
'''
caminfo = CameraInfo()
caminfo.width = resolution[0]
caminfo.height = resolution[1]
caminfo.distortion_model = "plumb_bob"
caminfo.header.frame_id = "rgbd_camera_link"
# caminfo.header.stamp = time_now
caminfo.D = [0.0, 0.0, 0.0, 0.0, 0.0] # 畸变系数，无初始值需要指定
caminfo.K = K
caminfo.P = P
return caminfo
def get_camera_info(pipeline, array):
profile = pipeline.get_active_profile()
if "color" in array:
stream_profile = rs.video_stream_profile(
profile.get_stream(rs.stream.color))
stream_intrinsics = stream_profile.get_intrinsics()
elif "depth" in array:
stream_profile = rs.video_stream_profile(
profile.get_stream(rs.stream.depth))
stream_intrinsics = stream_profile.get_intrinsics()
camera_info = CameraInfo()
camera_info.width = stream_intrinsics.width
camera_info.height = stream_intrinsics.height
camera_info.distortion_model = 'plumb_bob'
cx = stream_intrinsics.ppx
cy = stream_intrinsics.ppy
fx = stream_intrinsics.fx
fy = stream_intrinsics.fy
camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
camera_info.D = [0, 0, 0, 0, 0]
camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
return camera_info
def __init__(self):
rospy.init_node('image_converter', anonymous=True)
self.filtered_face_locations = rospy.get_param('camera_topic')
self.image_pub = rospy.Publisher(self.filtered_face_locations,Image)
self.image_info = rospy.Publisher('camera_info',CameraInfo)
self.bridge = CvBridge()
cap = cv2.VideoCapture('/home/icog-labs/Videos/video.mp4')
print cap.get(5)
path = rospkg.RosPack().get_path("robots_config")
path = path + "/robot/camera.yaml"
stream = file(path, 'r')
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
while (not rospy.is_shutdown()) and (cap.isOpened()):
self.keystroke = cv2.waitKey(1000 / 30)
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# cv2.imshow('frame', gray)
self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8"))
self.image_info.publish(cam_info)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def __init__(self):
rospy.init_node('image_converter', anonymous=True)
self.filtered_face_locations = rospy.get_param('camera_topic')
self.image_pub = rospy.Publisher(self.filtered_face_locations, Image)
self.image_info = rospy.Publisher('camera_info', CameraInfo)
self.bridge = CvBridge()
cap = cv2.VideoCapture('/home/icog-labs/Videos/video.mp4')
print cap.get(5)
path = rospkg.RosPack().get_path("robots_config")
path = path + "/robot/camera.yaml"
stream = file(path, 'r')
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
while (not rospy.is_shutdown()) and (cap.isOpened()):
self.keystroke = cv2.waitKey(1000 / 30)
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# cv2.imshow('frame', gray)
self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8"))
self.image_info.publish(cam_info)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def yaml_to_CameraInfo(calib_yaml):
"""
Parse a yaml file containing camera calibration data (as produced by
rosrun camera_calibration cameracalibrator.py) into a
sensor_msgs/CameraInfo msg.
Parameters
----------
yaml_fname : str
Path to yaml file containing camera calibration data
Returns
-------
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
data
"""
# Load data from file
calib_data = yaml.load(calib_yaml)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
return camera_info_msg
def yaml_to_CameraInfo(yaml_fname):
"""
Parse a yaml file containing camera calibration data (as produced by
rosrun camera_calibration cameracalibrator.py) into a
sensor_msgs/CameraInfo msg.
Parameters
----------
yaml_fname : str
Path to yaml file containing camera calibration data
Returns
-------
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
data
"""
# Load data from file
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
return camera_info_msg
def yaml_to_CameraInfo():
rospy.init_node("camera_info_publisher", anonymous=True)
info_publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=10)
# yaml_publisher = rospy.Publisher("yaml_filename", String, queue_size=10)
rate = rospy.Rate(10) # 10hz
yaml_fname = rospy.get_param('~camera_yaml')
# yaml_fname = rospy.get_param('camera_yaml')
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
while not rospy.is_shutdown():
# yaml_publisher.publish(yaml_fname)
info_publisher.publish(camera_info_msg)
rate.sleep()
def __init__(self):
# initialize this code as a ROS node named stream_to_ROS_node:
rospy.init_node("stream_to_ROS_node", anonymous=True)
# create publisher to publish frames from the video stream:
self.image_pub = rospy.Publisher("/balrog_camera/image_raw", Image, queue_size = 1)
# create publisher to publish camera info (calibration params):
self.camera_info_pub = rospy.Publisher("/balrog_camera/camera_info", CameraInfo, queue_size = 1)
# initialize cv_bridge for conversion between openCV and ROS images:
self.cv_bridge = CvBridge()
# the most recently read camera frame:
self.latest_frame = None
# start a thread constantly reading frames from the RPI video stream:
self.thread_video = Thread(target = self.video_thread)
self.thread_video.start()
# camera info and calibration parameters:
camera_info_msg = CameraInfo()
camera_info_msg.height = 360
camera_info_msg.width = 640
camera_info_msg.distortion_model = "plumb_bob"
camera_info_msg.D = [1.6541397736403166e-01, -3.1762679367786140e-01, 0.0, 0.0, -1.4358526468495059e-01] # ([k1, k2, t1, t2, k3])
camera_info_msg.K = [4.8488441935486514e+02, 0, 320, 0, 4.8488441935486514e+02, 180, 0, 0, 1] # ([fx, 0, cx, 0, fy, cy, 0, 0, 1])
camera_info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
self.camera_info_msg = camera_info_msg
def __init__(self):
rospy.init_node('image_publish')
name = sys.argv[1]
image = cv2.imread(name)
#cv2.imshow("im", image)
#cv2.waitKey(5)
hz = rospy.get_param("~rate", 1)
frame_id = rospy.get_param("~frame_id", "map")
use_image = rospy.get_param("~use_image", True)
rate = rospy.Rate(hz)
self.ci_in = None
ci_sub = rospy.Subscriber('camera_info_in', CameraInfo,
self.camera_info_callback, queue_size=1)
if use_image:
pub = rospy.Publisher('image', Image, queue_size=1)
ci_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1)
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.encoding = 'bgr8'
msg.height = image.shape[0]
msg.width = image.shape[1]
msg.step = image.shape[1] * 3
msg.data = image.tostring()
if use_image:
pub.publish(msg)
ci = CameraInfo()
ci.header = msg.header
ci.height = msg.height
ci.width = msg.width
ci.distortion_model ="plumb_bob"
# TODO(lucasw) need a way to set these values- have this node
# subscribe to an input CameraInfo?
ci.D = [0.0, 0.0, 0.0, 0, 0]
ci.K = [500.0, 0.0, msg.width/2, 0.0, 500.0, msg.height/2, 0.0, 0.0, 1.0]
ci.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
ci.P = [500.0, 0.0, msg.width/2, 0.0, 0.0, 500.0, msg.height/2, 0.0, 0.0, 0.0, 1.0, 0.0]
# ci_pub.publish(ci)
# TODO(lwalter) only run this is hz is positive,
# otherwise wait for input trigger message to publish an image
while not rospy.is_shutdown():
if self.ci_in is not None:
ci = self.ci_in
msg.header.stamp = rospy.Time.now()
ci.header = msg.header
if use_image:
pub.publish(msg)
ci_pub.publish(ci)
if hz <= 0:
rospy.sleep()
else:
rate.sleep()
def generateDepthImageAndInfo(self):
self.msg_counter += 1
width = 640
height = 480
img = np.empty((height, width), np.uint16)
img.fill(1400)
for i in range(width):
for j in range(height / 2 - 1, height / 2 + 1):
img[j][i] = 520
depthimg = Image()
depthimg.header.frame_id = 'depthmap_test'
depthimg.header.seq = self.msg_counter
depthimg.header.stamp = rospy.Time.now()
depthimg.height = height
depthimg.width = width
depthimg.encoding = "16UC1"
depthimg.step = depthimg.width * 2
depthimg.data = img.tostring()
info = CameraInfo()
info.header = depthimg.header
info.height = height
info.width = width
info.distortion_model = "plumb_bob"
info.K[0] = 570; info.K[2] = 314; info.K[4] = 570
info.K[5] = 239; info.K[8] = 1.0
info.R[0] = 1.0; info.R[4] = 1.0; info.R[8] = 1.0
info.P[0] = 570; info.P[2] = 314; info.P[5] = 570
info.P[6] = 235; info.P[10] = 1.0
return depthimg, info
def get_camera_info(self, yaml_fname):
"""
Parse a yaml file containing camera calibration data
Parameters
----------
yaml_fname : str
Path to yaml file containing camera calibration data
Returns
-------
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
data
"""
# Load data from file
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["intrinsics"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
camera_info_msg.header.frame_id = calib_data["frame_id"]
return camera_info_msg
def publish( self ):
# Get the image.
image = self.__videoDeviceProxy.getImageRemote( self.__videoDeviceProxyName );
# Create Image message.
ImageMessage = Image();
ImageMessage.header.stamp.secs = image[ 5 ];
ImageMessage.width = image[ 0 ];
ImageMessage.height = image[ 1 ];
ImageMessage.step = image[ 2 ] * image[ 0 ];
ImageMessage.is_bigendian = False;
ImageMessage.encoding = 'bgr8';
ImageMessage.data = image[ 6 ];
self.__imagePublisher.publish( ImageMessage );
# Create CameraInfo message.
# Data from the calibration phase is hard coded for now.
CameraInfoMessage = CameraInfo();
CameraInfoMessage.header.stamp.secs = image[ 5 ];
CameraInfoMessage.width = image[ 0 ];
CameraInfoMessage.height = image[ 1 ];
CameraInfoMessage.D = [ -0.0769218451517258, 0.16183180613612602, 0.0011626049774280595, 0.0018733894100460534, 0.0 ];
CameraInfoMessage.K = [ 581.090096189648, 0.0, 341.0926325830606, 0.0, 583.0323248080421, 241.02441593704128, 0.0, 0.0, 1.0 ];
CameraInfoMessage.R = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ];
CameraInfoMessage.P = [ 580.5918359588198, 0.0, 340.76398441334106, 0.0, 0.0, 582.4042541534321, 241.04182225157172, 0.0, 0.0, 0.0, 1.0, 0.0] ;
CameraInfoMessage.distortion_model = 'plumb_bob';
#CameraInfoMessage.roi.x_offset = self.__ROIXOffset;
#CameraInfoMessage.roi.y_offset = self.__ROIYOffset;
#CameraInfoMessage.roi.width = self.__ROIWidth;
#CameraInfoMessage.roi.height = self.__ROIHeight;
#CameraInfoMessage.roi.do_rectify = self.__ROIDoRectify;
self.__cameraInfoPublisher.publish( CameraInfoMessage );
def load_camera_info_3(robot):
# Load camera information
filename = (os.environ['DUCKIEFLEET_ROOT'] +
"/calibrations/camera_intrinsic/" + robot + ".yaml")
if not os.path.isfile(filename):
dtu.logger.warn(
"no intrinsic calibration parameters for {}, trying default".
format(robot))
filename = (os.environ['DUCKIEFLEET_ROOT'] +
"/calibrations/camera_intrinsic/default.yaml")
if not os.path.isfile(filename):
dtu.logger.error("can't find default either, something's wrong")
calib_data = dtu.yaml_wrap.yaml_load_file(filename)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
dtu.logger.info(
"Loaded camera calibration parameters for {} from {}".format(
robot, os.path.basename(filename)))
return cam_info
def toCameraInfo(self):
msg = CameraInfo()
(msg.width, msg.height) = self.size
if self.D.size > 5:
msg.distortion_model = "rational_polynomial"
else:
msg.distortion_model = "plumb_bob"
msg.D = numpy.ravel(self.D).copy().tolist()
msg.K = numpy.ravel(self.K).copy().tolist()
msg.R = numpy.ravel(self.R).copy().tolist()
msg.P = numpy.ravel(self.P).copy().tolist()
return msg
def publishing(pub_image, pub_camera, image, type_of_camera):
if type_of_camera is 1:
image.convert(carla.ColorConverter.Depth)
elif type_of_camera is 2:
image.convert(carla.ColorConverter.CityScapesPalette)
array = np.frombuffer(image.raw_data, dtype=np.uint8)
array = np.reshape(array, (image.height, image.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
img = Image()
infomsg = CameraInfo()
img.header.stamp = rospy.Time.now()
img.header.frame_id = 'base'
img.height = infomsg.height = image.height
img.width = infomsg.width = image.width
img.encoding = "rgb8"
img.step = img.width * 3 * 1
st1 = array.tostring()
img.data = st1
cx = infomsg.width / 2.0
cy = infomsg.height / 2.0
fx = fy = infomsg.width / (2.0 * math.tan(image.fov * math.pi / 360.0))
infomsg.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
infomsg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]
infomsg.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
infomsg.D = [0, 0, 0, 0, 0]
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = "plumb_bob"
infomsg.header = img.header
pub_image.publish(img)
pub_camera.publish(infomsg)
def test_CameraFOV(self):
msg = CameraInfo()
msg.height = 1024
msg.width = 1280
msg.distortion_model = 'plumb_bob'
msg.D = [
-0.5506515572367885, 0.16918149333674903, -0.0005494252446900035,
-0.003574460971943457, 0.08824797068343779
]
msg.K = [
1547.3611792786492, 0.0, 645.7946620597459, 0.0,
1546.5965535476455, 512.489834878375, 0.0, 0.0, 1.0
]
msg.R = [
0.9976063902119301, 0.0008462845042432227, 0.06914314145929477,
-0.0007608300534628908, 0.9999989139538485, -0.0012622316559149822,
-0.06914413457374326, 0.0012066041858555048, 0.997605950644034
]
msg.P = [
1445.628365834274, 0.0, 521.7993656184378, 0.0, 0.0,
1445.628365834274, 514.7422812912976, 0.0, 0.0, 0.0, 1.0, 0.0
]
camfov = ru.camera.CameraFOV(msg, maxdist=2.)
corners = camfov.get_corners()
vertices, faces = camfov.get_trimesh()
self.assertTrue(camfov.contains(corners))
points_inside = [camfov.random_point_inside() for _ in range(100)]
self.assertTrue(camfov.contains(points_inside))
def toCameraInfo(self):
msg = CameraInfo()
(msg.width, msg.height) = self.size
if self.D.size > 5:
msg.distortion_model = "rational_polynomial"
else:
msg.distortion_model = "plumb_bob"
msg.D = numpy.ravel(self.D).copy().tolist()
msg.K = numpy.ravel(self.K).copy().tolist()
msg.R = numpy.ravel(self.R).copy().tolist()
msg.P = numpy.ravel(self.P).copy().tolist()
return msg
def _build_camera_info(self):
"""
Private function to compute camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
# store info without header
camera_info.header = self.get_msg_header()
camera_info.width = int(self.carla_actor.attributes['image_size_x'])
camera_info.height = int(self.carla_actor.attributes['image_size_y'])
camera_info.distortion_model = 'plumb_bob'
cx = camera_info.width / 2.0
cy = camera_info.height / 2.0
fx = camera_info.width / (
2.0 * math.tan(float(self.carla_actor.attributes['fov']) * math.pi / 360.0))
fy = fx
if ROS_VERSION == 1:
camera_info.K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0]
camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
camera_info.P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
elif ROS_VERSION == 2:
# pylint: disable=assigning-non-slot
camera_info.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0]
camera_info.d = [0.0, 0.0, 0.0, 0.0, 0.0]
camera_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
self._camera_info = camera_info
def make_camera_msg(cam, rgb_time_sec, rgb_time_nsec, test_image):
camera_info_msg = CameraInfo()
camera_info_msg.header.seq = test_image
if rgb_time_nsec + 20000000 > 999999999:
camera_info_msg.header.stamp.secs = rgb_time_sec + 1
camera_info_msg.header.stamp.nsecs = rgb_time_nsec + 20000000 - 100000000
else:
camera_info_msg.header.stamp.secs = rgb_time_sec
camera_info_msg.header.stamp.nsecs = rgb_time_nsec + 20000000
# camera_info_msg.header.stamp.secs = rgb_time_sec
# camera_info_msg.header.stamp.nsecs = rgb_time_nsec + 20000000
camera_info_msg.header.frame_id = "/openni_rgb_optical_frame"
camera_info_msg.distortion_model = "plumb_bob"
width, height = cam[0], cam[1]
fx, fy = cam[2], cam[3]
cx, cy = cam[4], cam[5]
camera_info_msg.width = width
camera_info_msg.height = height
camera_info_msg.K = [fx, 0, cx,
0, fy, cy,
0, 0, 1]
camera_info_msg.D = [0, 0, 0, 0, 0]
camera_info_msg.R = [1, 0, 0,
0, 1, 0,
0, 0, 1]
camera_info_msg.P = [fx, 0, cx, 0,
0, fy, cy, 0,
0, 0, 1, 0]
return camera_info_msg
def run():
rospy.init_node('publish_constant', anonymous=True)
pub = rospy.Publisher(args.topic, CameraInfo, queue_size=10)
rate = rospy.Rate(30)
filename = args.camera_info
with open(filename, 'r') as f:
camera_data = yaml.load(f)
camera_info = CameraInfo()
camera_info.height = camera_data['height']
camera_info.width = camera_data['width']
camera_info.K = camera_data['K']
camera_info.D = camera_data['D']
camera_info.R = camera_data['R']
camera_info.P = camera_data['P']
camera_info.distortion_model = camera_data['distortion_model']
camera_info.header.frame_id = args.frame
# Setting this to zero means 'take the newest camera pose if TF'.
camera_info.header.stamp = rospy.Time(0)
while not rospy.is_shutdown():
pub.publish(camera_info)
rate.sleep()
def rosmsg(self):
""":obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg
"""
msg_header = Header()
msg_header.frame_id = self._frame
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = self._height
msg.width = self._width
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [
self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0
]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0,
0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def load_camera_info(self):
'''Load camera intrinsics'''
filename = (sys.path[0] + "/calibrations/camera_intrinsic/" +
self.robot_name + ".yaml")
if not os.path.isfile(filename):
logger.warn(
"no intrinsic calibration parameters for {}, trying default".
format(self.robot_name))
filename = (sys.path[0] +
"/calibrations/camera_intrinsic/default.yaml")
if not os.path.isfile(filename):
logger.error("can't find default either, something's wrong")
calib_data = yaml_load_file(filename)
# logger.info(yaml_dump(calib_data))
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape(
(3, 3))
cam_info.D = np.array(
calib_data['distortion_coefficients']['data']).reshape((1, 5))
cam_info.R = np.array(
calib_data['rectification_matrix']['data']).reshape((3, 3))
cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape(
(3, 4))
cam_info.distortion_model = calib_data['distortion_model']
logger.info(
"Loaded camera calibration parameters for {} from {}".format(
self.robot_name, os.path.basename(filename)))
return cam_info
def dr_callback(self, config, level):
ci = CameraInfo()
ci.header.stamp = rospy.Time.now()
ci.header.frame_id = config['frame_id']
ci.width = config['width']
ci.height = config['height']
ci.distortion_model = config['distortion_model']
ci.D = [
config['d0'], config['d1'], config['d2'], config['d3'],
config['d4']
]
ci.K[0 * 3 + 0] = config['fx']
ci.K[0 * 3 + 2] = config['cx']
ci.K[1 * 3 + 1] = config['fy']
ci.K[1 * 3 + 2] = config['cy']
ci.K[2 * 3 + 2] = 1
ci.P[0 * 4 + 0] = config['fx']
ci.P[0 * 4 + 2] = config['cx']
ci.P[1 * 4 + 1] = config['fy']
ci.P[1 * 4 + 2] = config['cy']
ci.P[2 * 4 + 2] = 1
ci.R[0] = 1
ci.R[4] = 1
ci.R[8] = 1
self.camera_info = ci
self.pub.publish(ci)
return config
def info_480p():
data = CameraInfo()
data.height = 480
data.width = 640
data.distortion_model = "plumb_bob"
data.D = [-0.297117, 0.070173, -0.000390, 0.005232, 0.000000]
data.K = [
342.399061, 0.000000, 284.222700, 0.000000, 343.988302, 227.555237,
0.000000, 0.000000, 1.000000
]
data.R = [
1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000,
0.000000, 1.000000
]
data.P = [
247.555710, 0.000000, 290.282918, 0.000000, 0.000000, 281.358612,
220.776009, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000
]
data.binning_x = 0
data.binning_y = 0
data.roi.x_offset = 0
data.roi.y_offset = 0
data.roi.height = 0
data.roi.width = 0
data.roi.do_rectify = False
return data
def info_720p():
data = CameraInfo()
data.height = 720
data.width = 1280
data.distortion_model = "plumb_bob"
data.D = [-0.266169, 0.056566, 0.003569, 0.002922, 0.000000]
data.K = [
550.515474, 0.000000, 560.486530, 0.000000, 550.947091, 334.377088,
0.000000, 0.000000, 1.000000
]
data.R = [
1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000,
0.000000, 1.000000
]
data.P = [
395.931458, 0.000000, 570.158643, 0.000000, 0.000000, 474.049377,
329.846866, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000
]
data.binning_x = 0
data.binning_y = 0
data.roi.x_offset = 0
data.roi.y_offset = 0
data.roi.height = 0
data.roi.width = 0
data.roi.do_rectify = False
return data
def callback(data):
print(rospy.get_name(), "I heard %s" % str(data.data))
img_raveled = data.data[0:-2]
img_size = data.data[-2:].astype(int)
img = np.float32(np.reshape(img_raveled, (img_size[0], img_size[1])))
#img = np.float32((np.reshape(data.data, (DEPTH_IMG_WIDTH, DEPTH_IMG_HEIGHT))))
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
image_message = CvBridge().cv2_to_imgmsg(img, encoding="passthrough")
image_message.header = h
pub.publish(image_message)
camera_info_msg = CameraInfo()
camera_info_msg.header = h
fx, fy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2
cx, cy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2
camera_info_msg.width = DEPTH_IMG_WIDTH
camera_info_msg.height = DEPTH_IMG_HEIGHT
camera_info_msg.distortion_model = "plumb_bob"
camera_info_msg.K = np.float32([fx, 0, cx, 0, fy, cy, 0, 0, 1])
camera_info_msg.D = np.float32([0, 0, 0, 0, 0])
camera_info_msg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]
camera_info_pub.publish(camera_info_msg)
def camera_info(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def send_test_messages(self, filename):
self.msg_received = False
# Publish the camera info TODO make this a field in the annotations file to dictate the source calibration file
msg_info = CameraInfo()
msg_info.height = 480
msg_info.width = 640
msg_info.distortion_model = "plumb_bob"
msg_info.D = [-0.28048157543793056, 0.05674481026365553, -0.000988764087143394, -0.00026869128565781613, 0.0]
msg_info.K = [315.128501, 0.0, 323.069638, 0.0, 320.096636, 218.012581, 0.0, 0.0, 1.0]
msg_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg_info.P = [217.48876953125, 0.0, 321.3154072384932, 0.0, 0.0, 250.71084594726562, 202.30416165274983, 0.0,
0.0, 0.0, 1.0, 0.0]
msg_info.roi.do_rectify = False
# Publish the test image
img = cv2.imread(filename)
cvb = CvBridge()
msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8")
self.pub_info.publish(msg_info)
self.pub_raw.publish(msg_raw)
# Wait for the message to be received
timeout = rospy.Time.now() + rospy.Duration(10) # Wait at most 5 seconds for the node to reply
while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout:
rospy.sleep(0.1)
self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic_name, initial_time):
print(f"Exporting camera {camera}")
if kitti_type.find("raw") != -1:
camera_pad = '{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir, 'data')
image_filenames = sorted(os.listdir(image_path))
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height = tuple([int(x) for x in util[f'S_rect_{camera_pad}']])
calib.distortion_model = 'plumb_bob'
calib.k = util[f'K_{camera_pad}']
calib.r = util[f'R_rect_{camera_pad}']
calib.d = [float(x) for x in util[f'D_{camera_pad}']]
calib.p = util[f'P_rect_{camera_pad}']
elif kitti_type.find("odom") != -1:
camera_pad = '{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path, f'image_{camera_pad}')
image_filenames = sorted(os.listdir(image_path))
image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.p = util[f'P{camera_pad}']
iterable = zip(image_datetimes, image_filenames)
bar = progressbar.ProgressBar()
for dt, filename in bar(iterable):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]
if camera in (0, 1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding = "mono8" if camera in (0, 1) else "bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw") != -1:
image_message.header.stamp = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg()
topic_ext = "/image_raw"
elif kitti_type.find("odom") != -1:
image_message.header.stamp = Time(seconds=dt).to_msg()
topic_ext = "/image_rect"
calib.header.stamp = image_message.header.stamp
camera_info_topic = create_topic(topic_name + '/camera_info', "sensor_msgs/msg/CameraInfo", "cdr")
bag.create_topic(camera_info_topic)
camera_topic = create_topic(topic_name + topic_ext, "sensor_msgs/msg/Image", "cdr")
bag.create_topic(camera_topic)
bag.write((topic_name + topic_ext, serialize_message(image_message),
timestamp_to_nanoseconds_from_epoch(image_message.header.stamp)))
bag.write((topic_name + '/camera_info', serialize_message(calib),
timestamp_to_nanoseconds_from_epoch(calib.header.stamp)))
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera,
camera_frame_id, topic, initial_time):
print("Exporting camera {}".format(camera))
if kitti_type.find("raw") != -1:
camera_pad = '{0:02d}'.format(camera)
image_dir = os.path.join(kitti.data_path,
'image_{}'.format(camera_pad))
image_path = os.path.join(image_dir, 'data')
image_filenames = sorted(os.listdir(image_path))
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
image_datetimes = map(
lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'),
f.readlines())
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.width, calib.height = tuple(
util['S_rect_{}'.format(camera_pad)].tolist())
calib.distortion_model = 'plumb_bob'
calib.K = util['K_{}'.format(camera_pad)]
calib.R = util['R_rect_{}'.format(camera_pad)]
calib.D = util['D_{}'.format(camera_pad)]
calib.P = util['P_rect_{}'.format(camera_pad)]
elif kitti_type.find("odom") != -1:
camera_pad = '{0:01d}'.format(camera)
image_path = os.path.join(kitti.sequence_path,
'image_{}'.format(camera_pad))
image_filenames = sorted(os.listdir(image_path))
image_datetimes = map(lambda x: initial_time + x.total_seconds(),
kitti.timestamps)
calib = CameraInfo()
calib.header.frame_id = camera_frame_id
calib.P = util['P{}'.format(camera_pad)]
iterable = zip(image_datetimes, image_filenames)
bar = progressbar.ProgressBar()
for dt, filename in bar(iterable):
image_filename = os.path.join(image_path, filename)
cv_image = cv2.imread(image_filename)
calib.height, calib.width = cv_image.shape[:2]
if camera in (0, 1):
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
encoding = "mono8" if camera in (0, 1) else "bgr8"
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
image_message.header.frame_id = camera_frame_id
if kitti_type.find("raw") != -1:
image_message.header.stamp = rospy.Time.from_sec(
float(datetime.strftime(dt, "%s.%f")))
topic_ext = "/image_raw"
elif kitti_type.find("odom") != -1:
image_message.header.stamp = rospy.Time.from_sec(dt)
topic_ext = "/image_rect"
calib.header.stamp = image_message.header.stamp
bag.write(topic + topic_ext,
image_message,
t=image_message.header.stamp)
bag.write(topic + '/camera_info', calib, t=calib.header.stamp)
def default(self, ci="unused"):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data["image"]
image = Image()
image.header = self.get_ros_header()
image.header.frame_id += "/base_image"
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = "rgba8"
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data["intrinsic_matrix"]
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = "plumb_bob"
camera_info.K = [
intrinsic[0][0],
intrinsic[0][1],
intrinsic[0][2],
intrinsic[1][0],
intrinsic[1][1],
intrinsic[1][2],
intrinsic[2][0],
intrinsic[2][1],
intrinsic[2][2],
]
camera_info.R = R
camera_info.P = [
intrinsic[0][0],
intrinsic[0][1],
intrinsic[0][2],
Tx,
intrinsic[1][0],
intrinsic[1][1],
intrinsic[1][2],
Ty,
intrinsic[2][0],
intrinsic[2][1],
intrinsic[2][2],
0,
]
self.publish(image)
self.topic_camera_info.publish(camera_info)
def __init__(self, topic_name):
# Set publisher.
self._pub = rospy.Publisher(topic_name, CameraInfo, queue_size=5)
# Create default camera info:
camera_info = CameraInfo() # This is ROS camera info. Not mine.
camera_info.distortion_model = "plumb_bob"
camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
self._default_camera_info = camera_info
def fill_camera_info(calib):
ci = CameraInfo()
ci.width = int(calib['image_width'])
ci.height = int(calib['image_height'])
ci.distortion_model = calib['distortion_model']
ci.D = calib['distortion_coefficients']['data']
ci.K = calib['camera_matrix']['data']
ci.R = calib['rectification_matrix']['data']
ci.P = calib['projection_matrix']['data']
return ci
def callback(msg):
info = CameraInfo()
info.header = msg.header
info.height = data['image_height']
info.width = data['image_width']
info.distortion_model = data['distortion_model']
info.D = data['distortion_coefficients']['data']
info.K = data['camera_matrix']['data']
info.P = data['projection_matrix']['data']
info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
info_pub.publish(info)
def yaml_to_camera_info(yaml_data):
camera_info = CameraInfo()
camera_info.header.frame_id = yaml_data['camera_name']
camera_info.D = yaml_data['distortion_coefficients']['data']
camera_info.K = yaml_data['camera_matrix']['data']
camera_info.P = yaml_data['projection_matrix']['data']
camera_info.R = yaml_data['rectification_matrix']['data']
camera_info.distortion_model = yaml_data['distortion_model']
camera_info.height = yaml_data['image_height']
camera_info.width = yaml_data['image_width']
return camera_info
def parse_yaml(filename):
stream = file(filename, 'r')
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def load_cam_info(calib_file):
cam_info = CameraInfo()
with open(calib_file,'r') as cam_calib_file :
cam_calib = yaml.load(cam_calib_file)
cam_info.height = cam_calib['image_height']
cam_info.width = cam_calib['image_width']
cam_info.K = cam_calib['camera_matrix']['data']
cam_info.D = cam_calib['distortion_coefficients']['data']
cam_info.R = cam_calib['rectification_matrix']['data']
cam_info.P = cam_calib['projection_matrix']['data']
cam_info.distortion_model = cam_calib['distortion_model']
return cam_info
def parse_yaml(self, filename):
stream = file(filename, "r")
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data["image_width"]
cam_info.height = calib_data["image_height"]
cam_info.K = calib_data["camera_matrix"]["data"]
cam_info.D = calib_data["distortion_coefficients"]["data"]
cam_info.R = calib_data["rectification_matrix"]["data"]
cam_info.P = calib_data["projection_matrix"]["data"]
cam_info.distortion_model = calib_data["distortion_model"]
return cam_info
def createMsg(self): msg = CameraInfo() msg.height = 480 msg.width = 640 msg.distortion_model = "plumb_bob" msg.D = [0.08199114285264993, -0.04549390835713936, -0.00040960290587863145, 0.0009833748346549968, 0.0] msg.K = [723.5609128875188, 0.0, 315.9845354772031, 0.0, 732.2615109685506, 242.26660681756633,\ 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [737.1736450195312, 0.0, 316.0324931112791, 0.0, 0.0, 746.1573486328125, 241.59042622688867,\ 0.0, 0.0, 0.0, 1.0, 0.0] self.msg = msg
def yaml_to_CameraInfo(yaml_fname):
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
return camera_info_msg
def get_cam_info(self, cam_name):
cam_name = "calibrations/" + cam_name + ".yaml"
stream = file(os.path.join(os.path.dirname(__file__), cam_name), 'r')
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
return cam_info
def makeROSInfo(image): ci = CameraInfo() head = Header() head.stamp = rospy.Time.now() ci.header = head w,h = image.shape[:2] ci.width = w ci.height = h ci.distortion_model = 'plumb_bob' return ci
def GetCameraInfo(width, height):
cam_info = CameraInfo()
cam_info.width = width
cam_info.height = height
cam_info.distortion_model = model
#cam_info.D = [0.0]*5
#cam_info.K = [0.0]*9
#cam_info.R = [0.0]*9
#cam_info.P = [0.0]*12
cam_info.D = D
cam_info.K = K
cam_info.R = R
cam_info.P = P
cam_info.binning_x = 0
cam_info.binning_y = 0
return cam_info
def default(self, ci='unused'):
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = self.encoding
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.D = [0]
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
if self.pub_tf:
self.publish_with_robot_transform(image)
else:
self.publish(image)
self.topic_camera_info.publish(camera_info)
def parse_yaml(filename):
stream = file(filename, "r")
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data["image_width"]
cam_info.height = calib_data["image_height"]
cam_info.K = calib_data["camera_matrix"]["data"]
cam_info.D = calib_data["distortion_coefficients"]["data"]
cam_info.R = calib_data["rectification_matrix"]["data"]
cam_info.P = calib_data["projection_matrix"]["data"]
cam_info.distortion_model = calib_data["distortion_model"]
cam_info.binning_x = calib_data["binning_x"]
cam_info.binning_y = calib_data["binning_y"]
cam_info.roi.x_offset = calib_data["roi"]["x_offset"]
cam_info.roi.y_offset = calib_data["roi"]["y_offset"]
cam_info.roi.height = calib_data["roi"]["height"]
cam_info.roi.width = calib_data["roi"]["width"]
cam_info.roi.do_rectify = calib_data["roi"]["do_rectify"]
return cam_info
def parse_yaml(filename):
stream = file(filename, 'r')
calib_data = yaml.load(stream)
cam_info = CameraInfo()
cam_info.width = calib_data['image_width']
cam_info.height = calib_data['image_height']
cam_info.K = calib_data['camera_matrix']['data']
cam_info.D = calib_data['distortion_coefficients']['data']
cam_info.R = calib_data['rectification_matrix']['data']
cam_info.P = calib_data['projection_matrix']['data']
cam_info.distortion_model = calib_data['distortion_model']
cam_info.binning_x = calib_data['binning_x']
cam_info.binning_y = calib_data['binning_y']
cam_info.roi.x_offset = calib_data['roi']['x_offset']
cam_info.roi.y_offset = calib_data['roi']['y_offset']
cam_info.roi.height = calib_data['roi']['height']
cam_info.roi.width = calib_data['roi']['width']
cam_info.roi.do_rectify = calib_data['roi']['do_rectify']
return cam_info
def build_camera_info(self):
"""
computing camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
camera_info.header.frame_id = self.name
camera_info.width = self.carla_object.ImageSizeX
camera_info.height = self.carla_object.ImageSizeY
camera_info.distortion_model = 'plumb_bob'
cx = self.carla_object.ImageSizeX / 2.0
cy = self.carla_object.ImageSizeY / 2.0
fx = self.carla_object.ImageSizeX / (
2.0 * math.tan(self.carla_object.FOV * math.pi / 360.0))
fy = fx
camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
camera_info.D = [0, 0, 0, 0, 0]
camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
self._camera_info = camera_info
def get_camera_info(hard_coded=True):
if hard_coded:
cx = 319.5
cy = 239.5
fx = 525.5
fy = 525.5
return (cx, cy, fx, fy)
#if we are using a different camera, then
#we can listen to the ros camera info topic for that device
#and get our values here.
else:
import image_geometry
from sensor_msgs.msg import CameraInfo
cam_info = CameraInfo()
cam_info.height = 480
cam_info.width = 640
cam_info.distortion_model = "plumb_bob"
cam_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
cam_info.K = [525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0]
cam_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
cam_info.P = [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0]
cam_info.binning_x = 0
cam_info.binning_y = 0
cam_info.roi.x_offset = 0
cam_info.roi.y_offset = 0
cam_info.roi.height = 0
cam_info.roi.width = 0
cam_info.roi.do_rectify = False
camera_model = image_geometry.PinholeCameraModel()
camera_model.fromCameraInfo(cam_info)
return camera_model.cx(), camera_model.cy(), camera_model.fx(), camera_model.fy()
def dr_callback(self, config, level):
ci = CameraInfo()
ci.header.stamp = rospy.Time.now()
ci.width = config['width']
ci.height = config['height']
ci.distortion_model = config['distortion_model']
ci.D = [config['d0'], config['d1'], config['d2'], config['d3'], config['d4']]
ci.K[0 * 3 + 0] = config['fx']
ci.K[0 * 3 + 2] = config['cx']
ci.K[1 * 3 + 1] = config['fy']
ci.K[1 * 3 + 2] = config['cy']
ci.K[2 * 3 + 2] = 1
ci.P[0 * 4 + 0] = config['fx']
ci.P[0 * 4 + 2] = config['cx']
ci.P[1 * 4 + 1] = config['fy']
ci.P[1 * 4 + 2] = config['cy']
ci.P[2 * 4 + 2] = 1
ci.R[0] = 1
ci.R[4] = 1
ci.R[8] = 1
self.camera_info = ci
self.pub.publish(ci)
return config
def loadCalibrationFile(filename, cname):
""" Load calibration data from a file.
This function returns a `sensor_msgs/CameraInfo`_ message, based
on the filename parameter. An empty or non-existent file is *not*
considered an error; a null CameraInfo being provided in that
case.
:param filename: location of CameraInfo to read
:param cname: Camera name.
:returns: `sensor_msgs/CameraInfo`_ message containing calibration,
if file readable; null calibration message otherwise.
:raises: :exc:`IOError` if an existing calibration file is unreadable.
"""
ci = CameraInfo()
try:
f = open(filename)
calib = yaml.load(f)
if calib is not None:
if calib['camera_name'] != cname:
rospy.logwarn("[" + cname + "] does not match name " +
calib['camera_name'] + " in file " + filename)
# fill in CameraInfo fields
ci.width = calib['image_width']
ci.height = calib['image_height']
ci.distortion_model = calib['distortion_model']
ci.D = calib['distortion_coefficients']['data']
ci.K = calib['camera_matrix']['data']
ci.R = calib['rectification_matrix']['data']
ci.P = calib['projection_matrix']['data']
except IOError: # OK if file did not exist
pass
return ci
def run(self):
img = Image()
r = rospy.Rate(self.config['frame_rate'])
while self.is_looping():
if self.pub_img_.get_num_connections() == 0:
if self.nameId:
rospy.loginfo('Unsubscribing from camera as nobody listens to the topics.')
self.camProxy.unsubscribe(self.nameId)
self.nameId = None
r.sleep()
continue
if self.nameId is None:
self.reconfigure(self.config, 0)
r.sleep()
continue
image = self.camProxy.getImageRemote(self.nameId)
if image is None:
continue
# Deal with the image
if self.config['use_ros_time']:
img.header.stamp = rospy.Time.now()
else:
img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
elif image[3] == kYUV422ColorSpace:
encoding = "yuv422" # this works only in ROS groovy and later
elif image[3] == kDepthColorSpace:
encoding = "mono16"
else:
rospy.logerr("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
self.pub_img_.publish(img)
# deal with the camera info
if self.config['source'] == kDepthCamera and image[3] == kDepthColorSpace:
infomsg = CameraInfo()
# yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
ratio_x = float(640)/float(img.width)
ratio_y = float(480)/float(img.height)
infomsg.width = img.width
infomsg.height = img.height
# [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
infomsg.K = [ 525, 0, 3.1950000000000000e+02,
0, 525, 2.3950000000000000e+02,
0, 0, 1 ]
infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
0, 525, 2.3950000000000000e+02, 0,
0, 0, 1, 0 ]
for i in range(3):
infomsg.K[i] = infomsg.K[i] / ratio_x
infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
infomsg.P[i] = infomsg.P[i] / ratio_x
infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
infomsg.D = []
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = ""
infomsg.header = img.header
self.pub_info_.publish(infomsg)
elif self.config['camera_info_url'] in self.camera_infos:
infomsg = self.camera_infos[self.config['camera_info_url']]
infomsg.header = img.header
self.pub_info_.publish(infomsg)
r.sleep()
if (self.nameId):
rospy.loginfo("unsubscribing from camera ")
self.camProxy.unsubscribe(self.nameId)
camera = '' if len(sys.argv) < 4 else sys.argv[3]
with open(file_cfg) as f:
lines = f.readlines()
parser = lambda label: map(float, [line for line in lines \
if line.startswith(label)][0].strip().split('(')[1].split(')')[0].split(','))
i = parser('CAMERA%s_INTRINSIC:'%camera)
d = parser('CAMERA%s_DISTORTION:'%camera)
# TODO
intrinsic = [[0]*3]*3
# http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
# http://wiki.ros.org/image_pipeline/CameraInfo
camera_info = CameraInfo()
camera_info.distortion_model = 'plumb_bob'
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
camera_info.D = [0]
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
with open(file_yml, 'w') as f:
f.write(str(camera_info))
def main():
parser = argparse.ArgumentParser(
description="Convert an LCM log to a ROS bag (mono/stereo images only).")
parser.add_argument('lcm_file', help='Input LCM log.', action='store')
parser.add_argument('left_img_channel', help='LCM channel for left image.')
parser.add_argument('left_camera_yml',
help='Image calibration YAML file from ROS calibrator')
parser.add_argument('--right_img_channel', help='LCM channel for right image.',
action='append', dest='lcm_channels')
parser.add_argument('--right_camera_yml',
help='Image calibration YAML file from ROS calibrator',
action='append', dest='yml_files')
roi_parser = parser.add_argument_group("Format7/ROI", "Format7/ROI options needed when dealing with non-standard video modes.")
roi_parser.add_argument('--binning_x', default=1, type=int, dest='binning_x', help='Image binning factor.')
roi_parser.add_argument('--binning_y', default=1, type=int, dest='binning_y', help='Image binning factor.')
roi_parser.add_argument('--x_offset', default=0, type=int, dest='x_offset', help="ROI x offset (in UNBINNED pixels)")
roi_parser.add_argument('--y_offset', default=0, type=int, dest='y_offset', help="ROI y offset (in UNBINNED pixels)")
roi_parser.add_argument('--width', default=640, type=int, dest='width', help="ROI width (in UNBINNED pixels)")
roi_parser.add_argument('--height', default=480, type=int, dest='height', help="ROI height (in UNBINNED pixels)")
roi_parser.add_argument('--do_rectify', default=False, type=bool, dest='do_rectify', help="Do rectification when querying ROI.")
args = parser.parse_args()
if args.lcm_channels is None:
args.lcm_channels = []
if args.yml_files is None:
args.yml_files = []
args.lcm_channels.append(args.left_img_channel)
args.yml_files.append(args.left_camera_yml)
if len(args.lcm_channels) != len(args.yml_files):
print "LCM channel-YAML file mismatch!"
print "Converting images in %s to ROS bag file..." % (args.lcm_file)
log = lcm.EventLog(args.lcm_file, 'r')
bag = rosbag.Bag(args.lcm_file + '.images.bag', 'w')
# Read in YAML files.
yml = []
for y in args.yml_files:
yml.append(yaml.load(file(y)))
try:
count = 0
for event in log:
for ii in range(len(args.lcm_channels)):
l = args.lcm_channels[ii]
y = yml[ii]
if event.channel == l:
lcm_msg = image_t.decode(event.data)
# Fill in image.
if lcm_msg.pixelformat != image_t.PIXEL_FORMAT_MJPEG:
print "Encountered non-MJPEG compressed image. Skipping..."
continue
ros_msg = CompressedImage()
ros_msg.header.seq = event.eventnum
secs_float = float(lcm_msg.utime)/1e6
nsecs_float = (secs_float - np.floor(secs_float)) * 1e9
ros_msg.header.stamp.secs = np.uint32(np.floor(secs_float))
ros_msg.header.stamp.nsecs = np.uint32(np.floor(nsecs_float))
ros_msg.header.frame_id = "camera"
ros_msg.format = 'jpeg'
ros_msg.data = lcm_msg.data
# Fill in camera info
camera_info = CameraInfo()
camera_info.header = ros_msg.header
camera_info.height = y['image_height']
camera_info.width = y['image_width']
if y["distortion_model"] != "plumb_bob":
print "Encountered non-supported distorion model %s. Skipping..." % y["distortion_model"]
continue
camera_info.distortion_model = y["distortion_model"]
camera_info.D = y["distortion_coefficients"]['data']
camera_info.K = y["camera_matrix"]['data']
camera_info.R = y["rectification_matrix"]['data']
camera_info.P = y["projection_matrix"]['data']
camera_info.binning_x = args.binning_x
camera_info.binning_y = args.binning_y
camera_info.roi.x_offset = args.x_offset
camera_info.roi.y_offset = args.y_offset
camera_info.roi.height = args.height
camera_info.roi.width = args.width
camera_info.roi.do_rectify = args.do_rectify
bag.write("/camera/" + l + "/image_raw/compressed", ros_msg, ros_msg.header.stamp)
bag.write("/camera/" + l + "/camera_info", camera_info, camera_info.header.stamp)
count += 1
if count % 100 == 0:
print "Wrote %i events" % count
finally:
log.close()
bag.close()
print("Done.")
def main_loop(self):
img = Image()
cimg = Image()
r = rospy.Rate(15)
while not rospy.is_shutdown():
if self.pub_img_.get_num_connections() == 0:
r.sleep()
continue
image = self.camProxy.getImageRemote(self.nameId)
if image is None:
continue
# Deal with the image
'''
#Images received from NAO have
if self.config['use_ros_time']:
img.header.stamp = rospy.Time.now()
else:
img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
'''
img.header.stamp = rospy.Time.now()
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
if image[3] == kDepthColorSpace:
encoding = "mono16"
else:
rospy.logerr("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
self.pub_img_.publish(img)
# deal with the camera info
infomsg = CameraInfo()
infomsg.header = img.header
# yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
ratio_x = float(640)/float(img.width)
ratio_y = float(480)/float(img.height)
infomsg.width = img.width
infomsg.height = img.height
# [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
infomsg.K = [ 525, 0, 3.1950000000000000e+02,
0, 525, 2.3950000000000000e+02,
0, 0, 1 ]
infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
0, 525, 2.3950000000000000e+02, 0,
0, 0, 1, 0 ]
for i in range(3):
infomsg.K[i] = infomsg.K[i] / ratio_x
infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
infomsg.P[i] = infomsg.P[i] / ratio_x
infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
infomsg.D = []
infomsg.binning_x = 0
infomsg.binning_y = 0
infomsg.distortion_model = ""
self.pub_info_.publish(infomsg)
#Currently we only get depth image from the 3D camera of NAO, so we make up a fake color image (a black image)
#and publish it under image_color topic.
#This should be update when the color image from 3D camera becomes available.
colorimg = np.zeros((img.height,img.width,3), np.uint8)
try:
cimg = self.bridge.cv2_to_imgmsg(colorimg, "bgr8")
cimg.header.stamp = img.header.stamp
cimg.header.frame_id = img.header.frame_id
self.pub_cimg_.publish(cimg)
except CvBridgeError, e:
print e
r.sleep()
caminfo_msg = None
with rosbag.Bag(bag_filename, 'w') as bag:
while(cap.isOpened()):
ret, frame = cap.read()
if ret:
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
msg = bridge.cv2_to_imgmsg(gray, "mono8")
msg.header.seq = seq
msg.header.stamp = rospy.Time.from_sec(initial_time + 1./fps * seq)
bag.write('/camera/image_raw', msg, msg.header.stamp)
if not caminfo_msg:
caminfo_msg = CameraInfo()
caminfo_msg.height, caminfo_msg.width, _ = np.shape(frame)
caminfo_msg.distortion_model = "plumb_bob"
caminfo_msg.D = calib_data['distortion_coefficients'].flatten()
caminfo_msg.K = calib_data['camera_matrix'].flatten()
caminfo_msg.header = msg.header
bag.write('/camera/camera_info', caminfo_msg, caminfo_msg.header.stamp)
seq += 1
else:
break
cap.release()
def post_image(self, component_instance):
""" Publish the data of the Camera as a ROS-Image message.
"""
image_local = component_instance.local_data['image']
if not image_local or image_local == '' or not image_local.image or not component_instance.capturing:
return # press [Space] key to enable capturing
parent_name = component_instance.robot_parent.blender_obj.name
component_name = component_instance.blender_obj.name
image = Image()
image.header.stamp = rospy.Time.now()
image.header.seq = self._seq
# http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support
image.header.frame_id = ('/' + parent_name + '/base_image')
image.height = component_instance.image_height
image.width = component_instance.image_width
image.encoding = 'rgba8'
image.step = image.width * 4
# NOTE: Blender returns the image as a binary string encoded as RGBA
# sensor_msgs.msg.Image.image need to be len() friendly
# TODO patch ros-py3/common_msgs/sensor_msgs/src/sensor_msgs/msg/_Image.py
# to be C-PyBuffer "aware" ? http://docs.python.org/c-api/buffer.html
image.data = bytes(image_local.image)
# http://wiki.blender.org/index.php/Dev:Source/GameEngine/2.49/VideoTexture
# http://www.blender.org/documentation/blender_python_api_2_57_release/bge.types.html#bge.types.KX_Camera.useViewport
for topic in self._topics:
# publish the message on the correct topic
if str(topic.name) == str("/" + parent_name + "/" + component_name + "/image"):
topic.publish(image)
# sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ]
# TODO fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = component_instance.local_data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header.stamp = image.header.stamp
camera_info.header.seq = image.header.seq
camera_info.header.frame_id = image.header.frame_id
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.K = [intrinsic[0][0], intrinsic[1][0], intrinsic[2][0],
intrinsic[0][1], intrinsic[1][1], intrinsic[2][1],
intrinsic[0][2], intrinsic[1][2], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[1][0], intrinsic[2][0], Tx,
intrinsic[0][1], intrinsic[1][1], intrinsic[2][1], Ty,
intrinsic[0][2], intrinsic[1][2], intrinsic[2][2], 0]
for topic in self._topics:
# publish the message on the correct topic
if str(topic.name) == str("/" + parent_name + "/" + component_name + "/camera_info"):
topic.publish(camera_info)
self._seq = self._seq + 1
pub_info = rospy.Publisher("~camera_info", CameraInfo)
frame_id = rospy.get_param('~frame_id', 'dummy_camera')
width = rospy.get_param('~width', 640)
height = rospy.get_param('~height', 480)
r = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.Time(0)
dummy_camera_info = CameraInfo()
dummy_camera_info.width = width
dummy_camera_info.height = height
dummy_camera_info.header.frame_id = frame_id
dummy_camera_info.header.stamp = now
dummy_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
dummy_camera_info.K = [525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0]
dummy_camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
dummy_camera_info.P = [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0]
dummy_camera_info.distortion_model = "plumb_bob"
pub_info.publish(dummy_camera_info)
dummy_img = Image()
dummy_img.width = width
dummy_img.height = height
dummy_img.header.frame_id = frame_id
dummy_img.header.stamp = now
dummy_img.step = width
dummy_img.encoding = "mono8"
dummy_img.data = [0] * width * height
pub.publish(dummy_img)
r.sleep()
def default(self, ci='unused'):
""" Publish the data of the Camera as a ROS Image message. """
if not self.component_instance.capturing:
return # press [Space] key to enable capturing
image_local = self.data['image']
image = Image()
image.header = self.get_ros_header()
image.header.frame_id += '/base_image'
image.height = self.component_instance.image_height
image.width = self.component_instance.image_width
image.encoding = 'rgba8'
image.step = image.width * 4
# VideoTexture.ImageRender implements the buffer interface
image.data = bytes(image_local)
# sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ]
# fill this 3 parameters to get correcty image with stereo camera
Tx = 0
Ty = 0
R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
intrinsic = self.data['intrinsic_matrix']
camera_info = CameraInfo()
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
camera_info.distortion_model = 'plumb_bob'
camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2],
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2],
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]]
camera_info.R = R
camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx,
intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty,
intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0]
self.publish(image)
self.topic_camera_info.publish(camera_info)
