lio-cpp/main.cpp

#include <csignal>
#include "lio/Estimator.h"
#include "lio/mapper.h"
#include "lddc.h"
#include "lds_hub.h"
#include "lds_lidar.h"
#include "lds_lvx.h"
#include "livox_sdk.h"

#include "pangolinViewer.h"
#include "segment/LidarFeatureExtractor.h"

#include "dijkstra/dijkstra.h"
#include "define/timedlockdata.hpp"
#include "MPC/navigation.h"

//雷达相关
livox_ros::Lddc *lddc= nullptr;
//特征提取相关
LidarFeatureExtractor* lidarFeatureExtractor;
pcl::PointCloud<PointType>::Ptr laserCloud;
int Lidar_Type=0;
int N_SCANS = 6;
bool Feature_Mode = false;
bool Use_seg = false;

//slam 地图相关
Mapper* pMaper= nullptr;

//路径相关
int node_beg=1,node_end=6;
std::mutex pathMutex;
std::vector<int> shortest_path;
PathMap DijkstraMap;

//控制相关
Navigation* navigator=nullptr;

TimedLockData<PointCloud> scan_cloud;
TimedLockData<PointCloud::Ptr> localmap_cloud;
TimedLockData<Eigen::Matrix4d> cur_pose;
double final_cost=0;

void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
{
    scan_cloud.reset(*cloud,0.1);
}

void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
{
    localmap_cloud.reset(cloud,0.1);
}

void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
{
  cur_pose.reset(tranform);
  //Eigen::Vector3d eulerAngle=tranform.topLeftCorner(3,3).eulerAngles(2,1,0);
  Pose2d pose(tranform(0,3),tranform(1,3),0);
  Navigation::GetInstance()->ResetPose(pose);
}

void FinalCostCallback(double cost)
{
  final_cost=cost;
}

void CloudCallback(TimeCloud tc,int handle)
{
    double timeSpan =tc.cloud->points.back().normal_x;
    laserCloud.reset(new pcl::PointCloud<PointType>);
    laserCloud->reserve(15000*N_SCANS);

    PointType point;
    for(const auto& p : tc.cloud->points)
    {

        //std::cout<<point.normal_y<<"   "<<point.normal_x<<"   "<<timeSpan<<std::endl;
        if (p.normal_y > N_SCANS - 1) continue;
        if (p.x < 0.01) continue;
        if (!pcl_isfinite(p.x) ||
                !pcl_isfinite(p.y) ||
                !pcl_isfinite(p.z))
        {
            continue;
        }
        point.x=p.x;
        point.y=p.y;
        point.z=p.z;
        point.normal_x = p.normal_x / timeSpan; //normao_x  time
        point.intensity=p.intensity;
        int line=int(p.normal_y);
        point.normal_y=LidarFeatureExtractor::_int_as_float(line);
        laserCloud->push_back(point);
      /*std::cout<<" x:"<<point.x<<" y:"<<point.y<<" z:"<<point.z
               <<" intnesity:"<<point.intensity<<" normal_x:"<<point.normal_x
               <<" normal_y:"<<point.normal_y<<std::endl;*/
    }

    tc.cloud=laserCloud;
    pMaper->AddCloud(tc);

}

void ImuDataCallback(ImuData imu,int handle){
  pMaper->AddImu(imu);
}

bool InitLidar(double freq);
bool InitMap(std::string config_file);
bool InitDijkstraMap();

