diff --git a/src/localization_module_lio_locus/CMakeLists.txt b/src/localization_module_lio_locus/CMakeLists.txt index a605860..3959a79 100755 --- a/src/localization_module_lio_locus/CMakeLists.txt +++ b/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") @@ -26,19 +27,19 @@ find_package(OpenMP) find_package(Eigen3) -set(CMAKE_PREFIX_PATH "/home/demo/deps_install" ${CMAKE_PREFIX_PATH}) -set(CMAKE_INCLUDE_PATH "/home/demo/deps_install/include" ${CMAKE_INCLUDE_PATH}) -set(CMAKE_LIBRARY_PATH "/home/demo/deps_install/lib" ${CMAKE_LIBRARY_PATH}) +# 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_DIR /home/demo/deps_install/share/pcl-1.10) +# set(PCL_DIR /home/slam10/hivecore_robot_slam/deps_install/share/pcl-1.10) -set(PCL_ROOT /home/demo/deps_install) +# set(PCL_ROOT /home/slam10/hivecore_robot_slam/deps_install) -find_package(PCL REQUIRED) +find_package(PCL 1.10 REQUIRED) 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}") @@ -53,17 +54,17 @@ include_directories( ${PCL_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIR} /user/include - /home/demo/deps_install/include/pcl-1.10 + # /home/slam10/hivecore_robot_slam/deps_install/include/pcl-1.10 ) -link_directories( - /home/demo/deps_install/lib -) +# link_directories( +# /home/slam10/hivecore_robot_slam/deps_install/lib +# ) add_subdirectory(src) -#add_subdirectory(apps/nodelets) +add_subdirectory(apps/nodelets) install( DIRECTORY launch diff --git a/src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt b/src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt index cbed26c..dae9d61 100755 --- a/src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt +++ b/src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt @@ -24,7 +24,7 @@ ament_target_dependencies( "pcl_conversions" "std_srvs" "tf2" - #"tf2_ros" + "tf2_ros" ) # install exe to folder within lib; diff --git a/src/localization_module_lio_locus/apps/nodelets/cloud_preprocessing_node.cpp b/src/localization_module_lio_locus/apps/nodelets/cloud_preprocessing_node.cpp index d7b209b..5e15fbe 100755 --- a/src/localization_module_lio_locus/apps/nodelets/cloud_preprocessing_node.cpp +++ b/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::Ptr cloud_raw( new pcl::PointCloud()); + params_cloud_preproc.topic_cloud_raw = "/livox/lidar"; if(params_cloud_preproc.topic_cloud_raw == "/rsm1_1") { cloud_raw = CloudPreProcessor::adaptCloudForm(msg); diff --git a/src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp b/src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp old mode 100755 new mode 100644 index 5bbfcc9..eedef82 --- a/src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp +++ b/src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp @@ -30,6 +30,7 @@ LioLocus::Ptr ptr_lio_locus; #include #include #include +#include using std::string; @@ -48,6 +49,7 @@ rclcpp::Publisher::SharedPtr pub_pose_array; rclcpp::Publisher::SharedPtr pub_pose; rclcpp::Publisher::SharedPtr dt_publisher_; +rclcpp::Publisher::SharedPtr pub_reliability; void publishRobotPose(); void publishLocalizedCloud(); void publishCloudMap(rclcpp::Time ts); @@ -79,6 +81,8 @@ void serviceInvoking(const std::shared_ptr reques RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "################## service invoked: !!! "); } +std::unique_ptr tf_broadcaster_; + int main(int argc, char **argv) { @@ -93,6 +97,9 @@ int main(int argc, char **argv) node = std::make_shared("lio_locus_node"); + tf_broadcaster_ = + std::make_unique(*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("/cloud_map", 9); dt_publisher_ = node->create_publisher("/dt_lio_locus", 9); + pub_reliability = node->create_publisher("/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) { diff --git a/src/localization_module_lio_locus/config/parameters_lio_locus.yaml b/src/localization_module_lio_locus/config/parameters_lio_locus.yaml old mode 100755 new mode 100644 index efde4f0..ba60ede --- a/src/localization_module_lio_locus/config/parameters_lio_locus.yaml +++ b/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 diff --git a/src/localization_module_lio_locus/config/parameters_lio_locus_reloc.yaml b/src/localization_module_lio_locus/config/parameters_lio_locus_reloc.yaml old mode 100755 new mode 100644 index 6109d92..c86de01 --- a/src/localization_module_lio_locus/config/parameters_lio_locus_reloc.yaml +++ b/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 diff --git a/src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/lio_locus.h b/src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/lio_locus.h old mode 100755 new mode 100644 index ed740e8..d4f6dca --- a/src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/lio_locus.h +++ b/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; diff --git a/src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/parameter_loading.h b/src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/parameter_loading.h old mode 100755 new mode 100644 index 887fe08..7bed4fe --- a/src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/parameter_loading.h +++ b/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 ¶ms_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(); - 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(); nm_map = yn["fn_cloud_map"].as(); @@ -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(); - 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(); @@ -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(); + LOG(INFO) << "... reverse_tf -> [" << params_imu_preproc.reverse_tf + << "]"; + params_imu_preproc.frame_fixed = yn["frame_fixed_"].as(); 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; } diff --git a/src/localization_module_lio_locus/launch/nodelets/lio_locus.launch.py b/src/localization_module_lio_locus/launch/nodelets/lio_locus.launch.py index eab222b..eaaf3fb 100755 --- a/src/localization_module_lio_locus/launch/nodelets/lio_locus.launch.py +++ b/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=[ diff --git a/src/localization_module_lio_locus/src/CMakeLists.txt b/src/localization_module_lio_locus/src/CMakeLists.txt index c08f13e..9656023 100755 --- a/src/localization_module_lio_locus/src/CMakeLists.txt +++ b/src/localization_module_lio_locus/src/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(point_cloud_odometry) add_subdirectory(point_cloud_localization) add_subdirectory(point_cloud_mapper) -#add_subdirectory(lio_locus) +add_subdirectory(lio_locus) diff --git a/src/localization_module_lio_locus/src/lio_locus/lio_locus.cpp b/src/localization_module_lio_locus/src/lio_locus/lio_locus.cpp old mode 100755 new mode 100644 index 536c0ec..0f711c9 --- a/src/localization_module_lio_locus/src/lio_locus/lio_locus.cpp +++ b/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!";