fix run failed

src/localization_module_lio_locus/CMakeLists.txt

@@ -1,5 +1,4 @@
 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")
@@ -26,20 +25,35 @@ 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 "/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(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(PCL_DIR /home/slam10/hivecore_robot_slam/deps_install/share/pcl-1.10)
-
-# set(PCL_ROOT /home/slam10/hivecore_robot_slam/deps_install)
+set(PCL_ROOT "${WORKSPACE_ROOT}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}")
@@ -47,21 +61,33 @@ 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(
-#         /home/slam10/hivecore_robot_slam/deps_install/lib
-# )
-
+link_directories(
+    ${WORKSPACE_ROOT}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,6 +7,7 @@ target_link_libraries(lio_locus_node
 	point_cloud_localization
 	point_cloud_mapper
 	boost_filesystem
+    ${PCL_LIBRARIES}
 )
 
 target_include_directories(lio_locus_node PUBLIC
@@ -24,7 +25,7 @@ ament_target_dependencies(
   "pcl_conversions"
   "std_srvs"
   "tf2"
-  "tf2_ros"
+  #"tf2_ros"
 )
 
 # install exe to folder within lib;
@@ -73,6 +74,7 @@ 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,7 +85,6 @@ 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,7 +30,6 @@ 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;
 
@@ -49,7 +48,6 @@ 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);
@@ -81,8 +79,6 @@ 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)
 {
 
@@ -97,9 +93,6 @@ 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;
 
@@ -133,7 +126,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_cloud_map);
+        ptr_lio_locus->uploadPriorCloudMap(params_lio_locus.fn_cloud_map, params_lio_locus.leaf_size_load_map);
 
         // set initial pose;
         geometry_utils::Transform3 pose_prior = geometry_utils::Transform3::Identity();
@@ -170,7 +163,6 @@ 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);
@@ -312,6 +304,40 @@ 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);
 
@@ -353,46 +379,8 @@ 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/slam10/cloud_map
+    path_cloud_map: /home/solomon/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/slam10/cloud_map_bkup
+    path_save_map: /home/demo/cloud_map
     fn_save_cloud_map: cloud_map.pcd
     flag_voxel_filtering: true 
     leaf_size: 0.1
 
-    path_log: /home/slam10/log_dt
+    path_log: no_log
 
     threshold_trans_map: 0.5
     threshold_rot_map: 0.35 
@@ -54,7 +54,7 @@ ros2_wrapper:
 
 
 cloud_preprocessing:
-    leaf_voxel_filtering: 0.2
+    leaf_voxel_filtering: 0.5
     filter_limit: 120
 
     size_cores: 6
@@ -67,7 +67,6 @@ 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/slam10/cloud_map_bkup
-    fn_cloud_map: cloud_map_test.pcd
+    path_cloud_map: /home/demo/cloud_map
+    fn_cloud_map: cloud_map.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/slam10/cloud_map_bkup
+    path_save_map: /home/solomon/cloud_map_bkup
     fn_save_cloud_map: cloud_map.pcd
     flag_voxel_filtering: true 
     leaf_size: 0.1
 
-    path_log: /home/slam10/log_dt
+    path_log: no_log
 
     threshold_trans_map: 0.5
     threshold_rot_map: 0.35 
@@ -67,7 +67,6 @@ 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_; // in seconds;
+	double timestamp_;
 };
 
 struct TransformStamped {
@@ -81,20 +81,6 @@ 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();
@@ -193,13 +179,6 @@ public:
 		path_log = path;
 	}
 
-	inline ReliablityStamped getStampedReliablityValue() {
-		return m_reliability_ts;
-	}
-	inline void setLeafSizeCloudMap(double res) {
-		m_res_cloud_map = res;
-	}
-
 	//===============================================================================
 
 private:
@@ -235,9 +214,6 @@ 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_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 leaf_size_load_map { 0.5 };
+	double leaf_size_vis_map { 0.5 };
 
 	double ext_imu2lidar_dx { 0.0 };
 	double ext_imu2lidar_dy { 0.0 };
@@ -113,7 +113,6 @@ struct ParameterImuPreprocessing {
 	string path_log { "" };
 
 	bool pub_imu_preproc { true };
-	bool reverse_tf { true };
 
 	bool do_bias_estimation { true };
 	bool do_adaptive_gain { true };
@@ -130,7 +129,6 @@ 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)) {
@@ -148,12 +146,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>();
@@ -218,17 +216,6 @@ 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>();
@@ -240,6 +227,10 @@ 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>();
@@ -408,10 +399,6 @@ 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 =
@@ -427,9 +414,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,6 +25,7 @@ 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,13 +185,10 @@ void LioLocus::trackCurrentCloudWithOnlineBuiltMap(
 				geometry_utils::Transform3 current_pose =
 						mp_cloud_localization->GetIntegratedEstimate();
 
-				// get fitness value and compute confidence/reliablity value;
-				double reliablity = 1
-						- mp_cloud_localization->getFitnessScore()
-								/ m_res_cloud_map;
+				//TODO
+
+				// set pose of robot;
 
-				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}
-	/home/demo/deps_install/include/pcl-1.10
+  ${PCL_INCLUDE_DIRS}
+	${WORKSPACE_ROOT}deps_install/include/pcl-1.10
 	)
 
 target_link_libraries(point_cloud_mapper
-  ${PCL_LIBRARIRES}
+  ${PCL_LIBRARIES}
   ${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