#include "utility.hpp"
#include "lio_sam/msg/cloud_info.hpp"
#include "lio_sam/srv/save_map.hpp"
using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
using symbol_shorthand::G;
struct PointXYZIRPYT {
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) (double, time, time))
typedef PointXYZIRPYT PointTypePose;
class mapOptimization : public ParamServer {
public:
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subSaveMapTrigger;
std::thread saveMapThread;
std::atomic<bool> saveMapRequested{false};
std::string requestedSavePath;
float requestedResolution = 0.0f;
mapOptimization(const rclcpp::NodeOptions & options) : ParamServer("lio_sam_mapOptimization", options) {
subSaveMapTrigger = create_subscription<std_msgs::msg::String>(
"lio_sam/save_map_trigger", 10, std::bind(&mapOptimization::saveMapTriggerHandler, this, std::placeholders::_1));
auto saveMapService = [this](const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<lio_sam::srv::SaveMap::Request> req, std::shared_ptr<lio_sam::srv::SaveMap::Response> res) -> void {
string saveMapDirectory;
cout << "****************************************************" << endl;
cout << "Saving map to pcd files ..." << endl;
if(req->destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
else saveMapDirectory = std::getenv("HOME") + req->destination;
cout << "Save destination: " << saveMapDirectory << endl;
int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());
unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);
pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
}
if(req->resolution != 0) {
downSizeFilterCorner.setInputCloud(globalCornerCloud);
downSizeFilterCorner.setLeafSize(req->resolution, req->resolution, req->resolution);
downSizeFilterCorner.filter(*globalCornerCloudDS);
pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);
downSizeFilterSurf.setInputCloud(globalSurfCloud);
downSizeFilterSurf.setLeafSize(req->resolution, req->resolution, req->resolution);
downSizeFilterSurf.filter(*globalSurfCloudDS);
pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);
} else {
pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
}
*globalMapCloud += *globalCornerCloud;
*globalMapCloud += *globalSurfCloud;
int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
res->success = ret == 0;
cout << "****************************************************" << endl;
cout << "Saving map to pcd files completed\n" << endl;
return;
};
srvSaveMap = create_service<lio_sam::srv::SaveMap>("lio_sam/save_map", saveMapService);
}
void saveMapInThread(const std::string& directory, float resolution) {
cout << "****************************************************" << endl;
cout << "Saving map to pcd files in thread..." << endl;
cout << "Save destination: " << directory << endl;
int unused = system((std::string("exec rm -r ") + directory).c_str());
unused = system((std::string("mkdir -p ") + directory).c_str());
pcl::io::savePCDFileBinary(directory + "/trajectory.pcd", *cloudKeyPoses3D);
pcl::io::savePCDFileBinary(directory + "/transformations.pcd", *cloudKeyPoses6D);
pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
}
if(resolution != 0) {
downSizeFilterCorner.setInputCloud(globalCornerCloud);
downSizeFilterCorner.setLeafSize(resolution, resolution, resolution);
downSizeFilterCorner.filter(*globalCornerCloudDS);
pcl::io::savePCDFileBinary(directory + "/CornerMap.pcd", *globalCornerCloudDS);
downSizeFilterSurf.setInputCloud(globalSurfCloud);
downSizeFilterSurf.setLeafSize(resolution, resolution, resolution);
downSizeFilterSurf.filter(*globalSurfCloudDS);
pcl::io::savePCDFileBinary(directory + "/SurfMap.pcd", *globalSurfCloudDS);
} else {
pcl::io::savePCDFileBinary(directory + "/CornerMap.pcd", *globalCornerCloud);
pcl::io::savePCDFileBinary(directory + "/SurfMap.pcd", *globalSurfCloud);
}
*globalMapCloud += *globalCornerCloud;
*globalMapCloud += *globalSurfCloud;
pcl::io::savePCDFileBinary(directory + "/GlobalMap.pcd", *globalMapCloud);
cout << "****************************************************" << endl;
cout << "Saving map to pcd files completed\n" << endl;
}
void saveMapTriggerHandler(const std_msgs::msg::String::SharedPtr msg) {
RCLCPP_INFO(get_logger(), "Received save map trigger: %s", msg->data.c_str());
std::string data = msg->data;
std::string path;
float resolution = 0.0f;
size_t colon_pos = data.find(':');
if (colon_pos != std::string::npos) {
path = data.substr(0, colon_pos);
try { resolution = std::stof(data.substr(colon_pos + 1)); }
catch (const std::exception& e) { RCLCPP_WARN(get_logger(), "Invalid resolution format, using default 0.0"); }
} else { path = data; }
if (path.empty()) { path = savePCDDirectory; }
const char* home_dir = std::getenv("HOME");
if (!home_dir) { RCLCPP_ERROR(get_logger(), "HOME environment variable not set!"); return; }
std::string full_path = std::string(home_dir) + path;
RCLCPP_INFO(get_logger(), "Starting save map thread for path: %s", full_path.c_str());
if (saveMapThread.joinable()) { saveMapThread.join(); }
saveMapThread = std::thread(&mapOptimization::saveMapInThread, this, full_path, resolution);
RCLCPP_INFO(get_logger(), "Save map thread started");
}
};
