run on docker ok

src/localization_module_lio_locus/CMakeLists.txt

@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
+cmake_policy(SET CMP0074 NEW)
 project(lio_locus_humble)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -25,35 +26,20 @@ find_package(yaml-cpp 0.6 REQUIRED)
 find_package(OpenMP)
 find_package(Eigen3)
 
-get_filename_component(WORKSPACE_ROOT_ABS "${CMAKE_CURRENT_SOURCE_DIR}/../../" ABSOLUTE)
-set(WORKSPACE_ROOT "${WORKSPACE_ROOT_ABS}/")
 
-set(CMAKE_PREFIX_PATH "${WORKSPACE_ROOT}deps_install" ${CMAKE_PREFIX_PATH})
-set(CMAKE_INCLUDE_PATH "${WORKSPACE_ROOT}deps_install/include" ${CMAKE_INCLUDE_PATH})
-set(CMAKE_LIBRARY_PATH "${WORKSPACE_ROOT}deps_install/lib" ${CMAKE_LIBRARY_PATH})
-set(PCL_DIR "${WORKSPACE_ROOT}deps_install/share/pcl-1.10")
+# set(CMAKE_PREFIX_PATH "/home/slam10/hivecore_robot_slam/deps_install" ${CMAKE_PREFIX_PATH})
+# set(CMAKE_INCLUDE_PATH "/home/slam10/hivecore_robot_slam/deps_install/include" ${CMAKE_INCLUDE_PATH})
+# set(CMAKE_LIBRARY_PATH "/home/slam10/hivecore_robot_slam/deps_install/lib" ${CMAKE_LIBRARY_PATH})
 
-set(PCL_ROOT "${WORKSPACE_ROOT}deps_install")
+# set(PCL_DIR /home/slam10/hivecore_robot_slam/deps_install/share/pcl-1.10)
+
+# set(PCL_ROOT /home/slam10/hivecore_robot_slam/deps_install)
 
 find_package(PCL 1.10 REQUIRED)
 
-set(PCL_LIBRARIES_ABS "")
-foreach(lib ${PCL_LIBRARIES})
-  string(STRIP "${lib}" lib)
-  if("${lib}" MATCHES "^pcl_")
-    set(lib_path "${WORKSPACE_ROOT}deps_install/lib/lib${lib}.so")
-    if(EXISTS "${lib_path}")
-      list(APPEND PCL_LIBRARIES_ABS "${lib_path}")
-    endif()
-  else()
-    list(APPEND PCL_LIBRARIES_ABS "${lib}")
-  endif()
-endforeach()
-set(PCL_LIBRARIES ${PCL_LIBRARIES_ABS})
-
 message("##################" ${PCL_INCLUDE_DIRS})
 message("##################" ${PCL_LIBRARIES})
