Published CameraInfo msgs is empty
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(®istration::callback,®,_1,_2,_3));
while(ros::ok())
{
camInt.header.stamp= ros::Time::now();
cameraIntrinsic.publish(camInt);
ros::spin();
//ros::Duration(1).sleep();
}
}
return 0;
}
Asked by AR13 on 2019-11-01 10:08:02 UTC
Comments