admin/Point-LIO_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update.

Browse Source
This commit is contained in:
uavfly
2025-11-15 21:30:00 +08:00
parent 5dcf0c4c26
commit a6f679d2ed
9 changed files with 46 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@@ -3,17 +3,17 @@ project(point_lio)
SET(CMAKE_BUILD_TYPE "Debug") SET(CMAKE_BUILD_TYPE "Debug")
ADD_COMPILE_OPTIONS(-std=c++14 ) ADD_COMPILE_OPTIONS(-std=c++17 )
ADD_COMPILE_OPTIONS(-std=c++14 ) ADD_COMPILE_OPTIONS(-std=c++17 )
set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) set( CMAKE_CXX_FLAGS "-std=c++17 -O3" )
add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -std=c++0x -std=c++17 -fexceptions")
#add_compile_definitions(BOOST_BIND_GLOBAL_PLACEHOLDERS) #add_compile_definitions(BOOST_BIND_GLOBAL_PLACEHOLDERS)

4
include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp
View File

@@ -38,8 +38,7 @@
#include <vector> #include <vector>
#include <cstdlib> #include <cstdlib>
#include <boost/bind/bind.hpp>
#include <boost/bind.hpp>
#include <Eigen/Core> #include <Eigen/Core>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <Eigen/Dense> #include <Eigen/Dense>
@@ -57,6 +56,7 @@
namespace esekfom { namespace esekfom {
using namespace Eigen; using namespace Eigen;
using namespace boost::placeholders;
template<typename T> template<typename T>
struct dyn_share_modified struct dyn_share_modified

3
src/IMU_Processing.cpp
View File

@@ -1,4 +1,5 @@
#include "IMU_Processing.h" #include "IMU_Processing.h"
#include "parameters.h"
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
@@ -116,7 +117,7 @@ void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_u
if (init_iter_num > MAX_INI_COUNT) if (init_iter_num > MAX_INI_COUNT)
{ {
ROS_INFO("IMU Initializing: %.1f %%", 100.0); RCLCPP_WARN(logger, "IMU Initializing: %.1f %%", 100.0);
imu_need_init_ = false; imu_need_init_ = false;
*cur_pcl_un_ = *(meas.lidar); *cur_pcl_un_ = *(meas.lidar);
} }

2
src/IMU_Processing.h
View File

@@ -10,6 +10,8 @@
#include <Eigen/Eigen> #include <Eigen/Eigen>
// #include "Estimator.h" // #include "Estimator.h"
#include <common_lib.h> #include <common_lib.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logging.hpp>
#include <pcl/common/io.h> #include <pcl/common/io.h>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>

12
src/laserMapping.cpp
View File

@@ -46,6 +46,8 @@ nav_msgs::msg::Path path;
nav_msgs::msg::Odometry odomAftMapped; nav_msgs::msg::Odometry odomAftMapped;
geometry_msgs::msg::PoseStamped msg_body_pose; geometry_msgs::msg::PoseStamped msg_body_pose;
rclcpp::Logger logger = rclcpp::get_logger("laserMapping");
void SigHandle(int sig) void SigHandle(int sig)
{ {
flg_exit = true; flg_exit = true;
@@ -268,7 +270,7 @@ void set_posestamp(T & out)
} }
} }
void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pubOdomAftMapped) void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pubOdomAftMapped, const std::shared_ptr<tf2_ros::TransformBroadcaster> &tf_br)
{ {
odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "body"; odomAftMapped.child_frame_id = "body";
@@ -384,12 +386,11 @@ int main(int argc, char** argv)
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 1000); rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 1000);
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped = nh->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 1000); rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped = nh->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init", 1000);
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath = nh->create_publisher<nav_msgs::msg::Path>("/path", 1000); rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath = nh->create_publisher<nav_msgs::msg::Path>("/path", 1000);
("/path", 1000);
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub = nh->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1000); rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub = nh->create_publisher<visualization_msgs::msg::Marker>("/planner_normal", 1000);
tf2_ros::TransformBroadcaster::SharedPtr tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(nh); std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(nh);
//------------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------------
signal(SIGINT, SigHandle); signal(SIGINT, SigHandle);
rclcpp::Rate loop_rate(500); rclcpp::Rate rate(500);
while (rclcpp::ok()) while (rclcpp::ok())
{ {
@@ -1055,8 +1056,7 @@ int main(int argc, char** argv)
dump_lio_state_to_log(fp); dump_lio_state_to_log(fp);
} }
} }
status = ros::ok(); rate.sleep();
loop_rate.sleep();
} }
//--------------------------save map----------------------------------- //--------------------------save map-----------------------------------
/* 1. make sure you have enough memories /* 1. make sure you have enough memories

11
src/li_initialization.cpp
View File

@@ -189,9 +189,10 @@ void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in)
// publish_count ++; // publish_count ++;
sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in)); sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in));
double timestamp = get_time_sec(msg->header.stamp);
msg->header.stamp = get_ros_time(timestamp - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar); msg->header.stamp = get_ros_time(timestamp - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar);
double timestamp = get_time_sec(msg->header.stamp);
// printf("time_diff%f, %f, %f\n", last_timestamp_imu - timestamp, last_timestamp_imu, timestamp); // printf("time_diff%f, %f, %f\n", last_timestamp_imu - timestamp, last_timestamp_imu, timestamp);
if (timestamp < last_timestamp_imu) if (timestamp < last_timestamp_imu)
@@ -309,7 +310,7 @@ bool sync_packages(MeasureGroup &meas)
/*** push imu data, and pop from imu buffer ***/ /*** push imu data, and pop from imu buffer ***/
if (p_imu->imu_need_init_) if (p_imu->imu_need_init_)
{ {
double imu_time = imu_deque.front()->header.stamp.toSec(); double imu_time = get_time_sec(imu_deque.front()->header.stamp);
imu_next = *(imu_deque.front()); imu_next = *(imu_deque.front());
meas.imu.shrink_to_fit(); meas.imu.shrink_to_fit();
while (imu_time < lidar_end_time) while (imu_time < lidar_end_time)
@@ -318,7 +319,7 @@ bool sync_packages(MeasureGroup &meas)
imu_last = imu_next; imu_last = imu_next;
imu_deque.pop_front(); imu_deque.pop_front();
if(imu_deque.empty()) break; if(imu_deque.empty()) break;
imu_time = imu_deque.front()->header.stamp.toSec(); // can be changed imu_time = get_time_sec(imu_deque.front()->header.stamp);
imu_next = *(imu_deque.front()); imu_next = *(imu_deque.front());
} }
} }
@@ -330,7 +331,7 @@ bool sync_packages(MeasureGroup &meas)
/*** push imu data, and pop from imu buffer ***/ /*** push imu data, and pop from imu buffer ***/
if (p_imu->imu_need_init_) if (p_imu->imu_need_init_)
{ {
double imu_time = imu_deque.front()->header.stamp.toSec(); double imu_time = get_time_sec(imu_deque.front()->header.stamp);
meas.imu.shrink_to_fit(); meas.imu.shrink_to_fit();
imu_next = *(imu_deque.front()); imu_next = *(imu_deque.front());
@@ -340,7 +341,7 @@ bool sync_packages(MeasureGroup &meas)
imu_last = imu_next; imu_last = imu_next;
imu_deque.pop_front(); imu_deque.pop_front();
if(imu_deque.empty()) break; if(imu_deque.empty()) break;
imu_time = imu_deque.front()->header.stamp.toSec(); // can be changed imu_time = get_time_sec(imu_deque.front()->header.stamp);
imu_next = *(imu_deque.front()); imu_next = *(imu_deque.front());
} }
} }

