int main(int argc, char* argv[])
{
ros::init(argc, argv, "calibration_publisher");
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
if (!private_nh.getParam("register_lidar2camera_tf", isRegister_tf)) {
isRegister_tf = true;
}
if (!private_nh.getParam("publish_extrinsic_mat", isPublish_extrinsic)) {
isPublish_extrinsic = true; /* publish extrinsic_mat in default */
}
if (!private_nh.getParam("publish_camera_info", isPublish_cameraInfo)) {
isPublish_extrinsic = false; /* doesn't publish camera_info in default */
}
if (argc < 2)
{
std::cout << "Usage: calibration_publisher <calibration-file>." << std::endl;
return -1;
}
cv::FileStorage fs(argv[1], cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "Cannot open " << argv[1] << std::endl;
return -1;
}
fs["CameraExtrinsicMat"] >> CameraExtrinsicMat;
fs["CameraMat"] >> CameraMat;
fs["DistCoeff"] >> DistCoeff;
fs["ImageSize"] >> ImageSize;
ros::Subscriber image_sub;
if (isRegister_tf) {
image_sub = n.subscribe("/image_raw", 10, image_raw_cb);
}
if (isPublish_cameraInfo) {
camera_info_pub = n.advertise<sensor_msgs::CameraInfo>("/camera/camera_info", 10, true);
cameraInfo_sender(CameraMat, DistCoeff, ImageSize);
}
if (isPublish_extrinsic) {
projection_matrix_pub = n.advertise<calibration_camera_lidar::projection_matrix>("/projection_matrix", 10, true);
projectionMatrix_sender(CameraExtrinsicMat);
}
ros::spin();
return 0;
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "calibration_publisher");
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
if (!private_nh.getParam("register_lidar2camera_tf", isRegister_tf)) {
isRegister_tf = true;
}
if (!private_nh.getParam("publish_extrinsic_mat", isPublish_extrinsic)) {
isPublish_extrinsic = true; /* publish extrinsic_mat in default */
}
if (!private_nh.getParam("publish_camera_info", isPublish_cameraInfo)) {
isPublish_extrinsic = false; /* doesn't publish camera_info in default */
}
if (argc < 2)
{
std::cout << "Usage: calibration_publisher <calibration-file>." << std::endl;
return -1;
}
cv::FileStorage fs(argv[1], cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "Cannot open " << argv[1] << std::endl;
return -1;
}
fs["CameraExtrinsicMat"] >> CameraExtrinsicMat;
fs["CameraMat"] >> CameraMat;
fs["DistCoeff"] >> DistCoeff;
fs["ImageSize"] >> ImageSize;
std::string image_topic_name("/image_raw");
std::string camera_info_name("/camera/camera_info");
std::string projection_matrix_name("/projection_matrix");
std::string name_space_str = ros::this_node::getNamespace();
camera_id_str = "camera";
if (name_space_str != "/") {
image_topic_name = name_space_str + image_topic_name;
camera_info_name = name_space_str + camera_info_name;
projection_matrix_name = name_space_str + projection_matrix_name;
if (name_space_str.substr(0, 2) == "//") {
/* if name space obtained by ros::this::node::getNamespace()
starts with "//", delete one of them */
name_space_str.erase(name_space_str.begin());
}
camera_id_str = name_space_str;
}
ros::Subscriber image_sub;
if (isRegister_tf) {
image_sub = n.subscribe(image_topic_name, 10, image_raw_cb);
}
if (isPublish_cameraInfo) {
camera_info_pub = n.advertise<sensor_msgs::CameraInfo>(camera_info_name, 10, true);
cameraInfo_sender(CameraMat, DistCoeff, ImageSize);
}
if (isPublish_extrinsic) {
projection_matrix_pub = n.advertise<calibration_camera_lidar::projection_matrix>(projection_matrix_name, 10, true);
projectionMatrix_sender(CameraExtrinsicMat);
}
ros::spin();
return 0;
}
