Published CameraInfo msgs is empty

asked 2019-11-01 10:08:02 -0600

AR13 gravatar image

Hi all, I need your help please. In one my my nodes I construct a CameraInfo msgs and publish it. My node was not running suddenly I realized that the /camera/camera_info1 is empty and when I echo it to the terminal for example it says no new messages

Here is some part of the main function which I construct the msgs and publish and subscribe to the topic. could you please help me to see why there is nothing inside. (I am sure that the camInt is not empty)

Many Thanks in advance!

int main(int argc, char** argv)

{ ros::init(argc,argv,"registration"); ros::NodeHandle node; registration reg;

sensor_msgs::CameraInfo camInt;
ros::Publisher cameraIntrinsic = node.advertise<sensor_msgs::CameraInfo>("/camera/camera_info1",1000);

        // reading the camera intrinsic parametrs from local directory
        double  val1;
        std::string line1;

        std::ifstream myfile1;
        myfile1.open("..................../calibration/calibration_resultes/1Fisheye_camera_Calibration_parameters.txt");

        std::vector<std::vector<double> > LIST1;

        if (myfile1.is_open())
        {

            while ( getline(myfile1,line1) )
            {

                std::vector<double> v1;

                std::istringstream IS(line1);

                while( IS >> val1)
                {
                    v1.push_back(val1);
                }

                LIST1.push_back(v1);
            }

            myfile1.close();
        }

        // constructing the camera info message

        camInt.header.stamp = ros::Time::now();
        camInt.K.elems[0] = static_cast<float>(LIST1[1][0]);camInt.K.elems[1] = static_cast<float>(LIST1[1][1]);camInt.K.elems[2] = static_cast<float>(LIST1[1][2]);
        camInt.K.elems[3] = static_cast<float>(LIST1[2][0]);camInt.K.elems[4] = static_cast<float>(LIST1[2][1]);camInt.K.elems[5] = static_cast<float>(LIST1[2][2]);
        camInt.K.elems[6] = static_cast<float>(LIST1[3][0]);camInt.K.elems[7] = static_cast<float>(LIST1[3][1]);camInt.K.elems[8] = static_cast<float>(LIST1[3][2]);
        camInt.D.push_back(static_cast<float>(LIST1[6][0]));camInt.D.push_back(static_cast<float>(LIST1[6][1]));camInt.D.push_back(static_cast<float>(LIST1[6][2]));
        camInt.D.push_back(static_cast<float>(LIST1[6][3]));

        // subscribing to the topics

        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo, sensor_msgs::Image> MySyncPolicy;
        message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudsub(node,"/velodyne_points",1000);
        message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfo(node,"/camera/camera_info1",1000);
        message_filters::Subscriber<sensor_msgs::Image> image_sub(node,"/camera/image_color",1000);

        message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000), pointCloudsub, cameraInfo, image_sub);
        sync.registerCallback(boost::bind(&registration::callback,&reg,_1,_2,_3));



        while(ros::ok())
        {
            camInt.header.stamp= ros::Time::now();
            cameraIntrinsic.publish(camInt);
            ros::spin();
            //ros::Duration(1).sleep();
        }
    }

return 0;

}

edit retag flag offensive close merge delete