def main(): rospy.init_node('publish_saved_cloud') wait_for_time() argv = rospy.myargv() if len(argv) < 2: print 'Publishes a saved point cloud to a latched topic.' print 'Usage: rosrun applications publish_saved_cloud.py ~/cloud.bag [optional frame]' return path = argv[1] camera = perception.MockCamera() cloud = camera.read_cloud(path) if cloud is None: rospy.logerr('Could not load point cloud from {}'.format(path)) return #new_cloud = copy_point_cloud(cloud, 10, 10) #new_cloud2 = copy_point_cloud(cloud, 4, 2) #final = combine_clouds([new_cloud])#, new_cloud2]) final = cloud topic = 'mock_point_cloud' pub = rospy.Publisher(topic, PointCloud2, queue_size=1) rate = rospy.Rate(2) while not rospy.is_shutdown(): final.header.stamp = rospy.Time.now() if len(argv) > 2: final.header.frame_id = argv[2] pub.publish(final) rate.sleep()
def main(): rospy.init_node('publish_saved_cloud') wait_for_time() argv = rospy.myargv() if len(argv) < 2: print 'Publishes a saved point cloud to a latched topic.' print 'Usage: rosrun applications publish_saved_cloud.py ~/cloud.bag' return path = argv[1] camera = perception.MockCamera() cloud = camera.read_cloud(path) if cloud is None: rospy.logerr('Could not load point cloud from {}'.format(path)) return print( "RUNNING NOW! Remember to load this point cloud with 'cam_image_topic:=mock_point_cloud'" ) pub = rospy.Publisher('mock_point_cloud', PointCloud2, queue_size=1) rate = rospy.Rate(2) while not rospy.is_shutdown(): cloud.header.stamp = rospy.Time.now() pub.publish(cloud) rate.sleep()
def main(): rospy.init_node('publish_saved_cloud') wait_for_time() argv = rospy.myargv() if len(argv) < 2: print 'Publishes a saved point cloud to a latched topic.' print 'Usage: rosrun applications publish_saved_cloud.py ~/cloud.bag' return path = argv[1] camera = perception.MockCamera() cloud = camera.read_cloud(path) if cloud is None: rospy.logerr('Could not load point cloud from {}'.format(path)) return pub = rospy.Publisher('mock_point_cloud', PointCloud2, latch=True, queue_size=1) rate = rospy.Rate(1) while not rospy.is_shutdown(): cloud.header.stamp = rospy.Time.now() # Publish frame id at shelf so that even if robot moves, the point cloud would # not move along with it. Publish transform from map to shelf cloud.header.frame_id = 'shelf' pub.publish(cloud) rate.sleep()
def main(): rospy.init_node('publish_saved_cloud') wait_for_time() argv = rospy.myargv() if len(argv) < 2: is_sim = False else: is_sim = True path = argv[1] camera = perception.MockCamera() # rospy.sleep(0.5) pub = rospy.Publisher('mock_point_cloud', PointCloud2, queue_size=1) rate = rospy.Rate(2) while not rospy.is_shutdown(): if is_sim: cloud = camera.read_cloud(path) else: cloud = rospy.wait_for_message( 'head_camera/depth_registered/points', PointCloud2) if cloud is None: rospy.logerr('Could not load point cloud from {}'.format(path)) exit(1) cloud.header.stamp = rospy.Time.now() pub.publish(cloud) rate.sleep()