Can I get PointCloud2 data from reprojection.py?

[gokcesena]

Can I produce pointcloud2 data and publish it from the steps taken in the reprojection.py file, especially in the def callback function?

If I understand correctly, pointcloud data is already generated here:
"cloud = lp.projectLaser(scan)
points = pc2.read_points(cloud)" here is PointCloud or PointCloud2?

When I try to publish this "cloud" data myself, I get the following error: "
``[ERROR] [1705042997.159589]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7fa8a914a400>>
Traceback (most recent call last):
File "/home/gokce/lidar_camera/src/ros_comm/clients/rospy/src/rospy/topics.py", line 750, in _invoke_callback
cb(msg)
File "", line 76, in callback
File "", line 58, in signalMessage
File "", line 330, in add
File "", line 58, in signalMessage
File "/home/gokce/lidar_camera/src/camera_2d_lidar_calibration/src/reprojection.py", line 230, in callback
pub.publish(cloud) # PointCloud2 mesajını yayınla
File "/home/gokce/lidar_camera/src/ros_comm/clients/rospy/src/rospy/topics.py", line 879, in publish
data = args_kwds_to_message(self.data_class, args, kwds)
File "/home/gokce/lidar_camera/src/ros_comm/clients/rospy/src/rospy/msg.py", line 121, in args_kwds_to_message
raise TypeError("expected [%s] but got [%s]"%(data_class._slot_types[0], arg._type))
TypeError: expected [std_msgs/Header] but got [sensor_msgs/PointCloud2]"

Can you guide me for this?

[ehong-tl]

hi @gokcesena

TypeError: expected [std_msgs/Header] but got [sensor_msgs/PointCloud2]"

It seems like you have defined the wrong message type at the Publisher initialization..

[gokcesena]

Thank you for your quick reply @ehong-tl
I added the following lines: "pub.publish(cloud)" and "pub = rospy.Publisher("/reprojection_points", PointCloud2, queue_size=10)" Can you advise?

[ehong-tl]

You've got the error after adding these 2 lines?

Try to use other name to prevent clashing as "pub" has already been used for the Image publisher.

[gokcesena]

Thank you @ehong-tl , this is the problem.
Is "cloud" pointcloud data or pointcloud2 data?

[ehong-tl]

it should be pointcloud2.

it's best if you can post here your modified code

[gokcesena]

This is the part where I changed:
`import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from pyquaternion import Quaternion
import yaml
import numpy as np
import message_filters
from sensor_msgs.msg import Image, LaserScan, PointCloud2, PointCloud, PointField
import laser_geometry.laser_geometry as lg
import sensor_msgs.point_cloud2 as pc2
import std_msgs.msg

pub = rospy.Publisher("/reprojection_points", PointCloud2, queue_size=10)

def get_z(T_cam_world, T_world_pc, K):
R = T_cam_world[:3,:3]
t = T_cam_world[:3,3]
proj_mat = np.dot(K, np.hstack((R, t[:,np.newaxis])))
xyz_hom = np.hstack((T_world_pc, np.ones((T_world_pc.shape[0], 1))))
xy_hom = np.dot(proj_mat, xyz_hom.T).T
z = xy_hom[:, -1]
z = np.asarray(z).squeeze()
return z

def extract(point):
return [point[0], point[1], point[2]]

def callback(scan, image):
rospy.loginfo("image timestamp: %d ns" % image.header.stamp.to_nsec())
rospy.loginfo("scan timestamp: %d ns" % scan.header.stamp.to_nsec())
diff = abs(image.header.stamp.to_nsec() - scan.header.stamp.to_nsec())
rospy.loginfo("diff: %d ns" % diff)
img = bridge.imgmsg_to_cv2(image)
cloud = lp.projectLaser(scan)
points = pc2.read_points(cloud)
#print(cloud)
objPoints = np.array(list(map(extract, points)))
Z = get_z(q, objPoints, K)
objPoints = objPoints[Z > 0]
if lens == 'pinhole':
img_points, _ = cv2.projectPoints(objPoints, rvec, tvec, K, D)
elif lens == 'fisheye':
objPoints = np.reshape(objPoints, (1,objPoints.shape[0],objPoints.shape[1]))
img_points, _ = cv2.fisheye.projectPoints(objPoints, rvec, tvec, K, D)
img_points = np.squeeze(img_points)

for i in range(len(img_points)):
    try:
        # pixel_data.append({'pixel_coordinates': ((round(img_points[i][0])), (round(img_points[i][1]))), 'z_value': Z[i]}) # lazer noktasinin piksel koordinatlarını ve z değerlerini saklamak için
        # print((int(round(img_points[i][0])),int(round(img_points[i][1]))))
        cv2.circle(img, (np.int32(round(img_points[i][0])),np.int32(round(img_points[i][1]))), laser_point_radius, (0,255,0), 1)
    except OverflowError:
        continue

pub.publish(cloud) # Publish PointCloud2 
#pub.publish(bridge.cv2_to_imgmsg(img))`

[ehong-tl]

Can you try this?

#!/usr/bin/env python

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from pyquaternion import Quaternion
import yaml
import numpy as np
import message_filters
from sensor_msgs.msg import Image, LaserScan, PointCloud2
import laser_geometry.laser_geometry as lg
import sensor_msgs.point_cloud2 as pc2

