本文整理汇总了Python中sensor_msgs.msg.CameraInfo.step方法的典型用法代码示例。如果您正苦于以下问题：Python CameraInfo.step方法的具体用法？Python CameraInfo.step怎么用？Python CameraInfo.step使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.CameraInfo的用法示例。
在下文中一共展示了CameraInfo.step方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: step
# 需要导入模块: from sensor_msgs.msg import CameraInfo [as 别名]
# 或者: from sensor_msgs.msg.CameraInfo import step [as 别名]
def step(self):
t = rospy.Time.now()
_, _, oldwidth, oldheight = glGetFloatv(GL_VIEWPORT)
height = min(oldheight, int(oldwidth / self.aspect + .5))
width = int(self.aspect * height + .5)
glViewport(0, 0, width, height)
msg = CameraInfo()
msg.header.stamp = t
msg.header.frame_id = '/' + self.name
msg.height = height
msg.width = width
f = 1/math.tan(math.radians(self.fovy)/2)*height/2
msg.P = [
f, 0, width/2-.5, 0,
0, f, height/2-.5, 0,
0, 0, 1, 0,
]
self.info_pub.publish(msg)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
perspective(self.fovy, width/height, 0.1)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
# rotates into the FLU coordinate system
glMultMatrixf([
[ 0., 0.,-1., 0.],
[-1., 0., 0., 0.],
[ 0., 1., 0., 0.],
[ 0., 0., 0., 1.]
])
# after that, +x is forward, +y is left, and +z is up
self.set_pose_func()
with GLMatrix:
rotate_to_body(self.base_link_body)
glcamera_from_body = glGetFloatv(GL_MODELVIEW_MATRIX).T
camera_from_body = numpy.array([ # camera from glcamera
[1, 0, 0, 0],
[0,-1, 0, 0],
[0, 0,-1, 0],
[0, 0, 0, 1],
]).dot(glcamera_from_body)
body_from_camera = numpy.linalg.inv(camera_from_body)
tf_br.sendTransform(transformations.translation_from_matrix(body_from_camera),
transformations.quaternion_from_matrix(body_from_camera),
t,
"/" + self.name,
"/base_link")
if not self.image_pub.get_num_connections():
glViewport(0, 0, oldwidth, oldheight)
return
self.world.draw()
x = glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, outputType=None)
x = numpy.reshape(x, (height, width, 4))
x = x[::-1]
msg = Image()
msg.header.stamp = t
msg.header.frame_id = '/' + self.name
msg.height = height
msg.width = width
msg.encoding = 'rgba8'
msg.is_bigendian = 0
msg.step = width * 4
msg.data = x.tostring()
self.image_pub.publish(msg)
glViewport(0, 0, oldwidth, oldheight)