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()