void pangolinSpinOnce(PangolinViewer* viewer)
{

  if(cur_pose.timeout()==false)
    viewer->ShowRealtimePose(cur_pose.Get());

  //绘制变换后的扫描点
  double ptSize=1;
  double alpha=1;

  PointCloud scan=scan_cloud.Get();
  if(scan.size()>0)
    viewer->DrawCloud(scan.makeShared(),1,1,1,alpha,ptSize);

  //地图点
  ptSize=0.1;
  alpha=0.2;
  viewer->DrawCloud(pMaper->GetMapCorner(),1,0,0,alpha,ptSize);
  viewer->DrawCloud(pMaper->GetMapSurf(),0.1,1,0,alpha,ptSize);
  viewer->DrawCloud(pMaper->GetMapNonf(),0.1,0,1,alpha,ptSize);

  viewer->ShowSlamCost(final_cost);
  if(pMaper!= nullptr)
    viewer->ShowSlamQueueTime(pMaper->_lidarMsgQueue.size(),pMaper->frame_tm_);

  //绘制dijkstra地图及最短路径
  pathMutex.lock();
  std::vector<int> solve_path=shortest_path;
  pathMutex.unlock();
  std::reverse(solve_path.begin(),solve_path.end());
  solve_path.push_back(node_end);
  viewer->DrawDijkastraMap(DijkstraMap, solve_path);
}
void OnStartBtn(std::string lvxfile)
{
  if(lddc!= nullptr)
  {
    if (lvxfile=="")
      lddc->Start();
    else
      lddc->Start(lvxfile);
  }
}
void OnStopBtn()
{
  if(lddc!= nullptr)
    lddc->Stop();
}
void OnFreqChanged(int freq)
{
  if(lddc!= nullptr)
    lddc->SetPublishFrq(freq);
}
bool OnDijkstraBtn(int beg,int end,float& distance,
        std::vector<int>& shortestPath)
{
  bool ret= DijkstraMap.GetShortestPath(beg, end, shortestPath, distance);
  if(ret)
  {
    node_beg=beg;
    node_end=end;
    pathMutex.lock();
    shortest_path = shortestPath;
    pathMutex.unlock();
  }
  return ret;
}

int main(int argc, char** argv)
{
  //reg参数
  std::string config_file="../config/horizon_config.yaml";
  if(InitMap(config_file)==false)
    return -1;

  if(!InitDijkstraMap())
  {
    printf("Dijkstra Map load failed\n");
    return -2;
  }

  //初始化雷达
  double publish_freq  = 10.0;
  if(!InitLidar(publish_freq))
    return -3;


  PangolinViewer* viewer=PangolinViewer::CreateViewer();
  viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged,OnDijkstraBtn);
  viewer->Join();

  TimerRecord::PrintAll();
  lddc->Stop();
  pMaper->completed();
  delete pMaper;
  delete lddc;
  delete viewer;
  return 0;
}