2
src/parameters.h
View File

@@ -75,6 +75,8 @@ extern double time_update_last, time_current, time_predict_last_const, t_last;
extern MeasureGroup Measures; extern MeasureGroup Measures;
extern rclcpp::Logger logger;
extern ofstream fout_out, fout_imu_pbp; extern ofstream fout_out, fout_imu_pbp;
void readParameters(rclcpp::Node::SharedPtr &nh); void readParameters(rclcpp::Node::SharedPtr &nh);
void open_file(); void open_file();

19
src/preprocess.cpp
View File

@@ -1,3 +1,4 @@
#include "common_lib.h"
#include "preprocess.h" #include "preprocess.h"
#define RETURN0 0x00 #define RETURN0 0x00
@@ -44,13 +45,13 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
point_filter_num = pfilt_num; point_filter_num = pfilt_num;
} }
void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{ {
avia_handler(msg); avia_handler(msg);
*pcl_out = pl_surf; *pcl_out = pl_surf;
} }
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) void Preprocess::process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{ {
switch (time_unit) switch (time_unit)
{ {
@@ -93,7 +94,7 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
} }
void Preprocess::process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, void Preprocess::process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg,
deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, std::deque<PointCloudXYZI::Ptr> &pcl_out, std::deque<double> &time_lidar,
const int required_frame_num, int scan_count) { const int required_frame_num, int scan_count) {
int plsize = msg->point_num; int plsize = msg->point_num;
pl_surf.clear(); pl_surf.clear();
@@ -141,7 +142,7 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg
for (uint i = 1; i < valid_pcl_size; i++) { for (uint i = 1; i < valid_pcl_size; i++) {
valid_num++; valid_num++;
//Compute new opffset time of each point：ms //Compute new opffset time of each point：ms
pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time; pl_surf[i].curvature += get_time_sec(msg->header.stamp) * 1000 - last_frame_end_time;
pcl_cut.push_back(pl_surf[i]); pcl_cut.push_back(pl_surf[i]);
if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) { if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {
cut_num++; cut_num++;
@@ -158,8 +159,8 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg
} }
#define MAX_LINE_NUM 128 #define MAX_LINE_NUM 128
void void
Preprocess::process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, Preprocess::process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, std::deque<PointCloudXYZI::Ptr> &pcl_out,
deque<double> &time_lidar, const int required_frame_num, int scan_count) { std::deque<double> &time_lidar, const int required_frame_num, int scan_count) {
pl_surf.clear(); pl_surf.clear();
pl_corn.clear(); pl_corn.clear();
pl_full.clear(); pl_full.clear();
@@ -371,7 +372,7 @@ void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::ConstShared
pl_surf.reserve(plsize); pl_surf.reserve(plsize);
double time_stamp = msg->header.stamp.toSec(); double time_stamp = get_time_sec(msg->header.stamp);
// cout << "===================================" << endl; // cout << "===================================" << endl;
// printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
for (int i = 0; i < pl_orig.points.size(); i++) for (int i = 0; i < pl_orig.points.size(); i++)
@@ -872,10 +873,10 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
} }
} }
void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct)
{ {
pl.height = 1; pl.width = pl.size(); pl.height = 1; pl.width = pl.size();
sensor_msgs::PointCloud2 output; sensor_msgs::msg::PointCloud2 output;
pcl::toROSMsg(pl, output); pcl::toROSMsg(pl, output);
output.header.frame_id = "livox"; output.header.frame_id = "livox";
output.header.stamp = ct; output.header.stamp = ct;

15
src/preprocess.h
View File

@@ -1,8 +1,15 @@
#pragma once
#include <common_lib.h>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <livox_ros_driver2/msg/custom_msg.hpp> #include <livox_ros_driver2/msg/custom_msg.hpp>
#include <cmath>
#include <deque>
#include <vector>
using namespace std; using namespace std;
#define IS_VALID(a) ((abs(a)>1e8) ? true : false) #define IS_VALID(a) ((abs(a)>1e8) ? true : false)
@@ -107,9 +114,9 @@ class Preprocess
Preprocess(); Preprocess();
~Preprocess(); ~Preprocess();
void process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count); void process_cut_frame_livox(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, std::deque<PointCloudXYZI::Ptr> &pcl_out, std::deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count); void process_cut_frame_pcl2(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, std::deque<PointCloudXYZI::Ptr> &pcl_out, std::deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); void process(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out); void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
@@ -123,7 +130,7 @@ class Preprocess
int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
double blind, det_range; double blind, det_range;
bool given_offset_time; bool given_offset_time;
ros::Publisher pub_full, pub_surf, pub_corn; // ros::Publisher pub_full, pub_surf, pub_corn;
private: private:
@@ -132,7 +139,7 @@ class Preprocess
void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void hesai_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); void hesai_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types); void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct); void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir); bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);