def get_z(T_cam_world, T_world_pc, K):
    R = T_cam_world[:3,:3]
    t = T_cam_world[:3,3]
    proj_mat = np.dot(K, np.hstack((R, t[:,np.newaxis])))
    xyz_hom = np.hstack((T_world_pc, np.ones((T_world_pc.shape[0], 1))))
    xy_hom = np.dot(proj_mat, xyz_hom.T).T
    z = xy_hom[:, -1]
    z = np.asarray(z).squeeze()
    return z

def extract(point):
    return [point[0], point[1], point[2]]

def callback(scan, image):
    rospy.loginfo("image timestamp: %d ns" % image.header.stamp.to_nsec())
    rospy.loginfo("scan timestamp: %d ns" % scan.header.stamp.to_nsec())
    diff = abs(image.header.stamp.to_nsec() - scan.header.stamp.to_nsec())
    rospy.loginfo("diff: %d ns" % diff)
    img = bridge.imgmsg_to_cv2(image)
    cloud = lp.projectLaser(scan)
    pub_pc2.publish(cloud)
    points = pc2.read_points(cloud)
    objPoints = np.array(map(extract, points))
    Z = get_z(q, objPoints, K)
    objPoints = objPoints[Z > 0]
    if lens == 'pinhole':
        img_points, _ = cv2.projectPoints(objPoints, rvec, tvec, K, D)
    elif lens == 'fisheye':
        objPoints = np.reshape(objPoints, (1,objPoints.shape[0],objPoints.shape[1]))
        img_points, _ = cv2.fisheye.projectPoints(objPoints, rvec, tvec, K, D)
    img_points = np.squeeze(img_points)
    for i in range(len(img_points)):
        try:
            cv2.circle(img, (int(round(img_points[i][0])),int(round(img_points[i][1]))), laser_point_radius, (0,255,0), 1)
        except OverflowError:
            continue
    pub.publish(bridge.cv2_to_imgmsg(img))

rospy.init_node('reprojection')
scan_topic = rospy.get_param("~scan_topic")
image_topic = rospy.get_param("~image_topic")
calib_file = rospy.get_param("~calib_file")
config_file = rospy.get_param("~config_file")
laser_point_radius = rospy.get_param("~laser_point_radius")
time_diff = rospy.get_param("~time_diff")
bridge = CvBridge()
lp = lg.LaserProjection()

with open(calib_file, 'r') as f:
    data = f.read().split()
    qx = float(data[0])
    qy = float(data[1])
    qz = float(data[2])
    qw = float(data[3])
    tx = float(data[4])
    ty = float(data[5])
    tz = float(data[6])
q = Quaternion(qw,qx,qy,qz).transformation_matrix
q[0,3] = tx
q[1,3] = ty
q[2,3] = tz
print("Extrinsic parameter - camera to laser")
print(q)
tvec = q[:3,3]
rot_mat = q[:3,:3]
rvec, _ = cv2.Rodrigues(rot_mat)

with open(config_file, 'r') as f:
    f.readline()
    config = yaml.load(f)
    lens = config['lens']
    fx = float(config['fx'])
    fy = float(config['fy'])
    cx = float(config['cx'])
    cy = float(config['cy'])
    k1 = float(config['k1'])
    k2 = float(config['k2'])
    p1 = float(config['p1/k3'])
    p2 = float(config['p2/k4'])  
K = np.matrix([[fx, 0.0, cx],
               [0.0, fy, cy],
               [0.0, 0.0, 1.0]])
D = np.array([k1, k2, p1, p2])
print("Camera parameters")
print("Lens = %s" % lens)
print("K =")
print(K)
print("D =")
print(D)

pub = rospy.Publisher("/reprojection", Image, queue_size=1)
pub_pc2 = rospy.Publisher("/reprojection_points", PointCloud2, queue_size=10)
scan_sub = message_filters.Subscriber(scan_topic, LaserScan, queue_size=1)
image_sub = message_filters.Subscriber(image_topic, Image, queue_size=1)
ts = message_filters.ApproximateTimeSynchronizer([scan_sub, image_sub], 10, time_diff)
ts.registerCallback(callback)

rospy.spin()

[gokcesena]

I got this error: "[ERROR] [1705051676.044145]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7fb84a0834c0>>
Traceback (most recent call last):
File "/home/gokce/lidar_camera/src/ros_comm/clients/rospy/src/rospy/topics.py", line 750, in _invoke_callback
cb(msg)
File "", line 76, in callback
File "", line 58, in signalMessage
File "", line 330, in add
File "", line 58, in signalMessage
File "/home/gokce/lidar_camera/src/camera_2d_lidar_calibration/src/reprojection.py", line 160, in callback
Z = get_z(q, objPoints, K)
File "/home/gokce/lidar_camera/src/camera_2d_lidar_calibration/src/reprojection.py", line 141, in get_z
xyz_hom = np.hstack((T_world_pc, np.ones((T_world_pc.shape[0], 1))))
IndexError: tuple index out of range
"

[ehong-tl]

I believe this is the problem from porting Python 2 code to Python 3.

Try this fix,

#3 (comment)