bool InitMap(std::string config_file)
{
  cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
  if (!fsSettings.isOpened())
  {
    std::cout << "config_file error: cannot open " << config_file << std::endl;
    return false;
  }
  Lidar_Type = static_cast<int>(fsSettings["Lidar_Type"]);
  N_SCANS = static_cast<int>(fsSettings["Used_Line"]);
  Feature_Mode = static_cast<int>(fsSettings["Feature_Mode"]);
  Use_seg = static_cast<int>(fsSettings["Use_seg"]);

  int NumCurvSize = static_cast<int>(fsSettings["NumCurvSize"]);
  float DistanceFaraway = static_cast<float>(fsSettings["DistanceFaraway"]);
  int NumFlat = static_cast<int>(fsSettings["NumFlat"]);
  int PartNum = static_cast<int>(fsSettings["PartNum"]);
  float FlatThreshold = static_cast<float>(fsSettings["FlatThreshold"]);
  float BreakCornerDis = static_cast<float>(fsSettings["BreakCornerDis"]);
  float LidarNearestDis = static_cast<float>(fsSettings["LidarNearestDis"]);
  float KdTreeCornerOutlierDis = static_cast<float>(fsSettings["KdTreeCornerOutlierDis"]);

  int ivox_nearby_type = static_cast<int>(fsSettings["ivox_nearby_type"]);
  int max_ivox_node = static_cast<int>(fsSettings["max_ivox_node"]);
  float ivox_resolution = static_cast<float>(fsSettings["ivox_node_solution"]);

  laserCloud.reset(new pcl::PointCloud<PointType>);

  lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,
                                                    NumCurvSize,
                                                    DistanceFaraway,
                                                    NumFlat,
                                                    PartNum,
                                                    FlatThreshold,
                                                    BreakCornerDis,
                                                    LidarNearestDis,
                                                    KdTreeCornerOutlierDis);

  //map 参数
  std::string map_dir="../map/";
  float filter_parameter_corner = 0.2;
  float filter_parameter_surf = 0.4;
  int IMU_Mode = 2;

  double vecTlb[]={1.0, 0.0, 0.0, -0.05512,
          0.0, 1.0, 0.0, -0.02226,
          0.0, 0.0, 1.0,  0.0297,
          0.0, 0.0, 0.0,  1.0};

  // set extrinsic matrix between lidar & IMU
  Eigen::Matrix3d R;
  Eigen::Vector3d t;
  R << vecTlb[0], vecTlb[1], vecTlb[2],
          vecTlb[4], vecTlb[5], vecTlb[6],
          vecTlb[8], vecTlb[9], vecTlb[10];
  t << vecTlb[3], vecTlb[7], vecTlb[11];
  Eigen::Quaterniond qr(R);
  R = qr.normalized().toRotationMatrix();


  int WINDOWSIZE = 0;
  if (IMU_Mode < 2)
    WINDOWSIZE = 1;
  else
    WINDOWSIZE = 20;

  pMaper = new Mapper(WINDOWSIZE, lidarFeatureExtractor,Use_seg,filter_parameter_corner, filter_parameter_surf,
                      ivox_nearby_type, max_ivox_node, ivox_resolution);
  pMaper->InitRT(R, t);
  pMaper->SetOdomCallback(OdometryCallback);
  pMaper->SetCloudMappedCallback(CloudFullCallback);
  pMaper->SetLocalMapCloudCallback(CloudLocalCallback);
  pMaper->SetFinalCostCallback(FinalCostCallback);
  pMaper->LoadMap(map_dir + "/featurebbt_transformed.txt");

  double RPI = M_PI / 180.0;
  Eigen::Vector3d eulerAngle(0 * RPI, 0 * RPI, 11.5 * RPI);
  Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
  Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
  Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));

  Eigen::Matrix3d rotation_matrix;
  rotation_matrix = yawAngle * pitchAngle * rollAngle;
  Eigen::Matrix4d transform;
  transform.topLeftCorner(3, 3) = rotation_matrix;
  transform.topRightCorner(3, 1) = Eigen::Vector3d(0.46, 0.48, 0.73);
  pMaper->SetInitPose(transform);

  printf("  window size :%d\n--------------", WINDOWSIZE);
  return true;
}

bool InitDijkstraMap()
{
  DijkstraMap.AddVertex(1, 2.3, 3.1);
  DijkstraMap.AddVertex(2, 19.2, 3.1);
  DijkstraMap.AddVertex(3, 59.7, 3.3);
  DijkstraMap.AddVertex(4, 98.4, 3.3);
  DijkstraMap.AddVertex(5, 98.5, -13);
  DijkstraMap.AddVertex(6, 59.5, -13);
  DijkstraMap.AddVertex(7, 20, 18);

  bool ret=true;
  ret=ret&&DijkstraMap.AddEdge(1, 2, false);
  ret=ret&&DijkstraMap.AddEdge(2, 3, false);
  ret=ret&&DijkstraMap.AddEdge(2, 7, false);
  ret=ret&&DijkstraMap.AddEdge(3, 4, false);
  ret=ret&&DijkstraMap.AddEdge(3, 6, false);
  ret=ret&&DijkstraMap.AddEdge(4, 5, false);
  ret=ret&&DijkstraMap.AddEdge(5, 6, false);
  return ret;
}

bool InitLidar(double publish_freq)
{
  LivoxSdkVersion _sdkversion;
  GetLivoxSdkVersion(&_sdkversion);
  const int32_t kSdkVersionMajorLimit = 2;
  if (_sdkversion.major < kSdkVersionMajorLimit) {
    printf("The SDK version[%d.%d.%d] is too low\n", _sdkversion.major,
           _sdkversion.minor, _sdkversion.patch);
    return false;
  }
  if (publish_freq > 100.0) {
    publish_freq = 100.0;
  } else if (publish_freq < 0.1) {
    publish_freq = 0.1;
  } else {
    publish_freq = publish_freq;
  }
  lddc= new livox_ros::Lddc(publish_freq);

  lddc->SetCloudCallback(CloudCallback);
  lddc->SetImuCallback(ImuDataCallback);
  return true;
}