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 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 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 publishimage(pub, value):
msg = bridge.cv2_to_imgmsg(value.load(), 'mono8')
msg.header = createheader(value)
pub.img.publish(msg)
publishtf(value, 'cam1')
tf = value >> "cam1"
K = value.K
R = tf[0:3, 0:3]
P = K.dot(tf[0:3, :])
name = distmodel
dist = value.distortion(distmodel)
if name == 'radtan':
name = 'plumb_bob'
dist = list(dist)
dist.append(0) # use k3 = 0
elif name == 'fov':
dist = [dist]
msg = CameraInfo(width=res.width,
height=res.height,
distortion_model=name,
D=dist,
K=K.flatten().tolist(),
R=R.flatten().tolist(),
P=P.flatten().tolist())
msg.header = createheader(value)
pub.cam.publish(msg)
pub.exp.publish(Float32(data=value.exposure))
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 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 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 _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 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 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 write_camera(self, folder, topic, frame, ext):
print(' Writing camera: ' + topic)
records = self.read_csv(self.path + self.name + '/' + folder + '/' +
'timestamps.txt')
bridge = cv_bridge.CvBridge()
seq = 0
for row in tqdm(records):
filename = row[0] + ext
timestamp = row[1]
image_path = self.path + self.name + '/' + folder + '/' + filename
img = cv2.imread(image_path)
encoding = "bgr8"
image_message = bridge.cv2_to_imgmsg(img, encoding=encoding)
image_message.header.frame_id = frame
image_message.header.stamp = self.timestamp_to_stamp(timestamp)
image_message.header.seq = seq
seq += 1
self.bag.write(topic + '/camera',
image_message,
t=image_message.header.stamp)
camera_info = CameraInfo()
camera_info.header = image_message.header
camera_info.height = img.shape[0]
camera_info.width = img.shape[1]
camera_info.distortion_model = "plumb_bob"
camera_info.D = [
-0.15402600433198144, 0.08558850995478451,
0.002075813671243975, 0.0006580423624898167,
-0.016293022125192957
]
camera_info.K = [
1376.8981317210023, 0.0, 957.4934213691823, 0.0,
1378.3903002987945, 606.5795886217022, 0.0, 0.0, 1.0
]
camera_info.R = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
camera_info.P = [
1376.8981317210023, 0.0, 957.4934213691823, 0.0, 0.0,
1378.3903002987945, 606.5795886217022, 0.0, 0.0, 0.0, 1.0, 0.0
]
camera_info.binning_x = 1
camera_info.binning_y = 1
self.bag.write(topic + '/camera_info',
image_message,
t=image_message.header.stamp)
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 cloud_callback(msg):
global image_header
CI = CameraInfo()
CI.header = msg.header
CI.header.frame_id = "camera"
image_header = CI.header
CI.height = 360
CI.width = 480
CI.K = [425, 0, 480 / 2, 0, 425, 360 / 2, 0, 0, 1]
CI.P = [425, 0, 480 / 2, 0, 0, 425, 360 / 2, 0, 0, 0, 1, 0]
CI_pub.publish(CI)
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 camera_info_left(self):
msg_header = Header()
msg_header.frame_id = "uav_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 camera_info_right(self):
msg_header = Header()
msg_header.frame_id = "uav_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 depth_estm(li,ri)
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=5)
def cameraInfoCallback(self, input_msg):
if self.tf_broadcaster is None:
self.setUpTfBroadcaster(input_msg.header.frame_id,
input_msg.header.stamp)
output_msg = CameraInfo()
output_msg.header = input_msg.header
output_msg.header.frame_id = rotated_frame_id(
input_msg.header.frame_id)
output_msg.width = input_msg.height
output_msg.height = input_msg.width
output_msg.distortion_model = input_msg.distortion_model
# Unclear what to do with D. Luckily, we work with virtual cameras
# without distortion.
output_msg.D = input_msg.D
output_msg.K[0] = input_msg.K[4]
output_msg.K[1] = 0
output_msg.K[2] = input_msg.K[5]
output_msg.K[3] = 0
output_msg.K[4] = input_msg.K[0]
output_msg.K[5] = input_msg.K[2]
output_msg.K[6] = 0
output_msg.K[7] = 0
output_msg.K[8] = 1
output_msg.R = input_msg.R
output_msg.P[0] = input_msg.P[5]
output_msg.P[1] = 0
output_msg.P[2] = input_msg.P[6]
output_msg.P[3] = 0 #input_msg.P[7]
output_msg.P[4] = 0
output_msg.P[5] = input_msg.P[0]
output_msg.P[6] = input_msg.P[2]
output_msg.P[7] = 0 #input_msg.P[3]
output_msg.P[8] = 0
output_msg.P[9] = 0
output_msg.P[10] = 1
output_msg.P[11] = 0
# Probably like this? In Virtual, both values are zero.
output_msg.binning_x = input_msg.binning_y
output_msg.binning_y = input_msg.binning_x
output_msg.roi.x_offset = input_msg.roi.y_offset
output_msg.roi.y_offset = input_msg.roi.x_offset
output_msg.roi.height = input_msg.roi.width
output_msg.roi.width = input_msg.roi.height
output_msg.roi.do_rectify = input_msg.roi.do_rectify
self.camera_info_publisher.publish(output_msg)
def construct_info_msg(EI_loader, im_size, name="left"):
info = CameraInfo()
info.height = im_size[0]
info.width = im_size[1]
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
info.distortion_model = "plumb_bob"
if name == "left":
info.D = EI_loader.paramaters.d1.reshape(1,5).tolist()[0]
info.K = EI_loader.paramaters.K1.reshape(1,9).tolist()[0]
info.R = EI_loader.paramaters.R1.reshape(1,9).tolist()[0]
info.P = EI_loader.paramaters.P1.reshape(1,12).tolist()[0]
header.frame_id = 'camera_left'
info.header = header
if name == "right":
info.D = EI_loader.paramaters.d2.reshape(1,5).tolist()[0]
info.K = EI_loader.paramaters.K2.reshape(1,9).tolist()[0]
info.R = EI_loader.paramaters.R2.reshape(1,9).tolist()[0]
info.P = EI_loader.paramaters.P2.reshape(1,12).tolist()[0]
header.frame_id = 'camera_right'
info.header = header
return info, header
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 _cb(self, cloud_msg):
info_msg = CameraInfo()
info_msg.header = cloud_msg.header
info_msg.header.frame_id = self.frame_id
fx = 589.3664541825391
fy = 589.3664541825391
info_msg.height = 480
info_msg.width = 640
info_msg.distortion_model = "plumb_bob"
cx = info_msg.width // 2
cy = info_msg.height // 2
info_msg.D = [1e-08, 1e-08, 1e-08, 1e-08, 1e-08]
info_msg.K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0]
info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
info_msg.P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
self.pub.publish(info_msg)
def create_mesg(frame_id, height, width, distortion_model, D, K, R, P):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = frame_id
cam_msg = CameraInfo()
cam_msg.header = header
cam_msg.height = height
cam_msg.width = width
cam_msg.distortion_model = distortion_model
cam_msg.D = D
cam_msg.K = K
cam_msg.R = R
cam_msg.P = P
return cam_msg
def __constructROSCameraInfo(calibration, dimensions, timestamp): (calibrationWidth, calibrationHeight) = dimensions (ret, cameraMatrix, distortion, rectification, projection) = calibration h = Header() h.stamp = timestamp distortion = __makeTuple(__unwrapValues(distortion)) cameraInfo = CameraInfo() cameraInfo.width = calibrationWidth cameraInfo.height = calibrationHeight cameraInfo.header = h cameraInfo.distortion_model = "plumb_bob" cameraInfo.D = distortion cameraInfo.K = cameraMatrix.flatten() cameraInfo.R = rectification.flatten() cameraInfo.P = projection.flatten() return cameraInfo
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 _createCameraInfoMessage(self):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "xtion_rgb_optical_frame"
message = CameraInfoMessage()
message.header = header
message.height = 640
message.width = 480
message.distortion_model = "plumb_bob"
# message.D = [0.01135616746000704, -0.0837425949431849, -0.003641203664053122, 0.002735692509361075, 0.0]
# message.K = [524.3530291768766, 0.0, 327.7532296877647, 0.0, 523.6295167359308, 228.0511491808866, 0.0, 0.0, 1.0]
# message.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# message.P = [519.40966796875, 0.0, 329.275793651701, 0.0, 0.0, 521.64208984375, 226.187870027934, 0.0, 0.0, 0.0, 1.0, 0.0]
# message.D = [-0.02817191766837119, -0.04456807713959347, 0.001406068512987967, 0.0004264518716543618, 0]
# message.K = [511.9643945409745, 0, 314.6780421043781, 0, 512.8531516026462, 253.7017236810161, 0, 0, 1]
# message.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
# message.P = [502.284423828125, 0, 314.6593117652083, 0, 0, 508.2977905273438, 254.495007154881, 0, 0, 0, 1, 0]
# message.D = [-0.005619, -0.077768, -0.003193, 0.008015, 0.000000]
# message.K = [502.809179, 0.000000, 313.329244, 0.000000, 500.768056, 235.729092, 0.000000, 0.000000, 1.000000]
# message.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
# message.P = [494.846283, 0.000000, 318.289662, 0.000000, 0.000000, 498.635223, 234.452651, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
message.D = [
-0.009740021407495024, -0.06810923458853556,
-0.0032394490021143262, 0.0032409844218177944, 0.0
]
message.K = [
522.6068524353595, 0.0, 322.48982744054325, 0.0, 524.4301716157004,
232.50832420397631, 0.0, 0.0, 1.0
]
message.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
message.P = [
515.2507934570312, 0.0, 324.70135989618575, 0.0, 0.0,
521.61376953125, 231.24797544667672, 0.0, 0.0, 0.0, 1.0, 0.0
]
# optional parameters, binning and roi, are not set
return message
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 _build_camera_info(self):
"""
Private Function used to compute camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
# store info without header
camera_info.header = None
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
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 fetch_image(self, cam):
cam.simulate()
if not cam.pixels:
return None, None
cv_img = cv.CreateImageHeader((cam.width , cam.height), cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_img, cam.pixels, cam.width*3)
cv.ConvertImage(cv_img, cv_img, cv.CV_CVTIMG_FLIP)
im = self.bridge.cv_to_imgmsg(cv_img, "rgb8")
caminfo = CameraInfo()
caminfo.header = im.header
caminfo.height = cam.height
caminfo.width = cam.width
caminfo.D = 5*[0.]
caminfo.K = sum([list(r) for r in cam.K],[])
caminfo.P = sum([list(r) for r in cam.P],[])
caminfo.R = sum([list(r) for r in cam.R],[])
return im, caminfo
def run(self):
r = rospy.Rate(self.frequency) # 10hz
i = 0
print(self.num_imgs)
while not rospy.is_shutdown():
if (i < self.num_imgs):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.seq = i
print("--------------------------------")
imgs = []
info_msgs = []
for j in range(self.num_topics):
img = cv2.imread(self.im_list[i + j * self.num_imgs],
cv2.IMREAD_GRAYSCALE)
print(self.im_list[i + j * self.num_imgs])
ros_img = self.bridge.cv2_to_imgmsg(img, "mono8")
ros_img.header = h
imgs.append(ros_img)
cam_info_msg = CameraInfo()
cam_info_msg.header = h
info_msgs.append(cam_info_msg)
for k in range(self.num_topics):
if self.to_bag:
print("writing" + str(i))
self.write_bag.write(self.topic_names[k], imgs[k],
imgs[k].header.stamp)
self.write_bag.write(self.info_topic_names[k],
info_msgs[k],
info_msgs[k].header.stamp)
else:
self.pubs[k].publish(imgs[k])
self.cam_info_pubs[k].publish(info_msgs[k])
i = i + 1
r.sleep()
else:
print(i)
break
r.sleep()
if self.to_bag:
self.write_bag.close()
def fetch_image(self, cam):
cam.simulate()
if not cam.pixels:
return None, None
cv_img = cv.CreateImageHeader((cam.width, cam.height), cv.IPL_DEPTH_8U,
3)
cv.SetData(cv_img, cam.pixels, cam.width * 3)
cv.ConvertImage(cv_img, cv_img, cv.CV_CVTIMG_FLIP)
im = self.bridge.cv_to_imgmsg(cv_img, "rgb8")
caminfo = CameraInfo()
caminfo.header = im.header
caminfo.height = cam.height
caminfo.width = cam.width
caminfo.D = 5 * [0.]
caminfo.K = sum([list(r) for r in cam.K], [])
caminfo.P = sum([list(r) for r in cam.P], [])
caminfo.R = sum([list(r) for r in cam.R], [])
return im, caminfo
def build_camera_info(width, height, f, x, y):
"""
Private function to compute camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
# store info without header
camera_info.header = None
camera_info.width = width
camera_info.height = height
camera_info.distortion_model = 'plumb_bob'
cx = camera_info.width / 2.0
cy = camera_info.height / 2.0
fx = f
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, x, 0, fy, cy, y, 0, 0, 1.0, 0]
return camera_info
def construct_info(header: Header, info: SimCameraInfo, height: int, width: int) -> CameraInfo:
msg = CameraInfo()
Tx = 0.0 # Assumed for now since we are not using stereo
hfov = np.deg2rad(info.fov)
# https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/86
f = width / (2 * np.tan(0.5 * hfov))
Fx = Fy = f
cx = width / 2
cy = height / 2
K = np.array([
[Fx, 0.0, cx],
[0.0, Fy, cy],
[0.0, 0.0, 1 ]
]).flatten()
R = np.array([
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]
]).flatten()
P = np.array([
[Fx, 0.0, cx, Tx ],
[0.0, Fy, cy, 0.0],
[0.0, 0.0, 1.0, 0.0]
]).flatten()
msg.header = header
msg.height = height
msg.width = width
msg.k = K
msg.r = R
msg.p = P
msg.binning_x = 0
msg.binning_y = 0
return msg
def build_camera_info(self, attributes):
"""
Private function to compute camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
# store info without header
camera_info.header = None
camera_info.width = int(attributes['width'])
camera_info.height = int(attributes['height'])
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(attributes['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]
return camera_info
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()
def publish(self, incomingData):
if "apriltags" in incomingData:
for tag in incomingData["apriltags"]:
# Publish the relative pose
newApriltagDetectionMsg = AprilTagExtended()
newApriltagDetectionMsg.header.stamp.secs = int(
tag["timestamp_secs"])
newApriltagDetectionMsg.header.stamp.nsecs = int(
tag["timestamp_nsecs"])
newApriltagDetectionMsg.header.frame_id = str(tag["source"])
newApriltagDetectionMsg.transform.translation.x = float(
tag["tvec"][0])
newApriltagDetectionMsg.transform.translation.y = float(
tag["tvec"][1])
newApriltagDetectionMsg.transform.translation.z = float(
tag["tvec"][2])
newApriltagDetectionMsg.transform.rotation.x = float(
tag["qvec"][0])
newApriltagDetectionMsg.transform.rotation.y = float(
tag["qvec"][1])
newApriltagDetectionMsg.transform.rotation.z = float(
tag["qvec"][2])
newApriltagDetectionMsg.transform.rotation.w = float(
tag["qvec"][3])
newApriltagDetectionMsg.tag_id = int(tag["tag_id"])
newApriltagDetectionMsg.tag_family = tag["tag_family"]
newApriltagDetectionMsg.hamming = int(tag["hamming"])
newApriltagDetectionMsg.decision_margin = float(
tag["decision_margin"])
newApriltagDetectionMsg.homography = tag["homography"].flatten(
)
newApriltagDetectionMsg.center = tag["center"]
newApriltagDetectionMsg.corners = tag["corners"].flatten()
newApriltagDetectionMsg.pose_error = tag["pose_error"]
self.publish_queue.put({
"topic": "apriltags",
"message": newApriltagDetectionMsg
})
# self.publishers["apriltags"].publish(newApriltagDetectionMsg)
# self.logger.info("Published pose for tag %d in sequence %d" % (
# tag["tag_id"], self.seq_stamper))
# Publish the test and raw data if submitted and requested:
if self.ACQ_TEST_STREAM:
if "test_stream_image" in incomingData:
imgMsg = incomingData["test_stream_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "test_stream_image",
"message": imgMsg
})
# self.publishers["test_stream_image"].publish(imgMsg)
if "raw_image" in incomingData:
imgMsg = incomingData["raw_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "raw_image",
"message": imgMsg
})
# self.publishers["raw_image"].publish(imgMsg)
if "rectified_image" in incomingData:
imgMsg = incomingData["rectified_image"]
imgMsg.header.seq = self.seq_stamper
self.publish_queue.put({
"topic": "rectified_image",
"message": imgMsg
})
# self.publishers["rectified_image"].publish(imgMsg)
if "raw_camera_info" in incomingData:
self.publish_queue.put({
"topic":
"raw_camera_info",
"message":
incomingData["raw_camera_info"]
})
# self.publishers["raw_camera_info"].publish(
# incomingData["raw_camera_info"])
if "new_camera_matrix" in incomingData:
new_camera_info = CameraInfo()
try:
new_camera_info.header = incomingData[
"raw_camera_info"].header
new_camera_info.height = incomingData["raw_image"].shape[0]
new_camera_info.width = incomingData["raw_image"].shape[1]
new_camera_info.distortion_model = incomingData[
"raw_camera_info"].distortion_model
new_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0]
except:
pass
new_camera_info.K = incomingData["new_camera_matrix"].flatten()
self.publish_queue.put({
"topic": "new_camera_matrix",
"message": new_camera_info
})
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)
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 = ""
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)
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 = [
883.998642, 0.000000, 349.540253, 0.000000, 887.969815, 247.902874,
0.000000, 0.000000, 1.000000
]
left_cam_info.D = [0.125962, -0.905045, 0.006512, 0.007531, 0.000000]
left_cam_info.R = [
0.985389, 0.006639, 0.170189, -0.004920, 0.999933, -0.010521,
-0.170248, 0.009530, 0.985355
]
left_cam_info.P = [
1022.167889, 0.000000, 150.220785, 0.000000, 0.000000, 1022.167889,
249.024044, 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 = [
874.019843, 0.000000, 331.121922, 0.000000, 875.918758, 244.867443,
0.000000, 0.000000, 1.000000
]
right_cam_info.D = [0.041385, -0.059698, 0.005392, 0.009075, 0.000000]
right_cam_info.R = [
0.976610, 0.003803, 0.214985, -0.005979, 0.999937, 0.009472,
-0.214936, -0.010535, 0.976571
]
right_cam_info.P = [
1022.167889, 0.000000, 150.220785, -41.006903, 0.000000,
1022.167889, 249.024044, 0.000000, 0.000000, 0.000000, 1.000000,
0.000000
]
right_cam_info.distortion_model = 'plumb_bob'
rate = rospy.Rate(20)
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()
image.encoding = "mono8"
image.header.frame_id = 'image' if sys.argv[1] == 'mono' \
else 'image_%i'%int(sys.argv[1])
image.header.seq = 0 if len(sys.argv) < 3 else int(sys.argv[2])
with open('camera_info.yml') as f:
data = yaml.load(f.read())
del data['roi'], data['header']
camera_info = CameraInfo(**data)
with rosbag.Bag('%s.bag'%image.header.frame_id, 'w') as bag:
while 1:
try:
img, time = image_time(image.header.frame_id + \
'_%07i'%image.header.seq)
except IOError as error:
print("stop at %i: %s"%(image.header.seq, str(error)))
break
image.data = img.tostring()
image.height, image.width = img.shape
image.step = image.width # grayscale image
image.header.stamp = rospy.Time.from_sec(time)
camera_info.header = image.header
camera_info.height = image.height
camera_info.width = image.width
bag.write('/%s/image'%image.header.frame_id, image, image.header.stamp)
bag.write('/%s/camera_info'%image.header.frame_id, camera_info, \
image.header.stamp)
image.header.seq += 1
def __call__(self, channel, data):
lcm_msg = agile.state_t.decode(data)
# List of messages and topics to publish
topics_to_publish = []
msgs_to_publish = []
# Populate pose message
topics_to_publish.append(self.rostopic)
ros_msg = PoseStamped()
ros_msg.header = self.make_header(lcm_msg.utime)
ros_msg.pose.position.x = lcm_msg.position[0]
ros_msg.pose.position.y = lcm_msg.position[1]
ros_msg.pose.position.z = lcm_msg.position[2]
ros_msg.pose.orientation.w = lcm_msg.orient[0]
ros_msg.pose.orientation.x = lcm_msg.orient[1]
ros_msg.pose.orientation.y = lcm_msg.orient[2]
ros_msg.pose.orientation.z = lcm_msg.orient[3]
msgs_to_publish.append(ros_msg)
# Populate TF tree message
tf_packet = tfMessage()
# Mocap NED <> Mocap ENU
# topics_to_publish.append("/tf_static")
# tf_ned_2_enu_msg = TransformStamped()
# tf_ned_2_enu_msg.header = self.make_header(lcm_msg.utime)
# tf_ned_2_enu_msg.header.frame_id = "mocap_ENU"
# tf_ned_2_enu_msg.child_frame_id = "mocap_NED"
# tf_ned_2_enu_msg.transform.translation = ros_msg.pose.position
# tf_ned_2_enu_msg.transform.rotation = ros_msg.pose.orientation
# msgs_to_publish.append(tf_ned_2_enu_msg)
# [ 0, 0.7071068, 0.7071068, 0 ]
# Mocap NED <> drone_body message
tf_drone_msg = TransformStamped()
tf_drone_msg.header = self.make_header(lcm_msg.utime)
tf_drone_msg.header.frame_id = "mocap_NED"
tf_drone_msg.child_frame_id = "body_frame"
tf_drone_msg.transform.translation = ros_msg.pose.position
tf_drone_msg.transform.rotation = ros_msg.pose.orientation
# Append message to transform list
tf_packet.transforms.append(tf_drone_msg)
# Static TFs
# drone_body <> IMU message
# topics_to_publish.append("/tf_static")
# tf_imu_msg = TransformStamped()
# tf_imu_msg.header = self.make_header(lcm_msg.utime)
# tf_imu_msg.header.frame_id = "body_frame"
# tf_imu_msg.child_frame_id = "imu"
# tf_imu_msg.transform.translation.x = 0.0
# tf_imu_msg.transform.translation.y = 0.0
# tf_imu_msg.transform.translation.z = 0.0
# # [ 0.707479362748676 0.002029830683065 -0.007745228390866 0.706688646087723 ]
# tf_imu_msg.transform.rotation.w = 0.707479362748676
# tf_imu_msg.transform.rotation.x = 0.002029830683065
# tf_imu_msg.transform.rotation.y = -0.007745228390866
# tf_imu_msg.transform.rotation.z = 0.706688646087723
# msgs_to_publish.append(tf_imu_msg)
# # drone_body <> camera_l
# topics_to_publish.append("/tf_static")
# tf_cam_l_msg = TransformStamped()
# tf_cam_l_msg.header = self.make_header(lcm_msg.utime)
# tf_cam_l_msg.header.frame_id = "body_frame"
# tf_cam_l_msg.child_frame_id = "camera_l"
# tf_cam_l_msg.transform.translation.x = 0.0
# tf_cam_l_msg.transform.translation.y = -0.05
# tf_cam_l_msg.transform.translation.z = 0.0
# tf_cam_l_msg.transform.rotation.w = 1.0
# tf_cam_l_msg.transform.rotation.x = 0.0
# tf_cam_l_msg.transform.rotation.y = 0.0
# tf_cam_l_msg.transform.rotation.z = 0.0
# msgs_to_publish.append(tf_cam_l_msg)
# # drone_body <> camera_r
# topics_to_publish.append("/tf_static")
# tf_cam_r_msg = TransformStamped()
# tf_cam_r_msg.header = self.make_header(lcm_msg.utime)
# tf_cam_r_msg.header.frame_id = "body_frame"
# tf_cam_r_msg.child_frame_id = "camera_r"
# tf_cam_r_msg.transform.translation.x = 0.0
# tf_cam_r_msg.transform.translation.y = 0.05
# tf_cam_r_msg.transform.translation.z = 0.0
# tf_cam_r_msg.transform.rotation.w = 1.0
# tf_cam_r_msg.transform.rotation.x = 0.0
# tf_cam_r_msg.transform.rotation.y = 0.0
# tf_cam_r_msg.transform.rotation.z = 0.0
# msgs_to_publish.append(tf_cam_r_msg)
# # drone_body <> camera_d
# topics_to_publish.append("/tf_static")
# tf_cam_d_msg = TransformStamped()
# tf_cam_d_msg.header = self.make_header(lcm_msg.utime)
# tf_cam_d_msg.header.frame_id = "body_frame"
# tf_cam_d_msg.child_frame_id = "camera_d"
# tf_cam_d_msg.transform.translation.x = 0.0
# tf_cam_d_msg.transform.translation.y = 0.0
# tf_cam_d_msg.transform.translation.z = 0.0
# # 0.707,0,-0.707,0
# tf_cam_d_msg.transform.rotation.w = 0.707
# tf_cam_d_msg.transform.rotation.x = 0.0
# tf_cam_d_msg.transform.rotation.y = -0.707
# tf_cam_d_msg.transform.rotation.z = 0.0
# msgs_to_publish.append(tf_cam_d_msg)
# Queue TF tree message for publication
topics_to_publish.append("/tf")
msgs_to_publish.append(tf_packet)
# Populate camera info message (if needed)
if (lcm_msg.utime in self.image_timestamps):
cam_info = CameraInfo()
cam_info.header = self.make_header(lcm_msg.utime)
cam_info.height = 768
cam_info.width = 1024
cam_info.distortion_model = 'plumb_bob'
cam_info.K = [
665.107510106, 0., 511.5, 0., 665.107510106, 383.5, 0., 0., 1.
]
cam_info.P = [
665.107510106, 0., 511.5, 0., 0., 665.107510106, 383.5, 0., 0.,
0., 1., 0
]
for cam_topic in [
"/camera_l/camera_info", "/camera_r/camera_info",
"/camera_d/camera_info"
]:
topics_to_publish.append(cam_topic)
msgs_to_publish.append(cam_info)
return topics_to_publish, msgs_to_publish
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.")
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 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)