-message("---------------------------"${PCL_INCLUDE_DIRS})
+message("---------------------------" ${PCL_INCLUDE_DIRS})
 
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
@@ -61,33 +47,21 @@ if (OPENMP_FOUND)
     set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
 endif()
 
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags")
-set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--disable-new-dtags")
-set(CMAKE_SKIP_BUILD_RPATH  FALSE)
-set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
-set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib;${WORKSPACE_ROOT}deps_install/lib")
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
 include_directories(
-    ${PCL_INCLUDE_DIRS}
-    ${WORKSPACE_ROOT}deps_install/include/pcl-1.10
     include/${PROJECT_NAME}
     ${pcl_conversions_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
     ${YAML_CPP_INCLUDE_DIR}
     /user/include
+    # /home/slam10/hivecore_robot_slam/deps_install/include/pcl-1.10
     )
 
-link_directories(
-    ${WORKSPACE_ROOT}deps_install/lib
-)
+# link_directories(
+#         /home/slam10/hivecore_robot_slam/deps_install/lib
+# )
+
 
-# Install PCL libraries to the package's lib directory
-install(
-    DIRECTORY ${WORKSPACE_ROOT}deps_install/lib/
-    DESTINATION lib
-    FILES_MATCHING PATTERN "libpcl_*.so*"
-)
 
 add_subdirectory(src)
 add_subdirectory(apps/nodelets)

src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt

@@ -7,7 +7,6 @@ target_link_libraries(lio_locus_node
 	point_cloud_localization
 	point_cloud_mapper
 	boost_filesystem
-    ${PCL_LIBRARIES}
 )
 
 target_include_directories(lio_locus_node PUBLIC
@@ -25,7 +24,7 @@ ament_target_dependencies(
   "pcl_conversions"
   "std_srvs"
   "tf2"
-  #"tf2_ros"
+  "tf2_ros"
 )
 
 # install exe to folder within lib;
@@ -74,7 +73,6 @@ add_executable(cloud_preprocessing_node cloud_preprocessing_node.cpp)
 target_link_libraries(cloud_preprocessing_node 
 	cloud_pre_processor
 	boost_filesystem
-    ${PCL_LIBRARIES}
 )
 
 #target_include_directories(cloud_preprocessing_node PUBLIC

src/localization_module_lio_locus/apps/nodelets/cloud_preprocessing_node.cpp

@@ -85,6 +85,7 @@ void callbackLidarRaw(const sensor_msgs::msg::PointCloud2 &msg)
 	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_raw(
 		new pcl::PointCloud<pcl::PointXYZINormal>());
 
+	params_cloud_preproc.topic_cloud_raw = "/livox/lidar";
 	if(params_cloud_preproc.topic_cloud_raw == "/rsm1_1")
 	{
 		cloud_raw = CloudPreProcessor::adaptCloudForm(msg);

src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp

@@ -30,6 +30,7 @@ LioLocus::Ptr ptr_lio_locus;
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 using std::string;
 
@@ -48,6 +49,7 @@ rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_pose_array;
 
 rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_pose;
 rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr dt_publisher_;
+rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr pub_reliability;
 void publishRobotPose();
 void publishLocalizedCloud();
 void publishCloudMap(rclcpp::Time ts);
@@ -79,6 +81,8 @@ void serviceInvoking(const std::shared_ptr<std_srvs::srv::Empty::Request> reques
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "################## service invoked: !!! ");
 }
 
+std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
 int main(int argc, char **argv)
 {
 
@@ -93,6 +97,9 @@ int main(int argc, char **argv)
 
     node = std::make_shared<rclcpp::Node>("lio_locus_node");
 
+    tf_broadcaster_ =
+      std::make_unique<tf2_ros::TransformBroadcaster>(*node);
+
     // algorithm initialization stuff;
     flag_init_pose_set = false;
 
@@ -126,7 +133,7 @@ int main(int argc, char **argv)
     // when use prior map,
     if (params_lio_locus.flag_use_prior_map)
     {
-        ptr_lio_locus->uploadPriorCloudMap(params_lio_locus.fn_cloud_map, params_lio_locus.leaf_size_load_map);
+        ptr_lio_locus->uploadPriorCloudMap(params_lio_locus.fn_cloud_map, params_lio_locus.leaf_size_cloud_map);
 
         // set initial pose;
         geometry_utils::Transform3 pose_prior = geometry_utils::Transform3::Identity();
@@ -163,6 +170,7 @@ int main(int argc, char **argv)
         node->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_map", 9);
 
     dt_publisher_ = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/dt_lio_locus", 9);
+    pub_reliability = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/reliablity_loc", 9);
 
     // independent threads for data processing, incrementals publication, map publication;
     // rclcpp::TimerBase::SharedPtr p_process_data = node->create_wall_timer(std::chrono::milliseconds(100), callbackProcessData);
@@ -304,40 +312,6 @@ void publishCloudMap(rclcpp::Time ts)
     if(!ptr_cloud_map->size())
         return ;
 
-    // check: normal vector information ;
-
-
-//     if(flag_invoke_saving_map)
-// {
-//     pcl::io::savePCDFileASCII(params_lio_locus.path_save_map, *ptr_cloud_map);
-//     flag_invoke_saving_map = false;
-// }
-
-    // // for first time and for time when size doubles;
-    // static bool flag_recorded = false;
-    // static int size_cloud_map;
-    // int size_now = ptr_cloud_map->size();
-
-    // bool flag_publish = false;
-    // if (!flag_recorded)
-    // {
-    //     flag_publish = true;
-    //     size_cloud_map = size_now;
-    // }
-    // else
-    // {
-    //     if (size_now > 1.6 * size_cloud_map)
-    //     {
-    //         size_cloud_map = size_now;
-    //         flag_publish = true;
-    //     }
-    // }
-
-    // flag_recorded = true;
-
-    // if (!flag_publish)
-    //     return;
-
     // voxel grid filtering for cloud map;
     PointCloudType::Ptr cloud_downsampled = LioLocus::downsampleCloud(ptr_cloud_map, params_lio_locus.leaf_size_vis_map);
 
@@ -379,8 +353,46 @@ void publishRobotPose()
 
     msg_pose.header.stamp = time_now;
     msg_pose.header.frame_id = params_lio_locus.frame_id_world;
+    msg_pose.child_frame_id = params_lio_locus.frame_id_ego;
     pub_pose->publish(msg_pose);
 
+	// reliablity value;
+	lio_locus::ReliablityStamped stamped_reliablity =
+			ptr_lio_locus->getStampedReliablityValue();
+
+	geometry_msgs::msg::Vector3Stamped msg_reliablity_ts;
+	msg_reliablity_ts.vector.x = stamped_reliablity.reliability;
+	msg_reliablity_ts.header.stamp = time_now;
+
+	pub_reliability->publish(msg_reliablity_ts);
+
+    //--------------------------------------------------------------------------
+    geometry_msgs::msg::TransformStamped t;
+
+    // Read message content and assign it to
+    // corresponding tf variables
+    t.header.stamp = time_now;
+    t.header.frame_id = params_lio_locus.frame_id_world;
+    t.child_frame_id = params_lio_locus.frame_id_ego;
+
+    t.transform.translation.x = p.x;
+    t.transform.translation.y = p.y;
+    t.transform.translation.z = p.z;
+
+    t.transform.rotation.x = q_tf2.x();
+    t.transform.rotation.y = q_tf2.y();
+    t.transform.rotation.z = q_tf2.z();
+    t.transform.rotation.w = q_tf2.w();
+
+    // Send the transformation
+    tf_broadcaster_->sendTransform(t);
+
+    //--------------------------------------------------------------------------
+
+
+
+
+
     bool flag_record_traj = false;
     if (!params_lio_locus.flag_use_prior_map)
     {

src/localization_module_lio_locus/config/parameters_lio_locus.yaml

@@ -12,7 +12,7 @@ ros2_wrapper:
     flag_use_scan2scan_matching: true
 
     flag_use_prior_map: false 
-    path_cloud_map: /home/solomon/cloud_map
+    path_cloud_map: /home/slam10/cloud_map
     fn_cloud_map: cloud_map.pcd
     leaf_size_load_map: 0.35
 
@@ -20,12 +20,12 @@ ros2_wrapper:
     leaf_size_vis_map: 0.2 
 
     flag_save_map_in_memory: true 
-    path_save_map: /home/demo/cloud_map
+    path_save_map: /home/slam10/cloud_map_bkup
     fn_save_cloud_map: cloud_map.pcd
     flag_voxel_filtering: true 
     leaf_size: 0.1
 
-    path_log: no_log
+    path_log: /home/slam10/log_dt
 
     threshold_trans_map: 0.5
     threshold_rot_map: 0.35 
@@ -54,7 +54,7 @@ ros2_wrapper:
 
 
 cloud_preprocessing:
-    leaf_voxel_filtering: 0.5
+    leaf_voxel_filtering: 0.2
     filter_limit: 120
 
     size_cores: 6
@@ -67,6 +67,7 @@ imu_complementary_filtering:
     do_bias_estimation: true
     do_adaptive_gain: true
     bias_alpha: 0.01
+    reverse_tf: true
     frame_fixed_: fixed_imu
     flag_publish_tf: false
     orientation_stddev: 0.0

src/localization_module_lio_locus/config/parameters_lio_locus_reloc.yaml

@@ -12,20 +12,20 @@ ros2_wrapper:
     flag_use_scan2scan_matching: true
 
     flag_use_prior_map: true 
-    path_cloud_map: /home/demo/cloud_map
-    fn_cloud_map: cloud_map.pcd
+    path_cloud_map: /home/slam10/cloud_map_bkup
+    fn_cloud_map: cloud_map_test.pcd
     leaf_size_load_map: 0.35
 
     ratio_fresh_map: 1.2
     leaf_size_vis_map: 0.2 
 
     flag_save_map_in_memory: true 
-    path_save_map: /home/solomon/cloud_map_bkup
+    path_save_map: /home/slam10/cloud_map_bkup
     fn_save_cloud_map: cloud_map.pcd
     flag_voxel_filtering: true 
     leaf_size: 0.1
 
-    path_log: no_log
+    path_log: /home/slam10/log_dt
 
     threshold_trans_map: 0.5
     threshold_rot_map: 0.35 
@@ -67,6 +67,7 @@ imu_complementary_filtering:
     do_bias_estimation: true
     do_adaptive_gain: true
     bias_alpha: 0.01
+    reverse_tf: true
     frame_fixed_: fixed_imu
     flag_publish_tf: false
     orientation_stddev: 0.0

src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/lio_locus.h

@@ -64,7 +64,7 @@ struct IMUData {
 	}
 
 	Eigen::Quaterniond orientation { Eigen::Quaterniond::Identity() };
-	double timestamp_;
+	double timestamp_; // in seconds;
 };
 
 struct TransformStamped {
@@ -81,6 +81,20 @@ struct TransformStamped {
 	std::uint64_t timestamp { 0 }; // PCL time stamp;
 };
 
+struct ReliablityStamped {
+	ReliablityStamped() {
+
+	}
+
+	ReliablityStamped(double r, std::uint64_t ts) {
+		reliability = r;
+		timestamp = ts;
+	}
+
+	double reliability { 0 };
+	std::uint64_t timestamp { 0 };
+};
+
 class LioLocus {
 public:
 	LioLocus();
@@ -179,6 +193,13 @@ public:
 		path_log = path;
 	}
 
+	inline ReliablityStamped getStampedReliablityValue() {
+		return m_reliability_ts;
+	}
+	inline void setLeafSizeCloudMap(double res) {
+		m_res_cloud_map = res;
+	}
+
 	//===============================================================================
 
 private:
@@ -214,6 +235,9 @@ private:
 
 	TransformStamped m_pose_latest;
 
+	double m_res_cloud_map { 0.5 };
+	ReliablityStamped m_reliability_ts;
+
 	// dependency processors;
 	PointCloudOdometryPtr mp_cloud_odometry;
 	PointCloudLocalizationPtr mp_cloud_localization;

src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/parameter_loading.h

@@ -51,8 +51,8 @@ struct ParameterLioLocus {
 
 	string path_log { "" };
 
-	double leaf_size_load_map { 0.5 };
-	double leaf_size_vis_map { 0.5 };
+	double leaf_size_cloud_map { 0.2 }; // parameter to compute reliability for localization result;
+	double leaf_size_vis_map { 0.5 }; // parameter to down-sample cloud map for visualization;
 
 	double ext_imu2lidar_dx { 0.0 };
 	double ext_imu2lidar_dy { 0.0 };
@@ -113,6 +113,7 @@ struct ParameterImuPreprocessing {
 	string path_log { "" };
 
 	bool pub_imu_preproc { true };
+	bool reverse_tf { true };
 
 	bool do_bias_estimation { true };
 	bool do_adaptive_gain { true };
@@ -129,6 +130,7 @@ using std::string;
 void writeDt2LogFile(string path, string fn, double dt);
 //-------------------------------------------------------------------------------------------
 
+// assign value of resolution for cloud map;
 bool loadParametersFromFileForLioLocus(std::string path_to_file,
 		ParameterLioLocus &params_lio_locus) {
 	if (!boost::filesystem::exists(path_to_file)) {
@@ -146,12 +148,12 @@ bool loadParametersFromFileForLioLocus(std::string path_to_file,
 	LOG(INFO) << "... reading parameter for lidar odometry from ["
 			<< path_to_file << "]";
 
+	// get parameters for cloud map;
 	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+
 	auto yn = yaml_node["ros2_wrapper"];
 
 	params_lio_locus.path_log = yn["path_log"].as<string>();
-	LOG(INFO) << "... path_log -> ["
-			<< params_lio_locus.path_log << "]";
 
 	params_lio_locus.flag_save_map_in_memory = yn["flag_save_map_in_memory"].as<
 			bool>();
@@ -216,6 +218,17 @@ bool loadParametersFromFileForLioLocus(std::string path_to_file,
 	LOG(INFO) << "... flag_use_prior_map -> ["
 			<< params_lio_locus.flag_use_prior_map << "]";
 
+	if (!params_lio_locus.flag_use_prior_map) {
+		// if not use prior map, then use value from cloud pre-processing;
+		params_lio_locus.leaf_size_cloud_map =
+				yaml_node["cloud_preprocessing"]["leaf_voxel_filtering"].as<
+						double>();
+	} else {
+		// take value from prior map loading;
+		params_lio_locus.leaf_size_cloud_map = yn["leaf_size_load_map"].as<
+				double>();
+	}
+
 	string path_map, nm_map;
 	path_map = yn["path_cloud_map"].as<string>();
 	nm_map = yn["fn_cloud_map"].as<string>();
@@ -227,10 +240,6 @@ bool loadParametersFromFileForLioLocus(std::string path_to_file,
 	LOG(INFO) << "... leaf size load map -> ["
 			<< params_lio_locus.leaf_size_vis_map << "]";
 
-	params_lio_locus.leaf_size_load_map = yn["leaf_size_load_map"].as<double>();
-	LOG(INFO) << "... leaf size load map -> ["
-			<< params_lio_locus.leaf_size_load_map << "]";
-
 	// parameters for cloud map generation;
 	params_lio_locus.threshold_trans_map =
 			yn["threshold_trans_map"].as<double>();
@@ -399,6 +408,10 @@ bool loadParametersFromFileForIMUPreprocessing(std::string path_to_file,
 	LOG(INFO) << "... flag_publish_tf -> ["
 			<< params_imu_preproc.flag_publish_tf << "]";
 
+	params_imu_preproc.reverse_tf = yn["reverse_tf"].as<bool>();
+	LOG(INFO) << "... reverse_tf -> [" << params_imu_preproc.reverse_tf
+			<< "]";
+
 	params_imu_preproc.frame_fixed = yn["frame_fixed_"].as<std::string>();
 
 	params_imu_preproc.orientation_stddev =
@@ -414,9 +427,9 @@ void writeDt2LogFile(string path, string fn, double dt) {
 		boost::filesystem::create_directory(path);
 	}
 
-	if(path=="no_log")
-	{
-		LOG(INFO) << "... disabled dt logging ... turn it on if needed by setting value of [path_log]!";
+	if (path == "no_log") {
+		LOG(INFO)
+				<< "... disabled dt logging ... turn it on if needed by setting value of [path_log]!";
 		return;
 	}
 

src/localization_module_lio_locus/launch/nodelets/lio_locus.launch.py

@@ -20,7 +20,7 @@ def generate_launch_description():
             Node(
                 package="lio_locus_humble",
                 executable="lio_locus_node",
-                #prefix="gdb -ex run --args",
+                # prefix="gdb -ex run --args",
                 name="lio_locus",
                 output="screen",
                 parameters=[

src/localization_module_lio_locus/src/lio_locus/CMakeLists.txt

@@ -6,7 +6,7 @@ target_link_libraries(${PROJECT_NAME}
     point_cloud_mapper
     boost_filesystem
     glog
-    ${PCL_LIBRARIES}
+      ${PCL_LIBRARIES}
     pcl_common
   ${YAML_CPP_LIBRARIES}
 )
@@ -25,7 +25,6 @@ add_library(cloud_pre_processor SHARED cloud_pre_processor.cpp)
 target_link_libraries(cloud_pre_processor
 	pcl_filters
 	pcl_features
-  ${PCL_LIBRARIES}
 	boost_filesystem
     glog
 	${YAML_CPP_LIBRARIES})

src/localization_module_lio_locus/src/lio_locus/lio_locus.cpp

@@ -185,10 +185,13 @@ void LioLocus::trackCurrentCloudWithOnlineBuiltMap(
 				geometry_utils::Transform3 current_pose =
 						mp_cloud_localization->GetIntegratedEstimate();
 
-				//TODO
-
-				// set pose of robot;
+				// get fitness value and compute confidence/reliablity value;
+				double reliablity = 1
+						- mp_cloud_localization->getFitnessScore()
+								/ m_res_cloud_map;
 
+				m_reliability_ts = ReliablityStamped(reliablity, timestamp_now);
+				
 			} else {
 				LOG(WARNING)
 						<< "... data association fails to find correspondences!";

src/localization_module_lio_locus/src/point_cloud_localization/CMakeLists.txt

@@ -2,7 +2,7 @@ add_library(point_cloud_localization SHARED
 	PointCloudLocalization.cc utils.cc)
 
 target_link_libraries(point_cloud_localization
-	  ${PCL_LIBRARIES}
+	#  ${PCL_LIBRARIES}
         pcl_octree
     pcl_filters
     pcl_search

src/localization_module_lio_locus/src/point_cloud_mapper/CMakeLists.txt

@@ -6,12 +6,12 @@ add_library(point_cloud_mapper SHARED
 
 
 target_include_directories(point_cloud_mapper PUBLIC
-  ${PCL_INCLUDE_DIRS}
-	${WORKSPACE_ROOT}deps_install/include/pcl-1.10
+	#	${PCL_INCLUDE_DIRS}
+	/home/demo/deps_install/include/pcl-1.10
 	)
 
 target_link_libraries(point_cloud_mapper
-  ${PCL_LIBRARIES}
+  ${PCL_LIBRARIRES}
   ${YAML_CPP_LIBRARIES}
   glog
   OpenMP::OpenMP_CXX

src/localization_module_lio_locus/src/point_cloud_odometry/CMakeLists.txt

@@ -7,7 +7,7 @@ add_library(point_cloud_odometry SHARED PointCloudOdometry.cc)
 
 target_link_libraries(point_cloud_odometry
     glog
-    ${PCL_LIBRARIES}
+    #    ${PCL_LIBRARIES}
         pcl_octree
     pcl_filters
     pcl_search