Browse Source

Fixed comments and included rospy loginfo.

noetic
hello-sanchez 2 years ago
parent
commit
536b931d4e
1 changed files with 17 additions and 16 deletions
  1. +17
    -16
      src/pointcloud_transformer.py

+ 17
- 16
src/pointcloud_transformer.py View File

@ -10,11 +10,11 @@ from sensor_msgs.msg import PointCloud2, PointCloud
from geometry_msgs.msg import Point32 from geometry_msgs.msg import Point32
from std_msgs.msg import Header from std_msgs.msg import Header
class PointCloudTransformer: class PointCloudTransformer:
""" """
A class that takes in a PointCloud2 message and transfers it into a
pointcloud message. Then that pointcloud is
A class that takes in a PointCloud2 message and stores its points into a
PointCloud message. Then that PointCloud is transformed to reference the
'base_link' frame.
""" """
def __init__(self): def __init__(self):
""" """
@ -27,26 +27,28 @@ class PointCloudTransformer:
# Initialize PointCloud Publisher # Initialize PointCloud Publisher
self.pointcloud_pub = rospy.Publisher("/camera_cloud", PointCloud, queue_size=1) self.pointcloud_pub = rospy.Publisher("/camera_cloud", PointCloud, queue_size=1)
# Initialize self.cloud for data storage in pointcloud_data callback function
# Initialize self.pcl2_cloud to store the PointCloud2
self.pcl2_cloud = None self.pcl2_cloud = None
# Intialize tf.Transformlister
# Intialize tf.Transformlistener
self.listener = tf.TransformListener(True, rospy.Duration(10.0)) self.listener = tf.TransformListener(True, rospy.Duration(10.0))
# Create rospy log message
rospy.loginfo('Publishing transformed PointCloud. Use RViz to visualize')
def callback_pcl2(self,msg): def callback_pcl2(self,msg):
""" """
Callback function that stores the pointcloud2 message.
Callback function that stores the PointCloud2 message.
:param self: The self reference. :param self: The self reference.
:param msg: The Pointcloud2 message type.
:param msg: The PointCloud2 message type.
""" """
self.pcl2_cloud = msg self.pcl2_cloud = msg
def pcl_transformer(self): def pcl_transformer(self):
""" """
A function that extracts the points from the stored PointCloud2 message A function that extracts the points from the stored PointCloud2 message
and appends those points to a PointCloud message. Then transforms the
PointCloud from its original header frame id, 'camera_color_optical_frame'
and appends those points to a PointCloud message. Then the function transforms
the PointCloud from its the header frame id, 'camera_color_optical_frame'
to the 'base_link' frame. to the 'base_link' frame.
:param self: The self reference. :param self: The self reference.
""" """
@ -55,8 +57,7 @@ class PointCloudTransformer:
temp_cloud = PointCloud() temp_cloud = PointCloud()
temp_cloud.header = self.pcl2_cloud.header temp_cloud.header = self.pcl2_cloud.header
# For loop to extract pointcloud2 data into a list of x,y,z, and
# For loop to extract PointCloud2 data into a list of x,y,z, points and
# append those values to the PointCloud message, temp_cloud # append those values to the PointCloud message, temp_cloud
for data in pc2.read_points(self.pcl2_cloud, skip_nans=True): for data in pc2.read_points(self.pcl2_cloud, skip_nans=True):
temp_cloud.points.append(Point32(data[0],data[1],data[2])) temp_cloud.points.append(Point32(data[0],data[1],data[2]))
@ -69,7 +70,7 @@ class PointCloudTransformer:
def transform_pointcloud(self,msg): def transform_pointcloud(self,msg):
""" """
Function that stores the pointcloud2 message.
Function that stores the PointCloud2 message.
:param self: The self reference. :param self: The self reference.
:param msg: The PointCloud message. :param msg: The PointCloud message.
@ -90,8 +91,8 @@ if __name__=="__main__":
# Initialize the node, and call it "pointcloud_transformer" # Initialize the node, and call it "pointcloud_transformer"
rospy.init_node('pointcloud_transformer',anonymous=True) rospy.init_node('pointcloud_transformer',anonymous=True)
# Instantiate the PointCloudTransformer class
obj = PointCloudTransformer()
# Declare object, PCT, from the PointCloudTransformer class.
PCT = PointCloudTransformer()
# We're going to publish information at 1 Hz # We're going to publish information at 1 Hz
rate = rospy.Rate(1) rate = rospy.Rate(1)
@ -103,5 +104,5 @@ if __name__=="__main__":
while not rospy.is_shutdown(): while not rospy.is_shutdown():
# Run the pcl_transformer method # Run the pcl_transformer method
obj.pcl_transformer()
PCT.pcl_transformer()
rate.sleep() rate.sleep()

Loading…
Cancel
Save