rate = rospy.Rate(40)
rospy.loginfo("Running...")
while not rospy.is_shutdown():
# print loop_count
t0 = time.time()
image_getter.getImage()
xyz_rgb = image_getter.createPointCloud()
##########################################################
#" Visualization by using rviz"
stamp = rospy.Time.now()
pcl_node.update(xyz_rgb)
pcl_node.publish(stamp)
br.sendTransform(image_getter.camera_pos, image_getter.camera_quat,
stamp, "bullet_camera", "world")
##########################################################
rospy.logdebug("Cloud creation time: %f" % (time.time() - t0))
if keyboard.is_pressed('n'):
image_getter.nextCameraPose()
elif keyboard.is_pressed('b'):
image_getter.previousCameraPose()
elif keyboard.is_pressed('r'):
simulation.reset()
simulation.step()
rate.sleep()
