LIDAR and Stereo Camera Calibration. Remap input topics

edit

Hi All

I have Quanergy LIDAR and PointGrey Grasshoppers RGB cameras that are connected as Master Slave and work as stereo Camera. I want to do Sensor Fusion of LIDAR and Cemeras and for that need to calibrate the LIDAR and Cameras. I have python code but is not working. I took the code from link text and modified to work for my sensors. So This package assumes that the bag file has at least the following topic names and message types by default, these can be modified in the launch scripts.

/sensors/camera/camera_info (sensor_msgs/CameraInfo)
/sensors/camera/image_color (sensor_msgs/Image)
/sensors/velodyne_points    (sensor_msgs/PointCloud2)

In my case these are

/stereo/left/camera_info
/stereo/left/image_rect_color
/Sensor/points

Here are the scripts

'''
The main ROS node which handles the topics
Inputs:
    camera_info - [str] - ROS sensor camera info topic
    image_color - [str] - ROS sensor image topic
    velodyne - [str] - ROS velodyne PCL2 topic
    camera_lidar - [str] - ROS projected points image topic

Outputs: None
'''
def listener(camera_info, image_raw, points, camera_lidar=None):
    # Start node
    rospy.init_node('calibrate_camera_lidar', anonymous=True)
    rospy.loginfo('Current PID: [%d]' % os.getpid())
    rospy.loginfo('Projection mode: %s' % PROJECT_MODE)
    rospy.loginfo('CameraInfo topic: %s' % camera_info)
    rospy.loginfo('Image topic: %s' % image_raw)
    rospy.loginfo('PointCloud2 topic: %s' % points)
    rospy.loginfo('Output topic: %s' % camera_lidar)

    # Subscribe to topics
    info_sub = message_filters.Subscriber(camera_info, CameraInfo)
    image_sub = message_filters.Subscriber(image_raw, Image)
    Sensor_sub = message_filters.Subscriber(points, PointCloud2)

    # Publish output topic
    image_pub = None
    if camera_lidar: image_pub = rospy.Publisher(camera_lidar, Image, queue_size=5)

    # Synchronize the topics by time
    ats = message_filters.ApproximateTimeSynchronizer(
        [image_sub, info_sub, Sensor_sub], queue_size=5, slop=0.1)
    ats.registerCallback(callback, image_pub)

    # Keep python from exiting until this node is stopped
    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo('Shutting down')


if __name__ == '__main__':

    # Calibration mode, rosrun
    if sys.argv[1] == '--calibrate':
        camera_info = '/stereo/left/camera_info'
        image_raw = '/stereo/left/image_raw'
        points = '/Sensor/points'
        camera_lidar = None
        PROJECT_MODE = False
    # Projection mode, run from launch file
    else:
        camera_info = rospy.get_param('camera_info_topic')
        image_raw = rospy.get_param('image_color_topic')
        points = rospy.get_param('points_topic')
        camera_lidar = rospy.get_param('camera_lidar_topic')
        PROJECT_MODE = bool(rospy.get_param('project_mode'))

    # Start keyboard handler thread
    if not PROJECT_MODE: start_keyboard_handler()

    # Start subscriber
    listener(camera_info, image_raw, points, camera_lidar)

and display lidar camera calibration

   ?xml version="1.0" encoding="UTF-8"?>
<launch>
    <arg name="camera" default="/stereo/left" />
    <!-- Play rosbag record -->
    <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
        <arg name="bagfile" value="2019-10-24-16-56-30.bag" />
    </include>
       <!-- Nodelet manager for this pipeline -->
    <node
        pkg="nodelet"
        type="nodelet"
        args="manager"
        name="lidar_camera_manager"
        output="screen" />

    <node
        pkg="image_proc"
        type="image_proc" 
        name="image_proc_node1" />

    <!-- Run image_proc/rectify nodelet -->
    <node
        pkg="nodelet"
        type="nodelet"
        name="rectify_color"
        args="load image_proc/rectify lidar_camera_manager --no-bond" >

        <!-- Remap input topics -->
        <remap from="image_mono"  to="$(arg camera)/image_rect_color" />
        <remap from="camera_info" to="$(arg camera)/camera_info" />

        <!-- Remap output topics -->
        <remap from="image_rect" to="$(arg camera)/image_rect" />
    </node>

    <!-- Wire static transform from the world to velodyne frame -->
    <node
        pkg="tf2_ros"
        type="static_transform_publisher"
        name="world_velodyne_tf"
        output="screen"
        args="-0.0 -0 ...