def publish(self, cloud, cloud_format="open3d"):
if cloud_format == "open3d":
cloud = convertCloudFromOpen3dToRos(cloud)
else: # ROS cloud
None
self.pub.publish(cloud)
self.cnt += 1
def updateCloud(self, new_cloud):
try:
new_cloud = mergeClouds(new_cloud, self.cloud_XYZaxis)
new_cloud = resizeCloudXYZ(
new_cloud, 5.0) # Resize cloud, so rviz has better view
self.pub.publish(convertCloudFromOpen3dToRos(new_cloud))
except:
print "Node 3 fails to update cloud, due to the input is empty!\n"
def pub_cloud(self):
filename = self.getFileName()
open3d_cloud = open3d.read_point_cloud(filename)
print "Node 1: sim: load cloud file:\n " + filename
print " points = " + str(getCloudSize(open3d_cloud))
ros_cloud = convertCloudFromOpen3dToRos(open3d_cloud, frame_id="base")
self.pub.publish(ros_cloud)
print("Node 1: sim: publishing cloud " + str(ith_goalpose) + "to: " +
self.topic_name_rgbd_cloud)
# Main
if __name__ == "__main__":
# Params setting
node_name = "read_cloud_and_pub_by_open3d"
topic_name_rgbd_cloud = rospy.get_param("/topic_name_rgbd_cloud",
"camera/depth_registered/points")
file_folder = rospy.get_param("file_folder")
file_name = rospy.get_param("file_name")
filename = file_folder + file_name
# Set node
rospy.init_node(node_name, anonymous=True)
# Read file
open3d_cloud = open3d.read_point_cloud(filename)
rospy.loginfo("Loading cloud file from: \n " + filename)
print(open3d_cloud)
ros_cloud = convertCloudFromOpen3dToRos(open3d_cloud)
# Publish
pub = rospy.Publisher(topic_name_rgbd_cloud, PointCloud2, queue_size=10)
cnt = 0
while not rospy.is_shutdown():
rospy.sleep(1)
pub.publish(ros_cloud)
rospy.loginfo("Publish {:d}th cloud...\n".format(cnt))
cnt += 1
rospy.loginfo("this node stops: " + node_name)
def publish(self, cloud, cloud_format="open3d", frame_id="head_camera"):
if cloud_format == "open3d":
cloud = convertCloudFromOpen3dToRos(cloud, frame_id)
else: # ROS cloud: Do nothing.
pass
self._pub.publish(cloud)