Update README.md

NeuralTraj/.catkin_workspace

@@ -1 +0,0 @@
-# This file currently only serves to mark the location of a catkin workspace for tool integration

NeuralTraj/.vscode/c_cpp_properties.json

@@ -1,21 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/opt/ros/noetic/include/**",
-        "/usr/include/**",
-        "${workspaceFolder}/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
-}

NeuralTraj/run.sh

@@ -1,4 +0,0 @@
-roslaunch random_map_generator test.launch            & sleep 1; #not sending map now
-roslaunch plan_manage plan_node.launch & sleep 1;
-wait
-

NeuralTraj/src/CMakeLists.txt

@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

NeuralTraj/src/path_search/CMakeLists.txt

@@ -1,73 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(path_search)
-set(CMAKE_VERBOSE_MAKEFILE "true")
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++17 -g ${TORCH_CXX_FLAGS}")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall")
-list(APPEND CMAKE_PREFIX_PATH
-    "${CMAKE_SOURCE_DIR}/../libtorch"
-    /usr/local/cuda
-    )
-set(CUDA_TOOLKIT_ROOT_DIR "/usr/local/cuda")
-set(Torch_DIR "${CMAKE_SOURCE_DIR}/../libtorch/share/cmake/Torch")
-set(ENABLE_CUDA true)
-
-find_package(Torch REQUIRED)
-
-
-if(ENABLE_CUDA)
-  find_package(CUDA REQUIRED)
-  SET(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math)
-  set(CUDA_NVCC_FLAGS 
-    -gencode arch=compute_74,code=sm_74
-  )
-endif()
-
-find_package(Eigen3 REQUIRED)
-find_package(ompl REQUIRED)
-
-
-
-find_package(PCL REQUIRED)
-find_package(catkin REQUIRED COMPONENTS
-    roscpp
-    visualization_msgs
-    std_msgs
-    tools
-    cv_bridge
-)
-
-include_directories(
-    include
-    ${catkin_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-    ${TORCH_INCLUDE_DIRS}
-)
-
-
-#if this catkin packge's header is used by other packages, use catkin_package to
-#declare the include directories of this package.
-catkin_package(
-    INCLUDE_DIRS include 
-)
-
-add_library(kino_astar
-    src/kino_astar.cpp
-)
-target_link_libraries(kino_astar 
-    ompl
-    ${catkin_LIBRARIES}
-    ${TORCH_LIBRARIES}
-
-)
-add_library(nn_pathsearch
-    src/nnPath.cpp
-)
-target_link_libraries(nn_pathsearch 
-    ${catkin_LIBRARIES}
-   ${TORCH_LIBRARIES}
-   ompl
-)
-

NeuralTraj/src/path_search/include/path_searching/kino_astar.h

@@ -1,289 +0,0 @@
-#ifndef _KINODYNAMIC_ASTAR_H
-#define _KINODYNAMIC_ASTAR_H
-
-#include <Eigen/Eigen>
-#include <iostream>
-#include <map>
-#include <ros/console.h>
-#include <ros/ros.h>
-#include <string>
-#include <unordered_map>
-#include <boost/functional/hash.hpp>
-#include <queue>
-#include <math.h>
-#include <utility> 
-#include <torch/torch.h>
-#include <torch/script.h>
-#include <pcl/point_cloud.h>
-#include <ompl/base/spaces/ReedsSheppStateSpace.h>
-#include <ompl/base/spaces/DubinsStateSpace.h>
-#include <ompl/geometric/SimpleSetup.h>
-#include <tools/config.hpp>
-#include <tools/gridmap.hpp>
-#include "kino_model.hpp"
-using namespace torch::indexing;
-namespace path_searching{
-
-using namespace std;
-
-#define IN_CLOSE_SET 'a'
-#define IN_OPEN_SET 'b'
-#define NOT_EXPAND 'c'
-#define inf 1 >> 30
-
-
-  class PathNode {
-  public:
-    /* -------------------- */
-    Eigen::Vector2i index;
-    int yaw_idx;
-    /* --- the state is x y theta(orientation) */
-    Eigen::Vector3d state;
-    double g_score, f_score;
-    /* control input should be steer and arc */
-    Eigen::Vector2d input;
-    PathNode* parent;
-    char node_state;
-    int singul = 0;
-    /* -------------------- */
-    PathNode() {
-      parent = NULL;
-      node_state = NOT_EXPAND;
-    }
-    ~PathNode(){};
-  };
-  typedef PathNode* PathNodePtr;
-
-
-
-
-  class NodeComparator {
-  public:
-    template <class NodePtr>
-    bool operator()(NodePtr node1, NodePtr node2) {
-      return node1->f_score > node2->f_score;
-    }
-  };
-
-  template <typename T>
-  struct matrix_hash : std::unary_function<T, size_t> {
-    std::size_t operator()(T const& matrix) const {
-      size_t seed = 0;
-      for (long int i = 0; i < matrix.size(); ++i) {
-        auto elem = *(matrix.data() + i);
-        seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
-      }
-      return seed;
-    }
-  };
-  template <class NodePtr>
-  class NodeHashTable {
-  private:
-    /* data */
-
-    std::unordered_map<Eigen::Vector2i, NodePtr, matrix_hash<Eigen::Vector2i>> data_2d_;
-    std::unordered_map<Eigen::Vector3i, NodePtr, matrix_hash<Eigen::Vector3i>> data_3d_;
-
-
-
-  public:
-    NodeHashTable(/* args */) {
-    }
-    ~NodeHashTable() {
-    }
-    // : for 2d vehicle planning
-    void insert(Eigen::Vector2i idx, NodePtr node) {
-      data_2d_.insert(std::make_pair(idx, node));
-    }
-    //for 3d vehicle planning
-    void insert(Eigen::Vector2i idx, int yaw_idx, NodePtr node) {
-      data_3d_.insert(std::make_pair(Eigen::Vector3i(idx(0), idx(1), yaw_idx), node));
-    }
-    void insert(Eigen::Vector3i idx,NodePtr node ){
-      data_3d_.insert(std::make_pair(idx,node));
-    }
-
-    NodePtr find(Eigen::Vector2i idx) {
-      auto iter = data_2d_.find(idx);
-      return iter == data_2d_.end() ? NULL : iter->second;
-    }
-    NodePtr find(Eigen::Vector2i idx, int yaw_idx) {
-      auto iter = data_3d_.find(Eigen::Vector3i(idx(0), idx(1), yaw_idx));
-      return iter == data_3d_.end() ? NULL : iter->second;
-    }
-
-    void clear() {
-      data_2d_.clear();
-      data_3d_.clear();
-    }
-  };
-  // struct FlatTrajData
-  //   {
-  //     int singul;
-  //     std::vector<Eigen::Vector3d> traj_pts;      // 3, N  x,y dur
-  //     std::vector<double> thetas;
-  //     Eigen::MatrixXd start_state;   // start flat state (2, 3)
-  //     Eigen::MatrixXd final_state;   // end flat state (2, 3)
-
-  //   };
-  //   typedef std::vector<FlatTrajData> KinoTrajData;
- 
-  class KinoAstar {
-  
-  private:
-    /* ---------- main data structure ---------- */
-    vector<PathNodePtr> path_node_pool_;
-    int use_node_num_, iter_num_;
-    NodeHashTable<PathNodePtr> expanded_nodes_;
-    std::priority_queue<PathNodePtr, std::vector<PathNodePtr>, NodeComparator> open_set_;
-    std::vector<PathNodePtr> path_nodes_;
-
-    /* ---------- record data ---------- */
-    Eigen::Vector4d start_state_, end_state_;
-    Eigen::Vector2d start_ctrl;
-    bool is_shot_succ_ = false;
-    bool has_path_ = false;
-    /* ---------- parameter ---------- */
-    /* search */
-     double step_arc = 1.0;
-    double max_cur_ = 0.35;
-    double max_steer_ = 0.78539815;
-    double max_seach_time = 0.1;
-    double max_forward_vel = 4.0;
-    double max_forward_acc = 2.0;
-    double max_backward_vel = 4.0;
-    double max_backward_acc = 2.0;
-    double wheel_base = 0.4;
-    double steer_res;
-
-    double traj_forward_penalty = 1.0;
-    double traj_back_penalty = 2.0;
-    double traj_gear_switch_penalty = 15.0;
-    double traj_steer_penalty = 0.5;
-    double traj_steer_change_penalty = 0.0;
-    double horizon_;
-    double lambda_heu_;
-
-    int allocate_num_;
-    int check_num_;
-    double tie_breaker_ = 1.0 + 1.0 / 10000;
-    //network
-    torch::jit::script::Module mpnet_;
-    torch::Tensor env_tensor = torch::zeros({1,1,200,200});
-    bool use_half = true;
-    torch::Tensor context_map = torch::zeros({1,1,200,200});
-    torch::Tensor context_cos_map = torch::zeros({1,1,200,200});
-    torch::Tensor context_sin_map = torch::zeros({1,1,200,200});//reset
-    torch::Tensor positive, predClass;
-    double neuralBudget = 0.02;
-
-
-
-    /* map */
-    double resolution_, inv_resolution_, yaw_resolution_, inv_yaw_resolution_;
-    Eigen::Vector2d origin_, map_size_3d_;
-    double yaw_origin_ = -M_PI;
-    /*shot hzc*/
-    std::vector<double>  shot_timeList;
-    std::vector<double>  shot_lengthList;
-    std::vector<int>     shotindex;
-    std::vector<int>     shot_SList; 
-    std::vector<Eigen::Vector3d> SampleTraj;
-
-    /* helper */
-    Eigen::Vector2i posToIndex(Eigen::Vector2d pt);
-    int yawToIndex(double yaw);
-    void retrievePath(PathNodePtr end_node);
-
-
-    
-    void getFlatState(Eigen::Vector4d state, Eigen::Vector2d control_input,
-                      Eigen::MatrixXd &flat_state, int singul);
-
-    bool is_shot_sucess(Eigen::Vector3d state1,Eigen::Vector3d state2);
-    void computeShotTraj(Eigen::Vector3d &state1, Eigen::Vector3d &state2,
-                          std::vector<Eigen::Vector3d> &path_list,
-                          double& len); 
-    void stateTransit(Eigen::Vector3d &state0,  Eigen::Vector3d &state1,
-              Eigen::Vector2d &ctrl_input);
-
-    double evaluateLength(double curt,double locallength,double localtime, double max_vel, double max_acc, double startV = 0.0, double endV = 0.0);
-    double evaluateDuration(double length, double max_vel, double max_acc, double startV = 0.0, double endV = 0.0);
-
-    map_util::OccMapUtil* frontend_map_itf_;
-    double non_siguav;
-    ompl::base::StateSpacePtr shotptr;
-
-    inline int getSingularity(double vel)
-    {
-      int singul = 0;
-      if (fabs(vel) > 1e-2){
-        if (vel >= 0.0){singul = 1;} 
-        else{singul = -1;}      
-      }
-      
-      return singul;
-    }
-    // getHeu
-    inline double getHeu(Eigen::VectorXd x1, Eigen::VectorXd x2)
-    {
-      double dx = abs(x1(0) - x2(0));
-      double dy = abs(x1(1) - x2(1));
-      return tie_breaker_* sqrt(dx * dx + dy * dy);
-    }
-
-
-
-  public:
-    KinoAstar(){};
-    ~KinoAstar();
-    void warm();
-
-    enum { REACH_HORIZON = 1, REACH_END = 2,  NO_PATH = 3, REACH_END_BUT_SHOT_FAILS = 4};
-
-    /* main API */
-    void setParam(ros::NodeHandle& nh);
-    void init(ConfigPtr cfg_, ros::NodeHandle nh, bool un = false);
-
-    void reset();
-    int search(Eigen::Vector4d start_state, Eigen::Vector2d init_ctrl,
-              Eigen::Vector4d end_state,double& model_time, bool use3d = false);
-    // inital semantic map
-    void intialMap(map_util::OccMapUtil *map_itf);
-    // get kino traj for optimization  
-    void getKinoNode(KinoTrajData &flat_trajs);
-    void getKinoNode(KinoTrajData &flat_trajs, std::vector<Eigen::Vector3d> inputSamples);
-
-    void NodeVis(Eigen::Vector3d state);
-    void nnVis(torch::Tensor map);
-
-    /*hzchzc*/
-    Eigen::Vector3d evaluatePos(double t);
-    std::vector<Eigen::Vector4d> SamplePosList(int N); //px py yaw t 
-    std::vector<Eigen::Vector3d> getSampleTraj();
-    double totalTrajTime;
-    double checkl = 0.2;
-
-    ros::Publisher expandNodesVis, networkVis;
-    bool use_network = false;
-    std::vector<float> nm;
-    std::vector<Eigen::Vector3d> getRoughSamples();
-
-
-
-
-
-    std::vector<PathNodePtr> getVisitedNodes();
-
-    std::vector<Eigen::Vector4d>  state_list;
-    std::vector<Eigen::Vector3d> acc_list;
-
-    typedef shared_ptr<KinoAstar> KinoAstarPtr;
-
-
-  };
- 
-
-}  // namespace 
-
-#endif

NeuralTraj/src/path_search/include/path_searching/kino_model.hpp

@@ -1,32 +0,0 @@
-#ifndef _KINOMODEL_H
-#define _KINOMODEL_H
-#include <boost/array.hpp>
-#include <boost/bind.hpp>
-#include <iostream>
-
-#include <boost/array.hpp>
-#include <boost/bind.hpp>
-#include <iostream>
-
-
-#include <Eigen/Geometry>
-#include <Eigen/StdVector>
-
-
-namespace  path_searching{
-  
-    struct FlatTrajData
-    {
-      int singul;
-      std::vector<Eigen::Vector3d> traj_pts;      // 3, N  x,y dur
-      std::vector<double> thetas;
-      Eigen::MatrixXd start_state;   // start flat state (2, 3)
-      Eigen::MatrixXd final_state;   // end flat state (2, 3)
-      double duration;
-
-    };
-    typedef std::vector<FlatTrajData> KinoTrajData;
-
-
-}
-#endif

NeuralTraj/src/path_search/include/path_searching/nnPath.h

@@ -1,95 +0,0 @@
-#ifndef _NNPATH_H
-#define _NNPATH_H
-#include <string>
-#include <iostream>
-#include <sstream>
-#include <ros/ros.h>
-#include <tools/visualization.hpp>
-#include <torch/script.h>
-#include <tools/config.hpp>
-#include "kino_model.hpp"
-#include <tools/gridmap.hpp>
-#include <ompl/base/spaces/ReedsSheppStateSpace.h>
-#include <ompl/base/spaces/DubinsStateSpace.h>
-#include <ompl/geometric/SimpleSetup.h>
-using namespace torch::indexing;
-//3.4
-namespace path_searching{
-
-
-    class NnPathSearch {
-    private:
-        std::string model_path_;
-        torch::jit::script::Module mpnet_;
-        std::shared_ptr<visualization::Visualization> vis_tool_;
-        map_util::OccMapUtil* frontend_map_itf_;
-        torch::Tensor input;
-        int trajLen = 200;
-        torch::Tensor env_tensor = torch::zeros({1,1,200,200});
-        bool use_half = true;
-        torch::Tensor context_map = torch::zeros({1,1,200,200});
-        torch::Tensor context_cos_map = torch::zeros({1,1,200,200});
-        torch::Tensor context_sin_map = torch::zeros({1,1,200,200});//reset
-        torch::Tensor label_opState =  torch::zeros({1,trajLen,2});
-        torch::Tensor label_opRot =  torch::zeros({1,trajLen,2});
-        // torch::Tensor label_anchors =  torch::zeros({1,trajLen,25,25});
-        torch::Tensor label_anchors =  torch::zeros({1,trajLen,20,20});
-        // std::vector<torch::IValue> all_input;
-        torch::Tensor opState,opRot;
-        bool has_path_ = false;
-        std::vector<Eigen::Vector3d> sampleTraj;
-        std::vector<double>  shot_timeList;
-        std::vector<double>  shot_lengthList;
-        std::vector<int>     shotindex;
-        std::vector<int>     shot_SList;
-        double totalTrajTime; 
-
-        double max_forward_vel;
-        double max_forward_acc;
-        double max_backward_vel;
-        double max_backward_acc;
-        double max_cur;
-        bool enable_shot_ = true;
-        ompl::base::StateSpacePtr shotptr;
-
-
-    public:
-        NnPathSearch(){};
-        ~NnPathSearch(){};
-        void init(ConfigPtr config, ros::NodeHandle& nh);
-        void intialMap(map_util::OccMapUtil *map_itf);
-        void search(Eigen::Vector3d start_state, Eigen::Vector3d end_state);
-        void reset();
-        void display();
-        void warm();
-
-        void getKinoNode(KinoTrajData &flat_trajs);
-        double evaluateLength(double curt,double locallength,double localtime, double max_vel, double max_acc, double startV = 0.0, double endV = 0.0);
-        double evaluateDuration(double length, double max_vel, double max_acc, double startV = 0.0, double endV = 0.0);
-        Eigen::Vector3d evaluatePos(double t);
-        void reedshep_process(torch::Tensor& opState, torch::Tensor& opRot);
-
-
-        
-        // std::vector<Eigen::Vector4d> SamplePosList(int N); //px py yaw t 
-        // std::vector<Eigen::Vector3d> getSampleTraj();
-
-        // double totalTrajTime;
-        // double checkl = 0.2;
-
-        // std::vector<Eigen::Vector4d>  state_list;
-        // std::vector<Eigen::Vector3d> acc_list;
-
-        typedef std::shared_ptr<NnPathSearch> NnPathSearchPtr;
-        
-    };
-
-
-
-}
-
-
-
-
-
-#endif

NeuralTraj/src/path_search/package.xml

@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>path_search</name>
-
-  <maintainer email="zhichaohan@zju.edu.cn">zhichaohan</maintainer>
-
-  <version>0.1.0</version>
-  <description> the path_search package</description>
-  <license>GPLV3</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>roscpp</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>tools</build_depend>
-  
-  <run_depend>roscpp</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>visualization_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>tools</run_depend>
-
-
-
-</package>
-

NeuralTraj/src/path_search/src/nnPath.cpp

@@ -1,422 +0,0 @@
-#include <path_searching/nnPath.h>
-#include <tools/tic_toc.hpp>
-#include <torch/torch.h>
-#include "torch/script.h"
-#include <opencv2/opencv.hpp>
-#include <opencv2/core/core.hpp>
-#include <sensor_msgs/PointCloud2.h>
-#include <fstream>
-using namespace cv;
-namespace path_searching{
-    void NnPathSearch::init(ConfigPtr config, ros::NodeHandle& nh){
-        vis_tool_.reset(new visualization::Visualization(nh));
-        vis_tool_->registe<visualization_msgs::MarkerArray>("/visualization/nnarrowTraj");
-        vis_tool_->registe<nav_msgs::Path>("/visualization/nnPath");
-        model_path_ = config->model_path;
-        std::cout << "Check1" << std::endl;
-        mpnet_ = torch::jit::load(model_path_,torch::kCUDA);
-        std::cout << "Check2" << std::endl;
-        if(use_half){
-            mpnet_.to(torch::kHalf);
-        }
-        mpnet_.eval();
-        torch::NoGradGuard no_grad_;
-        enable_shot_ = config->enable_shot;
-        //warm start
-
-        for(int i = 0; i < 30; i++){
-           warm();
-        }
-        std::cout << "neural network finish warm start" << std::endl;
-
-        max_forward_vel = config->vmax-0.2;
-        // max_forward_vel = 1.01;//hzc?
-        max_forward_acc = config->lonAccmax;
-        max_backward_vel = max_forward_vel;
-        max_backward_acc = max_forward_acc;
-        max_cur = config->kmax-0.3;
-        shotptr =std::make_shared<ompl::base::ReedsSheppStateSpace>(1.0/max_cur);
-    }
-    void NnPathSearch::warm(){
-        std::vector<torch::IValue> ws_inputs;
-        torch::Tensor ws0 = torch::zeros({1,4,200,200});
-        torch::Tensor ws1 = torch::zeros({1,trajLen,2});
-        torch::Tensor ws2 = torch::zeros({1,trajLen,2});        
-        torch::Tensor ws3 = torch::zeros({1,trajLen,20,20});
-        // torch::Tensor ws3 = torch::zeros({1,trajLen,25,25});
-        if(use_half){
-            ws_inputs.push_back(ws0.to(at::kCUDA).to(torch::kHalf));
-            ws_inputs.push_back(ws1.to(at::kCUDA).to(torch::kHalf));
-            ws_inputs.push_back(ws2.to(at::kCUDA).to(torch::kHalf));
-            ws_inputs.push_back(ws3.to(at::kCUDA).to(torch::kHalf));    
-        }
-        else{
-            ws_inputs.push_back(ws0.to(at::kCUDA));
-            ws_inputs.push_back(ws1.to(at::kCUDA));
-            ws_inputs.push_back(ws2.to(at::kCUDA));
-            ws_inputs.push_back(ws3.to(at::kCUDA));
-        }
-        auto outputs = mpnet_.forward(ws_inputs).toTuple();
-        torch::Tensor r1 = outputs->elements()[0].toTensor().to(at::kCPU).squeeze();
-        torch::Tensor r2 = outputs->elements()[1].toTensor().to(at::kCPU).squeeze();
-        return;
-    }
-    void NnPathSearch::intialMap(map_util::OccMapUtil *map_itf){
-        frontend_map_itf_ = map_itf;
-        Eigen::Vector2i dims = frontend_map_itf_->getDim();
-        double startT = ros::Time::now().toSec();
-        // for(int i = 0; i < dims[0]; i++){
-        //     for(int j = 0; j < dims[1]; j++){
-        //         env_tensor[0][0][i][j] =  frontend_map_itf_->getDistance(Eigen::Vector2i(i,j));
-        //     }
-        // }
-
-        env_tensor = torch::from_blob(frontend_map_itf_->getEsdfData(), {1,1,200,200}, torch::kFloat64);
-        env_tensor = torch::transpose(env_tensor, 2, 3);
-        // std::cout <<"env_tensor.sizes: "<<env_tensor.sizes()<<"\n";
-        // std::cout <<"esdf: "<<frontend_map_itf_->getDistance(Eigen::Vector2i(50,160))<<"\n";
-        // std::cout <<"env_tensor: "<<env_tensor[0][0][50][160]<<"\n";
-        double endT = ros::Time::now().toSec();
-        // ROS_WARN_STREAM("map T: "<<1000.0*(endT-startT) << " ms");
-
-
-
-    }
-    void NnPathSearch::search(Eigen::Vector3d start_state, Eigen::Vector3d end_state){
-
-        torch::NoGradGuard no_grad_;
-        int receptive_field = 16;
-        Eigen::Vector2i goalpos, startpos;
-        startpos = frontend_map_itf_->floatToInt(start_state.head(2));
-        goalpos = frontend_map_itf_->floatToInt(end_state.head(2));
-
-        int goal_start_x = std::max(0, goalpos[0]- receptive_field/2);
-        int goal_start_y = std::max(0, goalpos[1]- receptive_field/2);
-        int goal_end_x = std::min(200, goalpos[0]+ receptive_field/2);
-        int goal_end_y = std::min(200, goalpos[1]+ receptive_field/2);
-
-        context_map.index_put_({Slice(None), Slice(None), Slice(goal_start_x,goal_end_x),Slice(goal_start_y, goal_end_y)},1);
-        context_cos_map.index_put_({Slice(None), Slice(None), Slice(goal_start_x,goal_end_x), Slice(goal_start_y,goal_end_y)}, cos(end_state[2]));
-        context_sin_map.index_put_({Slice(None), Slice(None), Slice(goal_start_x,goal_end_x), Slice(goal_start_y,goal_end_y)}, sin(end_state[2]));
-
-
-        int start_start_x = std::max(0, startpos[0]- receptive_field/2);
-        int start_start_y = std::max(0, startpos[1]- receptive_field/2);
-        int start_end_x = std::min(200, startpos[0]+ receptive_field/2);
-        int start_end_y = std::min(200, startpos[1]+ receptive_field/2);
-
-        context_map.index_put_({Slice(None), Slice(None), Slice(start_start_x,start_end_x),Slice(start_start_y, start_end_y)},-1);
-        context_cos_map.index_put_({Slice(None), Slice(None), Slice(start_start_x,start_end_x), Slice(start_start_y,start_end_y)}, cos(start_state[2]));
-        context_sin_map.index_put_({Slice(None), Slice(None), Slice(start_start_x,start_end_x), Slice(start_start_y,start_end_y)}, sin(start_state[2]));
-
-
-
-
-
-
-        input = torch::cat({env_tensor,context_map,context_cos_map,context_sin_map },1);
-
-
-        label_opState[0][0][0] = start_state[0];
-        label_opState[0][0][1] = start_state[1];
-        label_opState[0][trajLen-1][0] = end_state[0];
-        label_opState[0][trajLen-1][1] = end_state[1];
-        label_opRot[0][0][0] = cos(start_state[2]);
-        label_opRot[0][0][1] = sin(start_state[2]);
-        label_opRot[0][trajLen-1][0] = cos(end_state[2]);
-        label_opRot[0][trajLen-1][1] = sin(end_state[2]);
-        {
-            // int sgx = floor((start_state[0]+10.0)/0.8);
-            // int sgy = floor((start_state[1]+10.0)/0.8);
-            // int egx = floor((end_state[0]+10.0)/0.8);
-            // int egy = floor((end_state[1]+10.0)/0.8);
-            int sgx = floor((start_state[0]+10.0)/1.0);
-            int sgy = floor((start_state[1]+10.0)/1.0);
-            int egx = floor((end_state[0]+10.0)/1.0);
-            int egy = floor((end_state[1]+10.0)/1.0);
-            label_anchors[0][0][sgx][sgy] = 1;
-            label_anchors[0][trajLen-1][egx][egy] = 1;
-        }
-        
-                
-        std::vector<torch::IValue> all_input;
-
-
-        if(use_half){
-            all_input.push_back(input.to(at::kCUDA).to(torch::kHalf));
-            all_input.push_back(label_opState.to(at::kCUDA).to(torch::kHalf));
-            all_input.push_back(label_opRot.to(at::kCUDA).to(torch::kHalf));
-            all_input.push_back(label_anchors.to(at::kCUDA).to(torch::kHalf));
-        }
-        else{
-            all_input.push_back(input.to(at::kCUDA));   
-            all_input.push_back(label_opState.to(at::kCUDA));
-            all_input.push_back(label_opRot.to(at::kCUDA));
-            all_input.push_back(label_anchors.to(at::kCUDA));
-        }
-        auto outputs = mpnet_.forward(all_input).toTuple();
-        opState = outputs->elements()[0].toTensor().to(at::kCPU).squeeze();//100*2
-        opRot = outputs->elements()[1].toTensor().to(at::kCPU).squeeze();//100*2
-        has_path_ = true;
-        if(enable_shot_){
-            // double t1 = ros::Time::now().toSec();
-            reedshep_process(opState, opRot);
-            // double t2 = ros::Time::now().toSec();
-            // std::cout << "rs time: "<<(t2-t1)*1000.0 << " ms \n";
-        }
-
-        return;
-    }
-    void NnPathSearch::reedshep_process(torch::Tensor& opState, torch::Tensor& opRot){
-        int startIdx = 0.70 * trajLen;
-        namespace ob = ompl::base;
-        namespace og = ompl::geometric;
-        ob::ScopedState<> from(shotptr), to(shotptr), s(shotptr);
-        std::vector<double> reals;
-        to[0] = opState[trajLen-1][0].item<double>();
-        to[1] = opState[trajLen-1][1].item<double>();
-        to[2] = atan2(opRot[trajLen-1][1].item<double>(), opRot[trajLen-1][0].item<double>());
-        for(unsigned int i = startIdx; i < trajLen - 1; i++){
-            from[0] = opState[i-1][0].item<double>();
-            from[1] = opState[i-1][1].item<double>();
-            from[2] = atan2(opRot[i-1][1].item<double>(), opRot[i-1][0].item<double>()); 
-            double len = shotptr->distance(from(), to()); 
-            bool isocc =false;
-            std::vector<Eigen::Vector3d> shot_path; 
-            for (double l = 0.0; l <=len; l += frontend_map_itf_->getRes()*2.0)
-            {
-                shotptr->interpolate(from(), to(), l/len, s());
-                reals = s.reals();
-                frontend_map_itf_->CheckIfCollisionUsingPosAndYaw(Eigen::Vector3d(reals[0], reals[1], reals[2]), &isocc, 0.0);
-                if(isocc)
-                    break;
-                shot_path.emplace_back(reals[0], reals[1], reals[2]);
-            }
-            shot_path.emplace_back(to[0], to[1], to[2]);            
-            if(isocc==false){
-                //collision-free
-                torch::Tensor newOpState = torch::zeros({i+shot_path.size(),2});
-                torch::Tensor newOpRot = torch::zeros({i+shot_path.size(),2});
-                newOpState.index({Slice(0,i),Slice(None)}) = opState.index({Slice(0,i),Slice(None)});
-                newOpRot.index({Slice(0,i),Slice(None)}) = opRot.index({Slice(0,i),Slice(None)});
-                for(int j = 0; j < shot_path.size(); j++){
-                    newOpState[i+j][0] = shot_path[j][0];
-                    newOpState[i+j][1] = shot_path[j][1];
-                    newOpRot[i+j][0] = cos(shot_path[j][2]);
-                    newOpRot[i+j][1] = sin(shot_path[j][2]);
-                }
-                opState = newOpState;
-                opRot = newOpRot;
-                // ROS_WARN_STREAM("neural shot successfully i: "<<i);
-                return;
-            }
-
-        }
-
-        return;
-    }
-    void NnPathSearch::reset(){
-        context_map = torch::zeros({1,1,200,200});
-        context_cos_map = torch::zeros({1,1,200,200});
-        context_sin_map = torch::zeros({1,1,200,200});//reset
-        label_opState =  torch::zeros({1,trajLen,2});
-        label_opRot =  torch::zeros({1,trajLen,2});
-        label_anchors =  torch::zeros({1,trajLen,20,20});
-        // label_anchors =  torch::zeros({1,trajLen,25,25});
-        has_path_ = false;
-        return;
-    }
-    void NnPathSearch::display(){
-        if(has_path_){
-            std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> nn_arrows;
-            std::vector<Eigen::Vector3d> nn_points;
-            for(int i = 0; i < opState.sizes()[0]; i++){
-                double px = opState[i][0].item<double>();
-                double py = opState[i][1].item<double>();
-                Eigen::Vector2d pos1, pos2;
-                pos1 << px, py;
-                pos2 =  pos1 + 0.30*Eigen::Vector2d(opRot[i][0].item<double>(), opRot[i][1].item<double>());
-                nn_arrows.push_back(std::pair<Eigen::Vector2d, Eigen::Vector2d>(pos1, pos2));
-                nn_points.push_back(Eigen::Vector3d(px, py, 0.2));
-            }
-            vis_tool_->visualize_arrows(nn_arrows, "/visualization/nnarrowTraj", visualization::green);
-            vis_tool_->visualize_path(nn_points, "/visualization/nnPath");
-            
-
-        }
-        return;
-    }
-    void NnPathSearch::getKinoNode(KinoTrajData &flat_trajs){
-        flat_trajs.clear();
-        sampleTraj.clear();
-        for(int i = 0; i < opState.sizes()[0]; i++){
-            double px = opState[i][0].item<double>();
-            double py = opState[i][1].item<double>();
-            double c = opRot[i][0].item<double>();
-            double s = opRot[i][1].item<double>();
-            double yaw = atan2(s, c);
-            sampleTraj.emplace_back(px,py,yaw);
-        }
-        std::vector<Eigen::Vector3d> traj_pts;  // 3, N
-        std::vector<double> thetas;
-        /*divide the whole shot traj into different segments*/   
-        shot_lengthList.clear();
-        shot_timeList.clear();
-        shotindex.clear();
-        shot_SList.clear(); 
-        int lastS = (sampleTraj[1]-sampleTraj[0]).head(2).dot(Eigen::Vector2d(cos(sampleTraj[1][2]),sin(sampleTraj[1][2])))>=0?1:-1;
-        shotindex.push_back(0);
-        double tmpl = 0.0;
-        //hzchzc attention please!!! max_v must = min_v  max_acc must = min_acc
-        for(int i = 0; i<sampleTraj.size()-1; i++){
-            Eigen::Vector3d state1 = sampleTraj[i];
-            Eigen::Vector3d state2 = sampleTraj[i+1];
-            int curS = (state2-state1).head(2).dot(Eigen::Vector2d(cos(state2[2]),sin(state2[2]))) >=0 ? 1:-1;
-            if(curS*lastS >= 0){
-                tmpl += (state2-state1).head(2).norm();
-            }
-            else{  
-                shotindex.push_back(i);
-                shot_SList.push_back(lastS);
-                shot_lengthList.push_back(tmpl);
-                if(lastS>0)
-                shot_timeList.push_back(evaluateDuration(tmpl,max_forward_vel, max_forward_acc));
-                else
-                shot_timeList.push_back(evaluateDuration(tmpl,max_backward_vel, max_backward_acc));
-                tmpl = (state2-state1).head(2).norm();
-            }       
-            lastS = curS;
-        }
-        shot_SList.push_back(lastS);
-        shot_lengthList.push_back(tmpl);
-        if(lastS>0)
-            shot_timeList.push_back(evaluateDuration(tmpl,max_forward_vel, max_forward_acc));
-        else
-            shot_timeList.push_back(evaluateDuration(tmpl,max_backward_vel, max_backward_acc));
-        shotindex.push_back(sampleTraj.size()-1);
-
-        /*extract flat traj  
-        the flat traj include the end point but not the first point*/
-        for(int i=0;i<shot_lengthList.size();i++){
-            // double locallength = shot_lengthList[i];
-            int sig = shot_SList[i];
-            std::vector<Eigen::Vector3d> localTraj;localTraj.assign(sampleTraj.begin()+shotindex[i],sampleTraj.begin()+shotindex[i+1]+1);
-            traj_pts.clear();
-            thetas.clear();        
-            for(const auto pt:localTraj){
-                traj_pts.emplace_back(pt[0],pt[1],0.0);
-                thetas.push_back(pt[2]);
-            }
-            FlatTrajData flat_traj;
-            flat_traj.traj_pts = traj_pts;
-            flat_traj.thetas = thetas;
-            flat_traj.singul = sig;
-            flat_traj.duration = shot_timeList[i];
-            flat_trajs.push_back(flat_traj);
-        }
-        totalTrajTime = 0.0;
-        for(const auto dt : shot_timeList){
-            totalTrajTime += dt; 
-        }
-    }
-    double NnPathSearch::evaluateDuration(double length, double max_vel, double max_acc, double startV, double endV){
-        double critical_len; //the critical length of two-order optimal control, acc is the input
-        double startv2 = pow(startV,2);
-        double endv2 = pow(endV,2);
-        double maxv2 = pow(max_vel,2);
-        critical_len = (maxv2-startv2)/(2*max_acc)+(maxv2-endv2)/(2*max_acc);
-        if(length>=critical_len){
-            return (max_vel-startV)/max_acc+(max_vel-endV)/max_acc+(length-critical_len)/max_vel;
-        }
-        else{
-            double tmpv = sqrt(0.5*(startv2+endv2+2*max_acc*length));
-            return (tmpv-startV)/max_acc + (tmpv-endV)/max_acc;
-        }
-    }
-
-    Eigen::Vector3d NnPathSearch::evaluatePos(double t){
-        t = std::min<double>(std::max<double>(0,t),totalTrajTime);
-
-        int index = -1;
-        double tmpT = 0;
-        double CutTime;
-        //locate the local traj
-        for(int i = 0;i<shot_timeList.size();i++){
-        tmpT+=shot_timeList[i];
-        if(tmpT>=t) {
-            index = i; 
-            CutTime = t-tmpT+shot_timeList[i];
-            break;
-            }
-        }
-        double localtime = shot_timeList[index];
-        double locallength = shot_lengthList[index];
-        int front = shotindex[index]; int back =  shotindex[index+1];
-        std::vector<Eigen::Vector3d> localTraj;localTraj.assign(sampleTraj.begin()+front,sampleTraj.begin()+back+1);
-        //find the nearest point
-        double arclength;
-        if(shot_SList[index] > 0)
-            arclength= evaluateLength(CutTime,locallength,localtime,max_forward_vel,max_forward_acc);
-        else
-            arclength= evaluateLength(CutTime,locallength,localtime,max_backward_vel, max_backward_acc);
-
-        double tmparc = 0;
-        for(int i = 0; i < localTraj.size()-1;i++){
-            tmparc += (localTraj[i+1]-localTraj[i]).head(2).norm();
-            if(tmparc>=arclength){
-                double l1 = tmparc-arclength;
-                double l = (localTraj[i+1]-localTraj[i]).head(2).norm();
-                double l2 = l-l1;//l2
-                Eigen::Vector3d state = l1/l*localTraj[i]+l2/l*localTraj[i+1];
-                if(fabs(localTraj[i+1][2]-localTraj[i][2])>=M_PI){   
-                double normalize_yaw;
-                if(localTraj[i+1][2]<=0){
-                    normalize_yaw = l1/l*localTraj[i][2]+l2/l*(localTraj[i+1][2]+2*M_PI);
-                }
-                else if(localTraj[i][2]<=0){
-                    normalize_yaw = l1/l*(localTraj[i][2]+2*M_PI)+l2/l*localTraj[i+1][2];
-                }
-                state[2] = normalize_yaw;
-                }
-                return state;
-            }
-        }
-        return localTraj.back();
-    }
-
-    double NnPathSearch::evaluateLength(double curt,double locallength,double localtime,double max_vel, double max_acc, double startV, double endV){
-        double critical_len; //the critical length of two-order optimal control, acc is the input
-        if(startV>max_vel||endV>max_vel){
-        ROS_ERROR("kinoAstar:evaluateLength:start or end vel is larger that the limit!");
-        }
-        double startv2 = pow(startV,2);
-        double endv2 = pow(endV,2);
-        double maxv2 = pow(max_vel,2);
-        critical_len = (maxv2-startv2)/(2*max_acc)+(maxv2-endv2)/(2*max_acc);
-        if(locallength>=critical_len){
-        double t1 = (max_vel-startV)/max_acc;
-        double t2 = t1+(locallength-critical_len)/max_vel;
-        if(curt<=t1){
-            return startV*curt + 0.5*max_acc*pow(curt,2);
-        }
-        else if(curt<=t2){
-            return startV*t1 + 0.5*max_acc*pow(t1,2)+(curt-t1)*max_vel;
-        }
-        else{
-            return startV*t1 + 0.5*max_acc*pow(t1,2) + (t2-t1)*max_vel + max_vel*(curt-t2)-0.5*max_acc*pow(curt-t2,2);
-        }
-        }
-        else{
-        double tmpv = sqrt(0.5*(startv2+endv2+2*max_acc*locallength));
-        double tmpt = (tmpv-startV)/max_acc;
-        if(curt<=tmpt){
-            return startV*curt+0.5*max_acc*pow(curt,2);
-        }
-        else{
-            return startV*tmpt+0.5*max_acc*pow(tmpt,2) + tmpv*(curt-tmpt)-0.5*max_acc*pow(curt-tmpt,2);
-        }
-        }
-    }
-
-
-}

NeuralTraj/src/plan_manage/CMakeLists.txt

@@ -1,71 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(plan_manage)
-
-set(CMAKE_VERBOSE_MAKEFILE "true")
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++17 -g ${TORCH_CXX_FLAGS}")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall" )
-list(APPEND CMAKE_PREFIX_PATH
-    "${CMAKE_SOURCE_DIR}/../libtorch"
-    /usr/local/cuda
-    )
-set(CUDA_TOOLKIT_ROOT_DIR "/usr/local/cuda")
-set(Torch_DIR "${CMAKE_SOURCE_DIR}/../libtorch/share/cmake/Torch")
-set(ENABLE_CUDA true)
-
-find_package(Torch REQUIRED)
-
-if(ENABLE_CUDA)
-  find_package(CUDA REQUIRED)
-  SET(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math)
-  set(CUDA_NVCC_FLAGS 
-    -gencode arch=compute_74,code=sm_74
-  )
-endif()
-
-
-find_package(Eigen3 REQUIRED)
-
-find_package(PCL REQUIRED)
-find_package(catkin REQUIRED COMPONENTS
-    roscpp
-    visualization_msgs
-    std_msgs
-    tools
-    path_search
-    cv_bridge
-    decomp_ros_utils
-)
-
-include_directories(
-    include
-    ${catkin_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-    ${TORCH_INCLUDE_DIRS}
-)
-
-
-#if this catkin packge's header is used by other packages, use catkin_package to
-#declare the include directories of this package.
-catkin_package(
-    INCLUDE_DIRS include 
-)
-add_library(plan_manage
-    src/plan_manage.cpp
-)
-target_link_libraries(plan_manage 
-    kino_astar
-    nn_pathsearch
-    ${TORCH_LIBRARIES}
-    ${catkin_LIBRARIES}
-)
-add_executable(plan_node
-    src/plan_node.cpp
-)
-target_link_libraries(plan_node 
-    plan_manage
-    ${catkin_LIBRARIES}
-    ${PCL_LIBRARIES}
-)

NeuralTraj/src/plan_manage/config/planning_forest.yaml

@@ -1,68 +0,0 @@
-# trajectory Optimizer
-mini_T:                   0.05
-wei_time_:                10.0
-
-kmax:                     2.0 #2.0+0.1 1.5
-kdotmax:                  10000.0
-cons_eps_:                0.1
-
-
-
-vmax:                     2.0
-latAccmax:                1.5
-lonAccmax:                1.5
-accRatemax:               1.5
-phimax: 0.9
-omegamax: 100000.0 #0.5-0.02
-
-
-
-traj_res:                 8
-past:                     3
-rho:                      1.0
-rho_max:                  1000.0
-gamma:                    1.0
-mem_size:                 256
-g_epsilon:                1.0e-3
-min_step:                 1.0e-32
-delta:                    1.0e-3
-max_iterations:           1000
-amlMaxIter:               20
-
-penaWei:                  1000.0
-esdfWei:                  10000.0
-
-pieceTime:                1.0
-miniS:                    0.1
-safeMargin:               0.3 #0.15+0.05
-conpts:                   [[0.15, 0.0], [0.3, -0.00]]
-# 
-
-
-#map
-mapRes:                   0.1
-mapX:                     20.0
-mapY:                     20.0
-expandSize:               0 
-wheel_base:               0.6
-horizon_:                 50.0
-#res res res
-lambda_heu_:              2.0
-yaw_resolution_:          1.57
-steer_res:                0.5
-allocate_num_:            1000000
-
-
-
-check_num_:               8
-step_arc:                 0.3
-checkl:                   0.1
-
-max_seach_time:           1000.0
-non_siguav:               0.0
-backW:                    2.0
-useDP:                    true
-isdebug:                  false
-isfixGear:                false
-isVis:                    false
-enable_shot:              true

NeuralTraj/src/plan_manage/config/planning_office.yaml

@@ -1,70 +0,0 @@
-# trajectory Optimizer
-mini_T:                   0.05
-wei_time_:                10.0
-
-kmax:                     2.0 #2.0+0.1 1.5
-kdotmax:                  10000.0
-cons_eps_:                0.1
-
-
-
-vmax:                     2.0
-latAccmax:                1.5
-lonAccmax:                1.5
-accRatemax:               1.5
-phimax: 0.9
-omegamax: 100000.0 #0.5-0.02
-
-
-
-traj_res:                 8
-past:                     3
-rho:                      1.0
-rho_max:                  1000.0
-gamma:                    1.0
-mem_size:                 256
-g_epsilon:                1.0e-3
-min_step:                 1.0e-32
-delta:                    1.0e-3
-max_iterations:           1000
-amlMaxIter:               20
-
-penaWei:                  1000.0
-esdfWei:                  10000.0
-
-pieceTime:                1.0
-miniS:                    0.1
-# safeMargin:               0.25 #0.15+0.05
-# conpts:                   [[0.15, 0.0], [0.3, -0.00]]
-safeMargin:               0.27 #0.15+0.05
-conpts:                   [[0.15, 0.0], [0.3, -0.00]]
-# 
-
-
-#map
-mapRes:                   0.1
-mapX:                     20.0
-mapY:                     20.0
-expandSize:               0 
-wheel_base:               0.6
-horizon_:                 50.0
-#res res res
-lambda_heu_:              2.0
-
-
-yaw_resolution_:          1.57
-steer_res:                0.5
-allocate_num_:            1000000
-
-check_num_:               4
-step_arc:                 0.3
-checkl:                   0.1
-
-max_seach_time:           1000.0
-non_siguav:               0.0
-backW:                    2.0
-useDP:                    true
-isdebug:                  false
-isfixGear:                false
-isVis:                    false
-enable_shot:              true

NeuralTraj/src/plan_manage/config/planning_ruin.yaml

@@ -1,68 +0,0 @@
-# trajectory Optimizer
-mini_T:                   0.05
-wei_time_:                10.0
-
-kmax:                     2.0 #2.0+0.1 1.5
-kdotmax:                  10000.0
-cons_eps_:                0.1
-
-
-
-vmax:                     2.0
-latAccmax:                1.5
-lonAccmax:                1.5
-accRatemax:               1.5
-phimax: 0.9
-omegamax: 100000.0 #0.5-0.02
-
-
-
-traj_res:                 8
-past:                     3
-rho:                      1.0
-rho_max:                  1000.0
-gamma:                    1.0
-mem_size:                 256
-g_epsilon:                1.0e-3
-min_step:                 1.0e-32
-delta:                    1.0e-3
-max_iterations:           1000
-amlMaxIter:               20
-
-penaWei:                  1000.0
-esdfWei:                  10000.0
-
-pieceTime:                1.0
-miniS:                    0.1
-safeMargin:               0.3 #0.15+0.05
-conpts:                   [[0.15, 0.0], [0.3, -0.00]]
-# 
-
-
-#map
-mapRes:                   0.1
-mapX:                     20.0
-mapY:                     20.0
-expandSize:               0 
-wheel_base:               0.6
-horizon_:                 50.0
-#res res res
-lambda_heu_:              2.0
-
-
-yaw_resolution_:          1.57
-steer_res:                0.5
-allocate_num_:            1000000
-
-check_num_:               4
-step_arc:                 0.3
-checkl:                   0.1
-
-max_seach_time:           1000.0
-non_siguav:               0.0
-backW:                    2.0
-useDP:                    true
-isdebug:                  false
-isfixGear:                false
-isVis:                    false
-enable_shot:              true

NeuralTraj/src/plan_manage/include/arcPlan/geoutils2d.hpp

@@ -1,129 +0,0 @@
-#ifndef GEOUTILS_HPP
-#define GEOUTILS_HPP
-
-
-#include "sdlp.hpp"
-#include "quickhull.hpp"
-#include <Eigen/Eigen>
-
-#include <cfloat>
-#include <cstdint>
-#include <set>
-#include <chrono>
-
-namespace geoutils
-{
-
-// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
-// The outter_normal is assumed to be NORMALIZED
-inline bool findInterior(const Eigen::MatrixXd &hPoly,
-                         Eigen::Vector3d &interior)
-{
-    int m = hPoly.cols();
-
-    Eigen::MatrixXd A(m, 4);
-    Eigen::VectorXd b(m), c(4), x(4);
-    A.leftCols<3>() = hPoly.topRows<3>().transpose();
-    A.rightCols<1>().setConstant(1.0);
-    b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum().transpose();
-    c.setZero();
-    c(3) = -1.0;
-
-    double minmaxsd = sdlp::linprog(c, A, b, x);
-    interior = x.head<3>();
-
-    return minmaxsd < 0.0 && !std::isinf(minmaxsd);
-}
-
-struct filterLess
-{
-    inline bool operator()(const Eigen::Vector3d &l,
-                           const Eigen::Vector3d &r) const
-    {
-        return l(0) < r(0) ||
-               (l(0) == r(0) &&
-                (l(1) < r(1) ||
-                 (l(1) == r(1) &&
-                  l(2) < r(2))));
-    }
-};
-
-inline void filterVs(const Eigen::MatrixXd &rV,
-                     const double &epsilon,
-                     Eigen::MatrixXd &fV)
-{
-    double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff()));
-    double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON);
-    std::set<Eigen::Vector3d, filterLess> filter;
-    fV = rV;
-    int offset = 0;
-    Eigen::Vector3d quanti;
-    for (int i = 0; i < rV.cols(); i++)
-    {
-        quanti = (rV.col(i) / res).array().round();
-        if (filter.find(quanti) == filter.end())
-        {
-            filter.insert(quanti);
-            fV.col(offset) = rV.col(i);
-            offset++;
-        }
-    }
-    fV = fV.leftCols(offset).eval();
-    return;
-}
-
-// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
-// The outter_normal is assumed to be NORMALIZED
-// proposed epsilon is 1.0e-6
-inline void enumerateVs(const Eigen::MatrixXd &hPoly,
-                        const Eigen::Vector3d &inner,
-                        Eigen::MatrixXd &vPoly,
-                        const double epsilon = 1.0e-6)
-{
-    Eigen::RowVectorXd b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum() -
-                           inner.transpose() * hPoly.topRows<3>();
-    Eigen::MatrixXd A = hPoly.topRows<3>().array().rowwise() / b.array();
-
-    quickhull::QuickHull<double> qh;
-    double qhullEps = std::min(epsilon, quickhull::defaultEps<double>());
-    // CCW is false because the normal in quickhull towards interior
-    const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps);
-    const auto &idBuffer = cvxHull.getIndexBuffer();
-    int hNum = idBuffer.size() / 3;
-    Eigen::MatrixXd rV(3, hNum);
-    Eigen::Vector3d normal, point, edge0, edge1;
-    for (int i = 0; i < hNum; i++)
-    {
-        point = A.col(idBuffer[3 * i + 1]);
-        edge0 = point - A.col(idBuffer[3 * i]);
-        edge1 = A.col(idBuffer[3 * i + 2]) - point;
-        normal = edge0.cross(edge1); //cross in CW gives an outter normal
-        rV.col(i) = normal / normal.dot(point);
-    }
-    filterVs(rV, epsilon, vPoly);
-    vPoly = (vPoly.array().colwise() + inner.array()).eval();
-    return;
-}
-
-// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
-// The outter_normal is assumed to be NORMALIZED
-// proposed epsilon is 1.0e-6
-inline bool enumerateVs(const Eigen::MatrixXd &hPoly,
-                        Eigen::MatrixXd &vPoly,
-                        const double epsilon = 1.0e-6)
-{
-    Eigen::Vector3d inner;
-    if (findInterior(hPoly, inner))
-    {
-        enumerateVs(hPoly, inner, vPoly, epsilon);
-        return true;
-    }
-    else
-    {
-        return false;
-    }
-}
-
-} // namespace geoutils
-
-#endif

NeuralTraj/src/plan_manage/include/arcPlan/lbfgs.hpp

@@ -1,838 +0,0 @@
-#ifndef LBFGS_HPP
-#define LBFGS_HPP
-
-#include <Eigen/Eigen>
-#include <cmath>
-#include <algorithm>
-
-namespace lbfgs
-{
-    // ----------------------- Data Type Part -----------------------
-
-    /**
-     * L-BFGS optimization parameters.
-     */
-    struct lbfgs_parameter_t
-    {
-        /**
-         * The number of corrections to approximate the inverse hessian matrix.
-         *  The L-BFGS routine stores the computation results of previous m
-         *  iterations to approximate the inverse hessian matrix of the current
-         *  iteration. This parameter controls the size of the limited memories
-         *  (corrections). The default value is 8. Values less than 3 are
-         *  not recommended. Large values will result in excessive computing time.
-         */
-        int mem_size = 8;
-
-        /**
-         * Epsilon for grad convergence test. DO NOT USE IT in nonsmooth cases! 
-         *  Set it to 0.0 and use past-delta-based test for nonsmooth functions.
-         *  This parameter determines the accuracy with which the solution is to
-         *  be found. A minimization terminates when
-         *      ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon,
-         *  where ||.||_inf is the infinity norm. The default value is 1.0e-5. 
-         *  This should be greater than 1.0e-6 in practice because L-BFGS does 
-         *  not directly reduce first-order residual. It still needs the function 
-         *  value which can be corrupted by machine_prec when ||g|| is small.
-         */
-        double g_epsilon = 1.0e-5;
-
-        /**
-         * Distance for delta-based convergence test.
-         *  This parameter determines the distance, in iterations, to compute
-         *  the rate of decrease of the cost function. If the value of this
-         *  parameter is zero, the library does not perform the delta-based
-         *  convergence test. The default value is 3.
-         */
-        int past = 3;
-
-        /**
-         * Delta for convergence test.
-         *  This parameter determines the minimum rate of decrease of the
-         *  cost function. The library stops iterations when the following 
-         *  condition is met:
-         *      |f' - f| / max(1, |f|) < delta,
-         *  where f' is the cost value of past iterations ago, and f is
-         *  the cost value of the current iteration.
-         *  The default value is 1.0e-6.
-         */
-        double delta = 1.0e-6;
-
-        /**
-         * The maximum number of iterations.
-         *  The lbfgs_optimize() function terminates an minimization process with
-         *  ::LBFGSERR_MAXIMUMITERATION status code when the iteration count
-         *  exceedes this parameter. Setting this parameter to zero continues an
-         *  minimization process until a convergence or error. The default value
-         *  is 0.
-         */
-        int max_iterations = 0;
-
-        /**
-         * The maximum number of trials for the line search.
-         *  This parameter controls the number of function and gradients evaluations
-         *  per iteration for the line search routine. The default value is 64.
-         */
-        int max_linesearch = 64;
-
-        /**
-         * The minimum step of the line search routine.
-         *  The default value is 1.0e-20. This value need not be modified unless
-         *  the exponents are too large for the machine being used, or unless the
-         *  problem is extremely badly scaled (in which case the exponents should
-         *  be increased).
-         */
-        double min_step = 1.0e-20;
-
-        /**
-         * The maximum step of the line search.
-         *  The default value is 1.0e+20. This value need not be modified unless
-         *  the exponents are too large for the machine being used, or unless the
-         *  problem is extremely badly scaled (in which case the exponents should
-         *  be increased).
-         */
-        double max_step = 1.0e+20;
-
-        /**
-         * A parameter to control the accuracy of the line search routine.
-         *  The default value is 1.0e-4. This parameter should be greater
-         *  than zero and smaller than 1.0.
-         */
-        double f_dec_coeff = 1.0e-4;
-
-        /**
-         * A parameter to control the accuracy of the line search routine.
-         *  The default value is 0.9. If the function and gradient
-         *  evaluations are inexpensive with respect to the cost of the
-         *  iteration (which is sometimes the case when solving very large
-         *  problems) it may be advantageous to set this parameter to a small
-         *  value. A typical small value is 0.1. This parameter should be
-         *  greater than the f_dec_coeff parameter and smaller than 1.0.
-         */
-        double s_curv_coeff = 0.9;
-
-        /**
-         * A parameter to ensure the global convergence for nonconvex functions.
-         *  The default value is 1.0e-6. The parameter performs the so called 
-         *  cautious update for L-BFGS, especially when the convergence is 
-         *  not sufficient. The parameter must be positive but might as well 
-         *  be less than 1.0e-3 in practice.
-         */
-        double cautious_factor = 1.0e-6;
-
-        /**
-         * The machine precision for floating-point values. The default is 1.0e-16. 
-         *  This parameter must be a positive value set by a client program to
-         *  estimate the machine precision.
-         */
-        double machine_prec = 1.0e-16;
-    };
-
-    /**
-     * Return values of lbfgs_optimize().
-     *  Roughly speaking, a negative value indicates an error.
-     */
-    enum
-    {
-        /** L-BFGS reaches convergence. */
-        LBFGS_CONVERGENCE = 0,
-        /** L-BFGS satisfies stopping criteria. */
-        LBFGS_STOP,
-        /** The iteration has been canceled by the monitor callback. */
-        LBFGS_CANCELED,
-
-        /** Unknown error. */
-        LBFGSERR_UNKNOWNERROR = -1024,
-        /** Invalid number of variables specified. */
-        LBFGSERR_INVALID_N,
-        /** Invalid parameter lbfgs_parameter_t::mem_size specified. */
-        LBFGSERR_INVALID_MEMSIZE,
-        /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */
-        LBFGSERR_INVALID_GEPSILON,
-        /** Invalid parameter lbfgs_parameter_t::past specified. */
-        LBFGSERR_INVALID_TESTPERIOD,
-        /** Invalid parameter lbfgs_parameter_t::delta specified. */
-        LBFGSERR_INVALID_DELTA,
-        /** Invalid parameter lbfgs_parameter_t::min_step specified. */
-        LBFGSERR_INVALID_MINSTEP,
-        /** Invalid parameter lbfgs_parameter_t::max_step specified. */
-        LBFGSERR_INVALID_MAXSTEP,
-        /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */
-        LBFGSERR_INVALID_FDECCOEFF,
-        /** Invalid parameter lbfgs_parameter_t::s_curv_coeff specified. */
-        LBFGSERR_INVALID_SCURVCOEFF,
-        /** Invalid parameter lbfgs_parameter_t::machine_prec specified. */
-        LBFGSERR_INVALID_MACHINEPREC,
-        /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */
-        LBFGSERR_INVALID_MAXLINESEARCH,
-        /** The function value became NaN or Inf. */
-        LBFGSERR_INVALID_FUNCVAL,
-        /** The line-search step became smaller than lbfgs_parameter_t::min_step. */
-        LBFGSERR_MINIMUMSTEP,
-        /** The line-search step became larger than lbfgs_parameter_t::max_step. */
-        LBFGSERR_MAXIMUMSTEP,
-        /** Line search reaches the maximum, assumptions not satisfied or precision not achievable.*/
-        LBFGSERR_MAXIMUMLINESEARCH,
-        /** The algorithm routine reaches the maximum number of iterations. */
-        LBFGSERR_MAXIMUMITERATION,
-        /** Relative search interval width is at least lbfgs_parameter_t::machine_prec. */
-        LBFGSERR_WIDTHTOOSMALL,
-        /** A logic error (negative line-search step) occurred. */
-        LBFGSERR_INVALIDPARAMETERS,
-        /** The current search direction increases the cost function value. */
-        LBFGSERR_INCREASEGRADIENT,
-    };
-
-    /**
-     * Callback interface to provide cost function and gradient evaluations.
-     *
-     *  The lbfgs_optimize() function call this function to obtain the values of cost
-     *  function and its gradients when needed. A client program must implement
-     *  this function to evaluate the values of the cost function and its
-     *  gradients, given current values of variables.
-     *  
-     *  @param  instance    The user data sent for lbfgs_optimize() function by the client.
-     *  @param  x           The current values of variables.
-     *  @param  g           The gradient vector. The callback function must compute
-     *                      the gradient values for the current variables.
-     *  @retval double      The value of the cost function for the current variables.
-     */
-    typedef double (*lbfgs_evaluate_t)(void *instance,
-                                       const Eigen::VectorXd &x,
-                                       Eigen::VectorXd &g);
-
-    /**
-     * Callback interface to provide an upper bound at the beginning of the current line search.
-     *
-     *  The lbfgs_optimize() function call this function to obtain the values of the
-     *  upperbound of the stepsize to search in, provided with the beginning values of
-     *  variables before the line search, and the current step vector (can be descent direction). 
-     *  A client program can implement this function for more efficient linesearch. Any step 
-     *  larger than this bound should not be considered. For example, it has a very large or even 
-     *  inf function value. Note that the function value at the provided bound should be FINITE!
-     *  If it is not used, just set it nullptr.
-     *  
-     *  @param  instance    The user data sent for lbfgs_optimize() function by the client.
-     *  @param  xp          The values of variables before current line search.
-     *  @param  d           The step vector. It can be the descent direction.
-     *  @retval double      The upperboud of the step in current line search routine,
-     *                      such that (stpbound * d) is the maximum reasonable step.
-     */
-    typedef double (*lbfgs_stepbound_t)(void *instance,
-                                        const Eigen::VectorXd &xp,
-                                        const Eigen::VectorXd &d);
-
-    /**
-     * Callback interface to monitor the progress of the minimization process.
-     *
-     *  The lbfgs_optimize() function call this function for each iteration. Implementing
-     *  this function, a client program can store or display the current progress
-     *  of the minimization process. If it is not used, just set it nullptr.
-     *
-     *  @param  instance    The user data sent for lbfgs_optimize() function by the client.
-     *  @param  x           The current values of variables.
-     *  @param  g           The current gradient values of variables.
-     *  @param  fx          The current value of the cost function.
-     *  @param  step        The line-search step used for this iteration.
-     *  @param  k           The iteration count.
-     *  @param  ls          The number of evaluations called for this iteration.
-     *  @retval int         Zero to continue the minimization process. Returning a
-     *                      non-zero value will cancel the minimization process.
-     */
-    typedef int (*lbfgs_progress_t)(void *instance,
-                                    const Eigen::VectorXd &x,
-                                    const Eigen::VectorXd &g,
-                                    const double fx,
-                                    const double step,
-                                    const int k,
-                                    const int ls);
-
-    /**
-     * Callback data struct
-     */
-    struct callback_data_t
-    {
-        void *instance = nullptr;
-        lbfgs_evaluate_t proc_evaluate = nullptr;
-        lbfgs_stepbound_t proc_stepbound = nullptr;
-        lbfgs_progress_t proc_progress = nullptr;
-    };
-
-    // ----------------------- L-BFGS Part -----------------------
-
-    /**
-     * Line search method for smooth or nonsmooth functions.
-     *  This function performs line search to find a point that satisfy 
-     *  both the Armijo condition and the weak Wolfe condition. It is 
-     *  as robust as the backtracking line search but further applies 
-     *  to continuous and piecewise smooth functions where the strong 
-     *  Wolfe condition usually does not hold.
-     *
-     *  @see
-     *      Adrian S. Lewis and Michael L. Overton. Nonsmooth optimization 
-     *      via quasi-Newton methods. Mathematical Programming, Vol 141, 
-     *      No 1, pp. 135-163, 2013.
-     */
-    inline int line_search_lewisoverton(Eigen::VectorXd &x,
-                                        double &f,
-                                        Eigen::VectorXd &g,
-                                        double &stp,
-                                        const Eigen::VectorXd &s,
-                                        const Eigen::VectorXd &xp,
-                                        const Eigen::VectorXd &gp,
-                                        const double stpmin,
-                                        const double stpmax,
-                                        const callback_data_t &cd,
-                                        const lbfgs_parameter_t &param)
-    {
-        int count = 0;
-        bool brackt = false, touched = false;
-        double finit, dginit, dgtest, dstest;
-        double mu = 0.0, nu = stpmax;
-
-        /* Check the input parameters for errors. */
-        if (!(stp > 0.0))
-        {
-            return LBFGSERR_INVALIDPARAMETERS;
-        }
-
-        /* Compute the initial gradient in the search direction. */
-        dginit = gp.dot(s);
-        /* Make sure that s points to a descent direction. */
-        if (0.0 < dginit)
-        {
-            return LBFGSERR_INCREASEGRADIENT;
-        }
-
-        /* The initial value of the cost function. */
-        finit = f;
-        dgtest = param.f_dec_coeff * dginit;
-        dstest = param.s_curv_coeff * dginit;
-
-        while (true)
-        {
-            x = xp + stp * s;
-
-            /* Evaluate the function and gradient values. */
-            f = cd.proc_evaluate(cd.instance, x, g);
-            ++count;
-
-            /* Test for errors. */
-            if (std::isinf(f) || std::isnan(f))
-            {
-                return LBFGSERR_INVALID_FUNCVAL;
-            }
-
-            if(param.past > 0 && fabs(finit-f)/(fabs(finit)+1.0)<param.delta/param.past)
-            {
-                return count;
-            }
-            /* Check the Armijo condition. */
-            if (f > finit + stp * dgtest)
-            {
-                nu = stp;
-                brackt = true;
-            }
-            else
-            {
-                /* Check the weak Wolfe condition. */
-                if (g.dot(s) < dstest)
-                {
-                    mu = stp;
-                }
-                else
-                {
-                    return count;
-                }
-            }
-            if (param.max_linesearch <= count)
-            {
-                // std::cout<< fabs(dginit)/finit<<std::endl;
-                // std::cout << fabs(finit-f)/(fabs(finit)+1.0) << std::endl;
-                /* Maximum number of iteration. */
-                return LBFGSERR_MAXIMUMLINESEARCH;
-            }
-            if (brackt && (nu - mu) < param.machine_prec * nu)
-            {
-                /* Relative interval width is at least machine_prec. */
-                return LBFGSERR_WIDTHTOOSMALL;
-            }
-
-            if (brackt)
-            {
-                stp = 0.5 * (mu + nu);
-            }
-            else
-            {
-                stp *= 2.0;
-            }
-
-            if (stp < stpmin)
-            {
-                /* The step is the minimum value. */
-                return LBFGSERR_MINIMUMSTEP;
-            }
-            if (stp > stpmax)
-            {
-                if (touched)
-                {
-                    /* The step is the maximum value. */
-                    return LBFGSERR_MAXIMUMSTEP;
-                }
-                else
-                {
-                    /* The maximum value should be tried once. */
-                    touched = true;
-                    stp = stpmax;
-                }
-            }
-        }
-    }
-
-    /**
-     * Start a L-BFGS optimization.
-     * Assumptions: 1. f(x) is either C2 or C0 but piecewise C2;
-     *              2. f(x) is lower bounded;
-     *              3. f(x) has bounded level sets;
-     *              4. g(x) is either the gradient or subgradient;
-     *              5. The gradient exists at the initial guess x0.
-     * A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation
-     * callback) and pass the pointer to the callback function to lbfgs_optimize() 
-     * arguments. Similarly, a user can implement a function compatible with 
-     * ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and 
-     * ::lbfgs_progress_t (progress callback) to obtain the current progress 
-     * (e.g., variables, function, and gradient, etc) and to cancel the iteration 
-     * process if necessary. Implementation of the stepbound and the progress callback 
-     * is optional: a user can pass nullptr if progress notification is not necessary.
-     * 
-     *
-     *  @param  x               The vector of decision variables.
-     *                          THE INITIAL GUESS x0 SHOULD BE SET BEFORE THE CALL!
-     *                          A client program can receive decision variables 
-     *                          through this vector, at which the cost and its 
-     *                          gradient are queried during minimization.
-     *  @param  f               The ref to the variable that receives the final
-     *                          value of the cost function for the variables.
-     *  @param  proc_evaluate   The callback function to provide function f(x) and
-     *                          gradient g(x) evaluations given a current values of
-     *                          variables x. A client program must implement a
-     *                          callback function compatible with lbfgs_evaluate_t 
-     *                          and pass the pointer to the callback function.
-     *  @param  proc_stepbound  The callback function to provide values of the
-     *                          upperbound of the stepsize to search in, provided
-     *                          with the beginning values of variables before the 
-     *                          line search, and the current step vector (can be 
-     *                          negative gradient). A client program can implement
-     *                          this function for more efficient linesearch. If it is
-     *                          not used, just set it nullptr.
-     *  @param  proc_progress   The callback function to receive the progress
-     *                          (the number of iterations, the current value of
-     *                          the cost function) of the minimization
-     *                          process. This argument can be set to nullptr if
-     *                          a progress report is unnecessary.
-     *  @param  instance        A user data pointer for client programs. The callback
-     *                          functions will receive the value of this argument.
-     *  @param  param           The parameters for L-BFGS optimization.
-     *  @retval int             The status code. This function returns a nonnegative 
-     *                          integer if the minimization process terminates without 
-     *                          an error. A negative integer indicates an error.
-     */
-    inline int lbfgs_optimize(Eigen::VectorXd &x,
-                              double &f,
-                              lbfgs_evaluate_t proc_evaluate,
-                              lbfgs_stepbound_t proc_stepbound,
-                              lbfgs_progress_t proc_progress,
-                              void *instance,
-                              const lbfgs_parameter_t &param)
-    {
-        int ret, i, j, k, ls, end, bound;
-        double step, step_min, step_max, fx, ys, yy;
-        double gnorm_inf, xnorm_inf, beta, rate, cau;
-
-        const int n = x.size();
-        const int m = param.mem_size;
-
-        /* Check the input parameters for errors. */
-        if (n <= 0)
-        {
-            return LBFGSERR_INVALID_N;
-        }
-        if (m <= 0)
-        {
-            return LBFGSERR_INVALID_MEMSIZE;
-        }
-        if (param.g_epsilon < 0.0)
-        {
-            return LBFGSERR_INVALID_GEPSILON;
-        }
-        if (param.past < 0)
-        {
-            return LBFGSERR_INVALID_TESTPERIOD;
-        }
-        if (param.delta < 0.0)
-        {
-            return LBFGSERR_INVALID_DELTA;
-        }
-        if (param.min_step < 0.0)
-        {
-            return LBFGSERR_INVALID_MINSTEP;
-        }
-        if (param.max_step < param.min_step)
-        {
-            return LBFGSERR_INVALID_MAXSTEP;
-        }
-        if (!(param.f_dec_coeff > 0.0 &&
-              param.f_dec_coeff < 1.0))
-        {
-            return LBFGSERR_INVALID_FDECCOEFF;
-        }
-        if (!(param.s_curv_coeff < 1.0 &&
-              param.s_curv_coeff > param.f_dec_coeff))
-        {
-            return LBFGSERR_INVALID_SCURVCOEFF;
-        }
-        if (!(param.machine_prec > 0.0))
-        {
-            return LBFGSERR_INVALID_MACHINEPREC;
-        }
-        if (param.max_linesearch <= 0)
-        {
-            return LBFGSERR_INVALID_MAXLINESEARCH;
-        }
-
-        /* Prepare intermediate variables. */
-        Eigen::VectorXd xp(n);
-        Eigen::VectorXd g(n);
-        Eigen::VectorXd gp(n);
-        Eigen::VectorXd d(n);
-        Eigen::VectorXd pf(std::max(1, param.past));
-
-        /* Initialize the limited memory. */
-        Eigen::VectorXd lm_alpha = Eigen::VectorXd::Zero(m);
-        Eigen::MatrixXd lm_s = Eigen::MatrixXd::Zero(n, m);
-        Eigen::MatrixXd lm_y = Eigen::MatrixXd::Zero(n, m);
-        Eigen::VectorXd lm_ys = Eigen::VectorXd::Zero(m);
-
-        /* Construct a callback data. */
-        callback_data_t cd;
-        cd.instance = instance;
-        cd.proc_evaluate = proc_evaluate;
-        cd.proc_stepbound = proc_stepbound;
-        cd.proc_progress = proc_progress;
-
-        /* Evaluate the function value and its gradient. */
-        fx = cd.proc_evaluate(cd.instance, x, g);
-
-        /* Store the initial value of the cost function. */
-        pf(0) = fx;
-
-        /*
-        Compute the direction;
-        we assume the initial hessian matrix H_0 as the identity matrix.
-        */
-        d = -g;
-
-        /*
-        Make sure that the initial variables are not a stationary point.
-        */
-        gnorm_inf = g.cwiseAbs().maxCoeff();
-        xnorm_inf = x.cwiseAbs().maxCoeff();
-
-        if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon)
-        {
-            /* The initial guess is already a stationary point. */
-            ret = LBFGS_CONVERGENCE;
-        }
-        else
-        {
-            /* 
-            Compute the initial step:
-            */
-            step = 1.0 / d.norm();
-
-            k = 1;
-            end = 0;
-            bound = 0;
-
-            while (true)
-            {
-                /* Store the current position and gradient vectors. */
-                xp = x;
-                gp = g;
-
-                /* If the step bound can be provied dynamically, then apply it. */
-                step_min = param.min_step;
-                step_max = param.max_step;
-                if (cd.proc_stepbound)
-                {
-                    step_max = cd.proc_stepbound(cd.instance, xp, d);
-                    step_max = step_max < param.max_step ? step_max : param.max_step;
-                    step = step < step_max ? step : 0.5 * step_max;
-                }
-
-                /* Search for an optimal step. */
-                ls = line_search_lewisoverton(x, fx, g, step, d, xp, gp, step_min, step_max, cd, param);
-                
-                // int idx = 1;
-                // step = 1.0;
-                // if(d.norm()>1.0){
-                //         d = d.normalized().eval()*1.0;
-                // }
-                // while(1){
-                //     // std::cout<<"step * d"<<(step * d).transpose()<<std::endl;
-                //     x = xp + step * d;
-                //     /* Evaluate the function and gradient values. */
-                //     double f = cd.proc_evaluate(cd.instance, x, g);
-                //     if(g.norm()>1.0){
-                //         g = g.normalized().eval()*1.0;
-                //     }
-                //     std::cout<<"fx: "<<fx<<"  f: "<<f<<"step: "<<step<<std::endl;
-                //     if(f < fx || step <= step_min){
-                //         fx = f;
-                //         break;
-                //     }
-                    
-                //     step  = 1.0 / (++idx);
-                // }
-                // ls  = 1;
-
-                
-
-
-                
-
-                if (ls < 0)
-                {
-                    /* Revert to the previous point. */
-                    x = xp;
-                    g = gp;
-                    ret = ls;
-                    break;
-                }
-
-                /* Report the progress. */
-                if (cd.proc_progress)
-                {
-                    if (cd.proc_progress(cd.instance, x, g, fx, step, k, ls))
-                    {
-                        ret = LBFGS_CANCELED;
-                        break;
-                    }
-                }
-
-                /*
-                Convergence test.
-                The criterion is given by the following formula:
-                ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon
-                */
-                gnorm_inf = g.cwiseAbs().maxCoeff();
-                xnorm_inf = x.cwiseAbs().maxCoeff();
-                if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon)
-                {
-                    /* Convergence. */
-                    ret = LBFGS_CONVERGENCE;
-                    break;
-                }
-
-                /*
-                Test for stopping criterion.
-                The criterion is given by the following formula:
-                |f(past_x) - f(x)| / max(1, |f(x)|) < \delta.
-                */
-                if (0 < param.past)
-                {
-                    /* We don't test the stopping criterion while k < past. */
-                    if (param.past <= k)
-                    {
-                        /* The stopping criterion. */
-                        rate = std::fabs(pf(k % param.past) - fx) / std::max(1.0, std::fabs(fx));
-
-                        if (rate < param.delta)
-                        {
-                            ret = LBFGS_STOP;
-                            break;
-                        }
-                    }
-
-                    /* Store the current value of the cost function. */
-                    pf(k % param.past) = fx;
-                }
-
-                if (param.max_iterations != 0 && param.max_iterations <= k)
-                {
-                    /* Maximum number of iterations. */
-                    ret = LBFGSERR_MAXIMUMITERATION;
-                    break;
-                }
-
-                /* Count the iteration number. */
-                ++k;
-
-                /*
-                Update vectors s and y:
-                s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}.
-                y_{k+1} = g_{k+1} - g_{k}.
-                */
-                lm_s.col(end) = x - xp;
-                lm_y.col(end) = g - gp;
-
-                /*
-                Compute scalars ys and yy:
-                ys = y^t \cdot s = 1 / \rho.
-                yy = y^t \cdot y.
-                Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
-                */
-                ys = lm_y.col(end).dot(lm_s.col(end));
-                yy = lm_y.col(end).squaredNorm();
-                lm_ys(end) = ys;
-
-                /* Compute the negative of gradients. */
-                d = -g;
-
-                /* 
-                Only cautious update is performed here as long as 
-                (y^t \cdot s) / ||s_{k+1}||^2 > \epsilon * ||g_{k}||^\alpha,
-                where \epsilon is the cautious factor and a proposed value 
-                for \alpha is 1.
-                This is not for enforcing the PD of the approxomated Hessian 
-                since ys > 0 is already ensured by the weak Wolfe condition. 
-                This is to ensure the global convergence as described in:
-                Dong-Hui Li and Masao Fukushima. On the global convergence of 
-                the BFGS method for nonconvex unconstrained optimization problems. 
-                SIAM Journal on Optimization, Vol 11, No 4, pp. 1054-1064, 2011.
-                */
-                cau = lm_s.col(end).squaredNorm() * gp.norm() * param.cautious_factor;
-
-                if (ys > cau)
-                {
-                    /*
-                    Recursive formula to compute dir = -(H \cdot g).
-                    This is described in page 779 of:
-                    Jorge Nocedal.
-                    Updating Quasi-Newton Matrices with Limited Storage.
-                    Mathematics of Computation, Vol. 35, No. 151,
-                    pp. 773--782, 1980.
-                    */
-                    ++bound;
-                    bound = m < bound ? m : bound;
-                    end = (end + 1) % m;
-
-                    j = end;
-                    for (i = 0; i < bound; ++i)
-                    {
-                        j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */
-                        /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */
-                        lm_alpha(j) = lm_s.col(j).dot(d) / lm_ys(j);
-                        /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */
-                        d += (-lm_alpha(j)) * lm_y.col(j);
-                    }
-
-                    d *= ys / yy;
-
-                    for (i = 0; i < bound; ++i)
-                    {
-                        /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */
-                        beta = lm_y.col(j).dot(d) / lm_ys(j);
-                        /* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */
-                        d += (lm_alpha(j) - beta) * lm_s.col(j);
-                        j = (j + 1) % m; /* if (++j == m) j = 0; */
-                    }
-                }
-
-                /* The search direction d is ready. We try step = 1 first. */
-                step = 1.0;
-            }
-        }
-
-        /* Return the final value of the cost function. */
-        f = fx;
-
-        return ret;
-    }
-
-    /**
-     * Get string description of an lbfgs_optimize() return code.
-     *
-     *  @param err          A value returned by lbfgs_optimize().
-     */
-    inline const char *lbfgs_strerror(const int err)
-    {
-        switch (err)
-        {
-        case LBFGS_CONVERGENCE:
-            return "Success: reached convergence (g_epsilon).";
-
-        case LBFGS_STOP:
-            return "Success: met stopping criteria (past f decrease less than delta).";
-
-        case LBFGS_CANCELED:
-            return "The iteration has been canceled by the monitor callback.";
-
-        case LBFGSERR_UNKNOWNERROR:
-            return "Unknown error.";
-
-        case LBFGSERR_INVALID_N:
-            return "Invalid number of variables specified.";
-
-        case LBFGSERR_INVALID_MEMSIZE:
-            return "Invalid parameter lbfgs_parameter_t::mem_size specified.";
-
-        case LBFGSERR_INVALID_GEPSILON:
-            return "Invalid parameter lbfgs_parameter_t::g_epsilon specified.";
-
-        case LBFGSERR_INVALID_TESTPERIOD:
-            return "Invalid parameter lbfgs_parameter_t::past specified.";
-
-        case LBFGSERR_INVALID_DELTA:
-            return "Invalid parameter lbfgs_parameter_t::delta specified.";
-
-        case LBFGSERR_INVALID_MINSTEP:
-            return "Invalid parameter lbfgs_parameter_t::min_step specified.";
-
-        case LBFGSERR_INVALID_MAXSTEP:
-            return "Invalid parameter lbfgs_parameter_t::max_step specified.";
-
-        case LBFGSERR_INVALID_FDECCOEFF:
-            return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified.";
-
-        case LBFGSERR_INVALID_SCURVCOEFF:
-            return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified.";
-
-        case LBFGSERR_INVALID_MACHINEPREC:
-            return "Invalid parameter lbfgs_parameter_t::machine_prec specified.";
-
-        case LBFGSERR_INVALID_MAXLINESEARCH:
-            return "Invalid parameter lbfgs_parameter_t::max_linesearch specified.";
-
-        case LBFGSERR_INVALID_FUNCVAL:
-            return "The function value became NaN or Inf.";
-
-        case LBFGSERR_MINIMUMSTEP:
-            return "The line-search step became smaller than lbfgs_parameter_t::min_step.";
-
-        case LBFGSERR_MAXIMUMSTEP:
-            return "The line-search step became larger than lbfgs_parameter_t::max_step.";
-
-        case LBFGSERR_MAXIMUMLINESEARCH:
-            return "Line search reaches the maximum try number, assumptions not satisfied or precision not achievable.";
-
-        case LBFGSERR_MAXIMUMITERATION:
-            return "The algorithm routine reaches the maximum number of iterations.";
-
-        case LBFGSERR_WIDTHTOOSMALL:
-            return "Relative search interval width is at least lbfgs_parameter_t::machine_prec.";
-
-        case LBFGSERR_INVALIDPARAMETERS:
-            return "A logic error (negative line-search step) occurred.";
-
-        case LBFGSERR_INCREASEGRADIENT:
-            return "The current search direction increases the cost function value.";
-
-        default:
-            return "(unknown)";
-        }
-    }
-
-} // namespace lbfgs
-
-#endif

NeuralTraj/src/plan_manage/include/arcPlan/sdlp.hpp

@@ -1,801 +0,0 @@
-/* 
- * Copyright (c) 1990 Michael E. Hohmeyer, 
- *       hohmeyer@icemcfd.com
- * Permission is granted to modify and re-distribute this code in any manner
- * as long as this notice is preserved.  All standard disclaimers apply.
- * 
- * R. Seidel's algorithm for solving LPs (linear programs.)
- */
-
-/* 
- * Copyright (c) 2021 Zhepei Wang,
- *       wangzhepei@live.com
- * 1. Bug fix in "move_to_front" function that "prev[m]" is illegally accessed
- *    while "prev" originally has only m ints. It is fixed by allocating a 
- *    "prev" with m + 1 ints.  
- * 2. Add Eigen interface.
- * Permission is granted to modify and re-distribute this code in any manner
- * as long as this notice is preserved.  All standard disclaimers apply.
- * 
- * Reference: Seidel, R. (1991), "Small-dimensional linear programming and convex hulls made easy", 
- *            Discrete & Computational Geometry 6 (1): 423–434, doi:10.1007/BF02574699
- */
-
-#ifndef SDLP_HPP
-#define SDLP_HPP
-
-#include <Eigen/Eigen>
-
-#include <cmath>
-#include <random>
-
-namespace sdlp
-{
-constexpr double prog_epsilon = 2.0e-16;
-
-enum PROG_STATE
-{
-    /* minimum attained */
-    MINIMUM = 0,
-    /* no feasible region */
-    INFEASIBLE,
-    /* unbounded solution */
-    UNBOUNDED,
-    /* only a vertex in the solution set */
-    AMBIGUOUS,
-};
-
-inline double dot2(const double a[2], const double b[2])
-{
-    return a[0] * b[0] + a[1] * b[1];
-}
-
-inline double cross2(const double a[2], const double b[2])
-{
-    return a[0] * b[1] - a[1] * b[0];
-}
-
-inline bool unit2(const double a[2], double b[2], double eps)
-{
-    double size;
-    size = sqrt(a[0] * a[0] + a[1] * a[1]);
-    if (size < 2 * eps)
-    {
-        return true;
-    }
-    b[0] = a[0] / size;
-    b[1] = a[1] / size;
-    return false;
-}
-
-/* unitize a d+1 dimensional point */
-inline bool lp_d_unit(int d, const double *a, double *b)
-{
-    int i;
-    double size;
-
-    size = 0.0;
-    for (i = 0; i <= d; i++)
-    {
-        size += a[i] * a[i];
-    }
-    if (size < (d + 1) * prog_epsilon * prog_epsilon)
-    {
-        return true;
-    }
-    size = 1.0 / sqrt(size);
-    for (i = 0; i <= d; i++)
-    {
-        b[i] = a[i] * size;
-    }
-    return false;
-}
-
-/* optimize the objective function when there are no contraints */
-inline int lp_no_constraints(int d, const double *n_vec, const double *d_vec, double *opt)
-{
-    int i;
-    double n_dot_d, d_dot_d;
-
-    n_dot_d = 0.0;
-    d_dot_d = 0.0;
-    for (i = 0; i <= d; i++)
-    {
-        n_dot_d += n_vec[i] * d_vec[i];
-        d_dot_d += d_vec[i] * d_vec[i];
-    }
-    if (d_dot_d < prog_epsilon * prog_epsilon)
-    {
-        d_dot_d = 1.0;
-        n_dot_d = 0.0;
-    }
-    for (i = 0; i <= d; i++)
-    {
-        opt[i] = -n_vec[i] + d_vec[i] * n_dot_d / d_dot_d;
-    }
-    /* normalize the optimal point */
-    if (lp_d_unit(d, opt, opt))
-    {
-        opt[d] = 1.0;
-        return AMBIGUOUS;
-    }
-    else
-    {
-        return MINIMUM;
-    }
-}
-
-/* returns the index of the plane that is in i's place */
-inline int move_to_front(int i, int *next, int *prev)
-{
-    int previ;
-    if (i == 0 || i == next[0])
-    {
-        return i;
-    }
-    previ = prev[i];
-    /* remove i from it's current position */
-    next[prev[i]] = next[i];
-    prev[next[i]] = prev[i];
-    /* put i at the front */
-    next[i] = next[0];
-    prev[i] = 0;
-    prev[next[i]] = i;
-    next[0] = i;
-    return previ;
-}
-
-inline void lp_min_lin_rat(int degen,
-                           const double cw_vec[2],
-                           const double ccw_vec[2],
-                           const double n_vec[2],
-                           const double d_vec[2],
-                           double opt[2])
-{
-    double d_cw, d_ccw, n_cw, n_ccw;
-
-    /* linear rational function case */
-    d_cw = dot2(cw_vec, d_vec);
-    d_ccw = dot2(ccw_vec, d_vec);
-    n_cw = dot2(cw_vec, n_vec);
-    n_ccw = dot2(ccw_vec, n_vec);
-    if (degen)
-    {
-        /* if degenerate simply compare values */
-        if (n_cw / d_cw < n_ccw / d_ccw)
-        {
-            opt[0] = cw_vec[0];
-            opt[1] = cw_vec[1];
-        }
-        else
-        {
-            opt[0] = ccw_vec[0];
-            opt[1] = ccw_vec[1];
-        }
-        /* check that the clock-wise and counter clockwise bounds are not near a poles */
-    }
-    else if (fabs(d_cw) > 2.0 * prog_epsilon &&
-             fabs(d_ccw) > 2.0 * prog_epsilon)
-    {
-        /* the valid region does not contain a poles */
-        if (d_cw * d_ccw > 0.0)
-        {
-            /* find which end has the minimum value */
-            if (n_cw / d_cw < n_ccw / d_ccw)
-            {
-                opt[0] = cw_vec[0];
-                opt[1] = cw_vec[1];
-            }
-            else
-            {
-                opt[0] = ccw_vec[0];
-                opt[1] = ccw_vec[1];
-            }
-        }
-        else
-        {
-            /* the valid region does contain a poles */
-            if (d_cw > 0.0)
-            {
-                opt[0] = -d_vec[1];
-                opt[1] = d_vec[0];
-            }
-            else
-            {
-                opt[0] = d_vec[1];
-                opt[1] = -d_vec[0];
-            }
-        }
-    }
-    else if (fabs(d_cw) > 2.0 * prog_epsilon)
-    {
-        /* the counter clockwise bound is near a pole */
-        if (n_ccw * d_cw > 0.0)
-        {
-            /* counter clockwise bound is a positive pole */
-            opt[0] = cw_vec[0];
-            opt[1] = cw_vec[1];
-        }
-        else
-        {
-            /* counter clockwise bound is a negative pole */
-            opt[0] = ccw_vec[0];
-            opt[1] = ccw_vec[1];
-        }
-    }
-    else if (fabs(d_ccw) > 2.0 * prog_epsilon)
-    {
-        /* the clockwise bound is near a pole */
-        if (n_cw * d_ccw > 2 * prog_epsilon)
-        {
-            /* clockwise bound is at a positive pole */
-            opt[0] = ccw_vec[0];
-            opt[1] = ccw_vec[1];
-        }
-        else
-        {
-            /* clockwise bound is at a negative pole */
-            opt[0] = cw_vec[0];
-            opt[1] = cw_vec[1];
-        }
-    }
-    else
-    {
-        /* both bounds are near poles */
-        if (cross2(d_vec, n_vec) > 0.0)
-        {
-            opt[0] = cw_vec[0];
-            opt[1] = cw_vec[1];
-        }
-        else
-        {
-            opt[0] = ccw_vec[0];
-            opt[1] = ccw_vec[1];
-        }
-    }
-}
-
-inline int wedge(const double (*halves)[2],
-                 int m,
-                 int *next,
-                 int *prev,
-                 double cw_vec[2],
-                 double ccw_vec[2],
-                 int *degen)
-{
-    int i;
-    double d_cw, d_ccw;
-    int offensive;
-
-    *degen = 0;
-    for (i = 0; i != m; i = next[i])
-    {
-        if (!unit2(halves[i], ccw_vec, prog_epsilon))
-        {
-            /* clock-wise */
-            cw_vec[0] = ccw_vec[1];
-            cw_vec[1] = -ccw_vec[0];
-            /* counter-clockwise */
-            ccw_vec[0] = -cw_vec[0];
-            ccw_vec[1] = -cw_vec[1];
-            break;
-        }
-    }
-    if (i == m)
-    {
-        return UNBOUNDED;
-    }
-    i = 0;
-    while (i != m)
-    {
-        offensive = 0;
-        d_cw = dot2(cw_vec, halves[i]);
-        d_ccw = dot2(ccw_vec, halves[i]);
-        if (d_ccw >= 2 * prog_epsilon)
-        {
-            if (d_cw <= -2 * prog_epsilon)
-            {
-                cw_vec[0] = halves[i][1];
-                cw_vec[1] = -halves[i][0];
-                unit2(cw_vec, cw_vec, prog_epsilon);
-                offensive = 1;
-            }
-        }
-        else if (d_cw >= 2 * prog_epsilon)
-        {
-            if (d_ccw <= -2 * prog_epsilon)
-            {
-                ccw_vec[0] = -halves[i][1];
-                ccw_vec[1] = halves[i][0];
-                unit2(ccw_vec, ccw_vec, prog_epsilon);
-                offensive = 1;
-            }
-        }
-        else if (d_ccw <= -2 * prog_epsilon && d_cw <= -2 * prog_epsilon)
-        {
-            return INFEASIBLE;
-        }
-        else if ((d_cw <= -2 * prog_epsilon) ||
-                 (d_ccw <= -2 * prog_epsilon) ||
-                 (cross2(cw_vec, halves[i]) < 0.0))
-        {
-            /* degenerate */
-            if (d_cw <= -2 * prog_epsilon)
-            {
-                unit2(ccw_vec, cw_vec, prog_epsilon);
-            }
-            else if (d_ccw <= -2 * prog_epsilon)
-            {
-                unit2(cw_vec, ccw_vec, prog_epsilon);
-            }
-            *degen = 1;
-            offensive = 1;
-        }
-        /* place this offensive plane in second place */
-        if (offensive)
-        {
-            i = move_to_front(i, next, prev);
-        }
-        i = next[i];
-        if (*degen)
-        {
-            break;
-        }
-    }
-    if (*degen)
-    {
-        while (i != m)
-        {
-            d_cw = dot2(cw_vec, halves[i]);
-            d_ccw = dot2(ccw_vec, halves[i]);
-            if (d_cw < -2 * prog_epsilon)
-            {
-                if (d_ccw < -2 * prog_epsilon)
-                {
-                    return INFEASIBLE;
-                }
-                else
-                {
-                    cw_vec[0] = ccw_vec[0];
-                    cw_vec[1] = ccw_vec[1];
-                }
-            }
-            else if (d_ccw < -2 * prog_epsilon)
-            {
-                ccw_vec[0] = cw_vec[0];
-                ccw_vec[1] = cw_vec[1];
-            }
-            i = next[i];
-        }
-    }
-    return MINIMUM;
-}
-
-/*
-* return the minimum on the projective line
-*/
-inline int lp_base_case(const double (*halves)[2], /* halves --- half lines */
-                        int m,                     /* m      --- terminal marker */
-                        const double n_vec[2],     /* n_vec  --- numerator funciton */
-                        const double d_vec[2],     /* d_vec  --- denominator function */
-                        double opt[2],             /* opt    --- optimum  */
-                        int *next,                 /* next, prev  --- double linked list of indices */
-                        int *prev)
-{
-    double cw_vec[2], ccw_vec[2];
-    int degen;
-    int status;
-    double ab;
-
-    /* find the feasible region of the line */
-    status = wedge(halves, m, next, prev, cw_vec, ccw_vec, &degen);
-
-    if (status == INFEASIBLE)
-    {
-        return status;
-    }
-    /* no non-trivial constraints one the plane: return the unconstrained optimum */
-    if (status == UNBOUNDED)
-    {
-        return lp_no_constraints(1, n_vec, d_vec, opt);
-    }
-    ab = fabs(cross2(n_vec, d_vec));
-    if (ab < 2 * prog_epsilon * prog_epsilon)
-    {
-        if (dot2(n_vec, n_vec) < 2 * prog_epsilon * prog_epsilon ||
-            dot2(d_vec, d_vec) > 2 * prog_epsilon * prog_epsilon)
-        {
-            /* numerator is zero or numerator and denominator are linearly dependent */
-            opt[0] = cw_vec[0];
-            opt[1] = cw_vec[1];
-            status = AMBIGUOUS;
-        }
-        else
-        {
-            /* numerator is non-zero and denominator is zero minimize linear functional on circle */
-            if (!degen && cross2(cw_vec, n_vec) <= 0.0 &&
-                cross2(n_vec, ccw_vec) <= 0.0)
-            {
-                /* optimum is in interior of feasible region */
-                opt[0] = -n_vec[0];
-                opt[1] = -n_vec[1];
-            }
-            else if (dot2(n_vec, cw_vec) > dot2(n_vec, ccw_vec))
-            {
-                /* optimum is at counter-clockwise boundary */
-                opt[0] = ccw_vec[0];
-                opt[1] = ccw_vec[1];
-            }
-            else
-            {
-                /* optimum is at clockwise boundary */
-                opt[0] = cw_vec[0];
-                opt[1] = cw_vec[1];
-            }
-            status = MINIMUM;
-        }
-    }
-    else
-    {
-        /* niether numerator nor denominator is zero */
-        lp_min_lin_rat(degen, cw_vec, ccw_vec, n_vec, d_vec, opt);
-        status = MINIMUM;
-    }
-    return status;
-}
-
-/* find the largest coefficient in a plane */
-inline void findimax(const double *pln, int idim, int *imax)
-{
-    double rmax;
-    int i;
-
-    *imax = 0;
-    rmax = fabs(pln[0]);
-    for (i = 1; i <= idim; i++)
-    {
-        double ab;
-        ab = fabs(pln[i]);
-        if (ab > rmax)
-        {
-            *imax = i;
-            rmax = ab;
-        }
-    }
-}
-
-inline void vector_up(const double *equation, int ivar, int idim,
-                      const double *low_vector, double *vector)
-{
-    int i;
-
-    vector[ivar] = 0.0;
-    for (i = 0; i < ivar; i++)
-    {
-        vector[i] = low_vector[i];
-        vector[ivar] -= equation[i] * low_vector[i];
-    }
-    for (i = ivar + 1; i <= idim; i++)
-    {
-        vector[i] = low_vector[i - 1];
-        vector[ivar] -= equation[i] * low_vector[i - 1];
-    }
-    vector[ivar] /= equation[ivar];
-}
-
-inline void vector_down(const double *elim_eqn, int ivar, int idim,
-                        const double *old_vec, double *new_vec)
-{
-    int i;
-    double fac, ve, ee;
-    ve = 0.0;
-    ee = 0.0;
-    for (i = 0; i <= idim; i++)
-    {
-        ve += old_vec[i] * elim_eqn[i];
-        ee += elim_eqn[i] * elim_eqn[i];
-    }
-    fac = ve / ee;
-    for (i = 0; i < ivar; i++)
-    {
-        new_vec[i] = old_vec[i] - elim_eqn[i] * fac;
-    }
-    for (i = ivar + 1; i <= idim; i++)
-    {
-        new_vec[i - 1] = old_vec[i] - elim_eqn[i] * fac;
-    }
-}
-
-inline void plane_down(const double *elim_eqn, int ivar, int idim,
-                       const double *old_plane, double *new_plane)
-{
-    double crit;
-    int i;
-
-    crit = old_plane[ivar] / elim_eqn[ivar];
-    for (i = 0; i < ivar; i++)
-    {
-        new_plane[i] = old_plane[i] - elim_eqn[i] * crit;
-    }
-    for (i = ivar + 1; i <= idim; i++)
-    {
-        new_plane[i - 1] = old_plane[i] - elim_eqn[i] * crit;
-    }
-}
-
-inline int linfracprog(const double *halves, /* halves  --- half spaces */
-                       int istart,           /* istart  --- should be zero unless doing incremental algorithm */
-                       int m,                /* m       --- terminal marker */
-                       const double *n_vec,  /* n_vec   --- numerator vector */
-                       const double *d_vec,  /* d_vec   --- denominator vector */
-                       int d,                /* d       --- projective dimension */
-                       double *opt,          /* opt     --- optimum */
-                       double *work,         /* work    --- work space (see below) */
-                       int *next,            /* next    --- array of indices into halves */
-                       int *prev,            /* prev    --- array of indices into halves */
-                       int max_size)         /* max_size --- size of halves array */
-/*
-**
-** half-spaces are in the form
-** halves[i][0]*x[0] + halves[i][1]*x[1] + 
-** ... + halves[i][d-1]*x[d-1] + halves[i][d]*x[d] >= 0
-**
-** coefficients should be normalized
-** half-spaces should be in random order
-** the order of the half spaces is 0, next[0] next[next[0]] ...
-** and prev[next[i]] = i
-**
-** halves: (max_size)x(d+1)
-**
-** the optimum has been computed for the half spaces
-** 0 , next[0], next[next[0]] , ... , prev[istart]
-** the next plane that needs to be tested is istart
-**
-** m is the index of the first plane that is NOT on the list
-** i.e. m is the terminal marker for the linked list.
-**
-** the objective function is dot(x,nvec)/dot(x,dvec)
-** if you want the program to solve standard d dimensional linear programming
-** problems then n_vec = ( x0, x1, x2, ..., xd-1, 0)
-** and           d_vec = (  0,  0,  0, ...,    0, 1)
-** and halves[0] = (0, 0, ... , 1)
-**
-** work points to (max_size+3)*(d+2)*(d-1)/2 double space
-*/
-{
-    int status;
-    int i, j, imax;
-    double *new_opt, *new_n_vec, *new_d_vec, *new_halves, *new_work;
-    const double *plane_i;
-    double val;
-
-    if (d == 1 && m != 0)
-    {
-        return lp_base_case((const double(*)[2])halves, m, n_vec,
-                            d_vec, opt, next, prev);
-    }
-    else
-    {
-        int d_vec_zero;
-        val = 0.0;
-        for (j = 0; j <= d; j++)
-        {
-            val += d_vec[j] * d_vec[j];
-        }
-        d_vec_zero = (val < (d + 1) * prog_epsilon * prog_epsilon);
-
-        /* find the unconstrained minimum */
-        if (!istart)
-        {
-            status = lp_no_constraints(d, n_vec, d_vec, opt);
-        }
-        else
-        {
-            status = MINIMUM;
-        }
-        if (m == 0)
-        {
-            return status;
-        }
-
-        /* allocate memory for next level of recursion */
-        new_opt = work;
-        new_n_vec = new_opt + d;
-        new_d_vec = new_n_vec + d;
-        new_halves = new_d_vec + d;
-        new_work = new_halves + max_size * d;
-        for (i = istart; i != m; i = next[i])
-        {
-            /* if the optimum is not in half space i then project the problem onto that plane */
-            plane_i = halves + i * (d + 1);
-            /* determine if the optimum is on the correct side of plane_i */
-            val = 0.0;
-            for (j = 0; j <= d; j++)
-            {
-                val += opt[j] * plane_i[j];
-            }
-            if (val < -(d + 1) * prog_epsilon)
-            {
-                /* find the largest of the coefficients to eliminate */
-                findimax(plane_i, d, &imax);
-                /* eliminate that variable */
-                if (i != 0)
-                {
-                    double fac;
-                    fac = 1.0 / plane_i[imax];
-                    for (j = 0; j != i; j = next[j])
-                    {
-                        const double *old_plane;
-                        double *new_plane;
-                        int k;
-                        double crit;
-
-                        old_plane = halves + j * (d + 1);
-                        new_plane = new_halves + j * d;
-                        crit = old_plane[imax] * fac;
-                        for (k = 0; k < imax; k++)
-                        {
-                            new_plane[k] = old_plane[k] - plane_i[k] * crit;
-                        }
-                        for (k = imax + 1; k <= d; k++)
-                        {
-                            new_plane[k - 1] = old_plane[k] - plane_i[k] * crit;
-                        }
-                    }
-                }
-                /* project the objective function to lower dimension */
-                if (d_vec_zero)
-                {
-                    vector_down(plane_i, imax, d, n_vec, new_n_vec);
-                    for (j = 0; j < d; j++)
-                    {
-                        new_d_vec[j] = 0.0;
-                    }
-                }
-                else
-                {
-                    plane_down(plane_i, imax, d, n_vec, new_n_vec);
-                    plane_down(plane_i, imax, d, d_vec, new_d_vec);
-                }
-                /* solve sub problem */
-                status = linfracprog(new_halves, 0, i, new_n_vec,
-                                     new_d_vec, d - 1, new_opt, new_work, next, prev, max_size);
-                /* back substitution */
-                if (status != INFEASIBLE)
-                {
-                    vector_up(plane_i, imax, d, new_opt, opt);
-
-                    /* in line code for unit */
-                    double size;
-                    size = 0.0;
-                    for (j = 0; j <= d; j++)
-                        size += opt[j] * opt[j];
-                    size = 1.0 / sqrt(size);
-                    for (j = 0; j <= d; j++)
-                        opt[j] *= size;
-                }
-                else
-                {
-                    return status;
-                }
-                /* place this offensive plane in second place */
-                i = move_to_front(i, next, prev);
-            }
-        }
-        return status;
-    }
-}
-
-inline void rand_permutation(int n, int *p)
-{
-    typedef std::uniform_int_distribution<int> rand_int;
-    typedef rand_int::param_type rand_range;
-    static std::mt19937_64 gen;
-    static rand_int rdi(0, 1);
-    int i, j, t;
-    for (i = 0; i < n; i++)
-    {
-        p[i] = i;
-    }
-    for (i = 0; i < n; i++)
-    {
-        rdi.param(rand_range(0, n - i - 1));
-        j = rdi(gen) + i;
-        t = p[j];
-        p[j] = p[i];
-        p[i] = t;
-    }
-}
-
-inline double linprog(const Eigen::VectorXd &c,
-                      const Eigen::MatrixXd &A,
-                      const Eigen::VectorXd &b,
-                      Eigen::VectorXd &x)
-/*
-**  min cTx, s.t. Ax<=b
-**  dim(x) << dim(b)
-*/
-{
-    int d = c.size();
-    int m = b.size() + 1;
-    x = Eigen::VectorXd::Zero(d);
-    double minimum = INFINITY;
-
-    int *perm, *next, *prev;
-    double *halves, *n_vec, *d_vec, *work, *opt;
-    int i, status = sdlp::AMBIGUOUS;
-
-    perm = (int *)malloc((m - 1) * sizeof(int));
-    next = (int *)malloc(m * sizeof(int));
-    /* original allocated size is m, here changed by m + 1 for legal tail accessing */
-    prev = (int *)malloc((m + 1) * sizeof(int));
-    halves = (double *)malloc(m * (d + 1) * sizeof(double));
-    n_vec = (double *)malloc((d + 1) * sizeof(double));
-    d_vec = (double *)malloc((d + 1) * sizeof(double));
-    work = (double *)malloc((m + 3) * (d + 2) * (d - 1) / 2 * sizeof(double));
-    opt = (double *)malloc((d + 1) * sizeof(double));
-
-    Eigen::Map<Eigen::MatrixXd> Af(halves, d + 1, m);
-    Eigen::Map<Eigen::VectorXd> nv(n_vec, d + 1);
-    Eigen::Map<Eigen::VectorXd> dv(d_vec, d + 1);
-    Eigen::Map<Eigen::VectorXd> xf(opt, d + 1);
-
-    Af.col(0).setZero();
-    Af(d, 0) = 1.0;
-    Af.topRightCorner(d, m - 1) = -A.transpose();
-    Af.bottomRightCorner(1, m - 1) = b.transpose();
-    nv.head(d) = c;
-    nv(d) = 0.0;
-    dv.setZero();
-    dv(d) = 1.0;
-
-    /* randomize the input planes */
-    rand_permutation(m - 1, perm);
-    /* previous to 0 is actually never used */
-    prev[0] = 0;
-    /* link the zero position in at the beginning */
-    next[0] = perm[0] + 1;
-    prev[perm[0] + 1] = 0;
-    /* link the other planes */
-    for (i = 0; i < m - 2; i++)
-    {
-        next[perm[i] + 1] = perm[i + 1] + 1;
-        prev[perm[i + 1] + 1] = perm[i] + 1;
-    }
-    /* flag the last plane */
-    next[perm[m - 2] + 1] = m;
-
-    status = sdlp::linfracprog(halves, 0, m, n_vec, d_vec,
-                               d, opt, work, next, prev, m);
-
-    /* handle states for linprog whose definitions differ from linfracprog */
-    if (status != sdlp::INFEASIBLE)
-    {
-        if (xf(d) != 0.0 && status != sdlp::UNBOUNDED)
-        {
-            x = xf.head(d) / xf(d);
-            minimum = c.dot(x);
-        }
-
-        if (xf(d) == 0.0 || status == sdlp::UNBOUNDED)
-        {
-            x = xf.head(d);
-            minimum = -INFINITY;
-        }
-    }
-
-    free(perm);
-    free(next);
-    free(prev);
-    free(halves);
-    free(n_vec);
-    free(d_vec);
-    free(work);
-    free(opt);
-
-    return minimum;
-}
-
-} // namespace sdlp
-
-#endif

NeuralTraj/src/plan_manage/include/plan_manage/plan_manage.h

@@ -1,54 +0,0 @@
-#ifndef PLAN_MANAGE_HPP
-#define PLAN_MANAGE_HPP
-#include <memory>
-#include <set>
-#include <string>
-#include <thread>
-#include <iostream>
-#include <sstream>
-#include <ros/ros.h>
-#include <path_searching/kino_astar.h>
-#include <tools/visualization.hpp>
-#include <tools/CorridorBuilder2d.hpp>
-#include <torch/script.h>
-#include <path_searching/nnPath.h>
-#include <nav_msgs/Odometry.h>
-
-#define BUDGET 0.1
-namespace plan_manage{
-    class PlanManager{
-        public:
-            PlanManager(){};
-            void init(ros::NodeHandle & nh);
-        private:
-            bool hasTarget = false, hasOdom = false;
-            Eigen::Vector3d targetPose, odom;
-            std::shared_ptr<visualization::Visualization> vis_tool;
-            std::shared_ptr<Config> config_;
-            std::unique_ptr<path_searching::KinoAstar> kino_path_finder_,kino_path_finder_2;
-            std::unique_ptr<path_searching::NnPathSearch> neural_path_finder_;
-
-            map_util::OccMapUtil gridMap;
-            Eigen::Matrix<double, 2, 3> iniState2d, finState2d;
-            Eigen::MatrixXd initInnerPts2d;
-            std::vector<Eigen::MatrixXd> hPolys;
-            void process(const ros::TimerEvent &);
-            void warm(const ros::TimerEvent &);
-            void targetCallback(const geometry_msgs::PoseStamped &msg);
-            void odomCallback(const nav_msgs::OdometryPtr &msg);
-            double pieceTime;
-
-            /*ros related*/
-            ros::Timer processTimer, cudaWarmTimer;
-            ros::Subscriber targetSub, odomSub;
-            ros::Publisher trajCmdPub;
-            int startid = 1, pathid = 1;
-            std::string scene;
-
-
-
-
-    };
-}
-
-#endif

NeuralTraj/src/plan_manage/launch/plan_node.launch

@@ -1,11 +0,0 @@
-<launch>
-
-
-<node pkg="plan_manage" type="plan_node" name="plan_node" output="screen">
-    <rosparam file="$(find plan_manage)/config/planning_ruin.yaml" command="load" />
-    <param name="scene" type="str" value="ruins" />
-</node>
-
-
-
-  </launch>

NeuralTraj/src/plan_manage/launch/plan_node2.launch

@@ -1,11 +0,0 @@
-<launch>
-
-
-<node pkg="plan_manage" type="plan_node" name="plan_node" output="screen">
-    <rosparam file="$(find plan_manage)/config/planning_office.yaml" command="load" />
-    <param name="scene" type="str" value="office" />
-</node>
-
-
-
-  </launch>

NeuralTraj/src/plan_manage/launch/plan_node3.launch

@@ -1,11 +0,0 @@
-<launch>
-
-
-<node pkg="plan_manage" type="plan_node" name="plan_node" output="screen">
-    <rosparam file="$(find plan_manage)/config/planning_forest.yaml" command="load" />
-    <param name="scene" type="str" value="forest" />
-</node>
-
-
-
-  </launch>

NeuralTraj/src/plan_manage/package.xml

@@ -1,36 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>plan_manage</name>
-
-  <maintainer email="zhichaohan@zju.edu.cn">zhichaohan</maintainer>
-
-  <version>0.1.0</version>
-  <description> the plan_manage package</description>
-  <license>GPLV3</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>roscpp</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>tools</build_depend>
-  <build_depend>path_search</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>decomp_ros_utils</build_depend>
-  <build_depend>mpc_controller</build_depend>
-  
-  <run_depend>roscpp</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>visualization_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>tools</run_depend>
-  <run_depend>path_search</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>decomp_ros_utils</run_depend>
-  <run_depend>mpc_controller</run_depend>
-
-
-
-</package>
-

NeuralTraj/src/plan_manage/rviz/default.rviz

@@ -1,691 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /Grid1
-        - /local_map1
-        - /sfc1
-        - /MarkerArray1
-        - /Marker1
-        - /MarkerArray2
-        - /PointCloud21
-        - /AstarPath1
-        - /PointCloud22
-        - /MarkerArray3
-        - /MarkerArray4
-        - /Path2
-        - /Path3
-        - /Path4
-        - /Path5
-        - /Path6
-        - /PointCloud23
-        - /PointCloud24
-        - /PointCloud25
-        - /PointCloud26
-        - /gtarrow1
-      Splitter Ratio: 0.5
-    Tree Height: 719
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Name: Time
-    SyncMode: 0
-    SyncSource: PointCloud2
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 0.5
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 100
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 52; 101; 164
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: local_map
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 10
-      Size (m): 0.05000000074505806
-      Style: Flat Squares
-      Topic: /ugv/laser
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Angle Tolerance: 9.999999747378752e-05
-      Class: rviz/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: true
-      Keep: 1
-      Name: Odometry
-      Position Tolerance: 9.999999747378752e-05
-      Queue Size: 10
-      Shape:
-        Alpha: 1
-        Axes Length: 0.30000001192092896
-        Axes Radius: 0.10000000149011612
-        Color: 255; 25; 0
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Axes
-      Topic: /ugv/odometry
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 196; 160; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: frontend
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/kinoPath
-      Unreliable: false
-      Value: true
-    - Alpha: 0.10000000149011612
-      BoundColor: 255; 0; 0
-      Class: decomp_rviz_plugins/PolyhedronArray
-      Enabled: false
-      MeshColor: 0; 170; 255
-      Name: sfc
-      Queue Size: 10
-      Scale: 0.10000000149011612
-      State: Mesh
-      Topic: /visualization/sfc
-      Unreliable: false
-      Value: false
-      VsColor: 0; 255; 0
-      VsScale: 1
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 164; 0; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: backend
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/optTraj
-      Unreliable: false
-      Value: true
-    - Class: rviz/Marker
-      Enabled: false
-      Marker Topic: /ugv/mesh
-      Name: ackmanCar
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /predict_path
-      Name: predict
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /reference_path
-      Name: reference
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /visualization/optArrowTraj
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /random_map/mesh_obstacles
-      Name: Marker
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: false
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 0
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/NewTraj
-      Unreliable: false
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /visualization/optArrowTraj2
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.05000000074505806
-      Style: Flat Squares
-      Topic: /global_esdf
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 92; 53; 102
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: AstarPath
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/AstarPath
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 0
-        Min Value: 0
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: AxisColor
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Spheres
-      Topic: /global_map
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /visualization/debugTraj
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /visualization/waittoRefine
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/refinedTraj
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 32; 74; 135
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/refinedTraj2
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 0; 0; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/debugRefinedTraj2
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: false
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/optTraj_kino
-      Unreliable: false
-      Value: false
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 252; 233; 79
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.05000000074505806
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /visualization/debugSe2Traj
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 0
-        Min Value: 0
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Boxes
-      Topic: /global_esdf
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /egeneration_node/mesh_obstacles
-      Name: Marker
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 164; 0; 0
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.20000000298023224
-      Style: Spheres
-      Topic: /visualization/wapoints
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /visualization/arrowTraj
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: ""
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: ""
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Boxes
-      Topic: /global_pcl
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 0
-        Min Value: 0
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 0; 0; 0
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 15
-      Size (m): 0.20000000298023224
-      Style: Points
-      Topic: /global_cloud
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /visualization/gtarrowTraj
-      Name: gtarrow
-      Namespaces:
-        "": true
-      Queue Size: 100
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /visualization/nnarrowTraj
-      Name: nnarrow
-      Namespaces:
-        "": true
-      Queue Size: 100
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 255; 255; 255
-    Default Light: true
-    Fixed Frame: world
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Angle: -0.039998609572649
-      Class: rviz/TopDownOrtho
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Scale: 89.17689514160156
-      Target Frame: <Fixed Frame>
-      X: -1.6424437761306763
-      Y: -1.2539986371994019
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 1016
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000039500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1848
-  X: 72
-  Y: 27

NeuralTraj/src/plan_manage/src/plan_manage.cpp

@@ -1,429 +0,0 @@
-#include <plan_manage/plan_manage.h>
-#include <tf/tf.h>
-#include <tools/tic_toc.hpp>
-#include <arcPlan/Trajopt_alm.hpp>
-#include <fstream>
-#include <random>
-using namespace plan_manage;
-using namespace std;
-void PlanManager::init(ros::NodeHandle& nh){
-    vis_tool.reset(new visualization::Visualization(nh));
-    config_.reset(new Config(nh));
-    gridMap.setParam(config_, nh);
-    //read map
-
-
-
-
-
-
-    hasTarget = false;
-    hasOdom = false;
-    processTimer = nh.createTimer(ros::Duration(0.02), &PlanManager::process, this);
-    cudaWarmTimer = nh.createTimer(ros::Duration(0.01), &PlanManager::warm,this);
-
-    targetSub = nh.subscribe("/move_base_simple/goal", 1, &PlanManager::targetCallback, this);
-    odomSub = nh.subscribe("/ugv/odometry", 1, &PlanManager::odomCallback, this);
-    vis_tool->registe<nav_msgs::Path>("/visualization/kinoPath");
-    vis_tool->registe<nav_msgs::Path>("/visualization/kinoPathNn");
-    vis_tool->registe<nav_msgs::Path>("/visualization/optTraj");
-    vis_tool->registe<nav_msgs::Path>("/visualization/optTraj_kino");
-    vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/debugTraj");
-    vis_tool->registe<nav_msgs::Path>("/visualization/NewTraj");
-    vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/waittoRefine");
-    vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/optArrowTraj");
-    vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/optArrowTraj2");
-    vis_tool->registe<decomp_ros_msgs::PolyhedronArray>("/visualization/sfc");
-    vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj2");
-    vis_tool->registe<nav_msgs::Path>("/visualization/debugRefinedTraj2");
-    vis_tool->registe<nav_msgs::Path>("/visualization/debugSe2Traj");
-    vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/arrowTraj");
-
-    vis_tool->registe<sensor_msgs::PointCloud2>("/visualization/wapoints_nn");
-    vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj_nn");
-    vis_tool->registe<visualization_msgs::Marker>("/visualization/fullshapeTraj_nn");
-
-    vis_tool->registe<sensor_msgs::PointCloud2>("/visualization/wapoints_kinoastar");
-    vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj_kinoastar");
-    vis_tool->registe<visualization_msgs::Marker>("/visualization/fullshapeTraj_kinoastar");
-
-
-    pieceTime = config_->pieceTime;
-    ROS_INFO("Begin to read map");
-
-    double flatObcs[30000] = {0};
-    nh.getParam("scene", scene);
-    
-    std::ifstream fin(ros::package::getPath("plan_manage") + "/testdata/"+scene+"/obcs/obc"+std::to_string(29500)+std::string(".dat"), std::ios::binary);
-    fin.read((char*)flatObcs, sizeof(double) * 30000);
-    fin.close();
-    pcl::PointCloud<pcl::PointXYZ> cloud_map;
-    cloud_map.points.clear();
-    for(int j = 0; j < 30000; j+=2){
-        pcl::PointXYZ pt;
-        pt.x = flatObcs[j];
-        pt.y = flatObcs[j+1];
-        pt.z = 0.0;
-        cloud_map.points.push_back(pt);
-    }
-    cloud_map.width = cloud_map.points.size();
-    cloud_map.height = 1;
-    cloud_map.is_dense = true;
-    sensor_msgs::PointCloud2 global_msg;
-    pcl::toROSMsg(cloud_map, global_msg);
-    global_msg.header.frame_id = "world";
-    gridMap.setEnv(global_msg);
-
-    std::cout << "Check Point1" << std::endl;
-    neural_path_finder_.reset(new path_searching::NnPathSearch);
-    std::cout << "Check Point2" << std::endl;
-    neural_path_finder_->init(config_,nh);
-    std::cout << "Check Point3" << std::endl;
-    neural_path_finder_->intialMap(&gridMap);
-    ROS_INFO("Finish Reading Map");
-
-    kino_path_finder_.reset(new path_searching::KinoAstar);
-    kino_path_finder_->init(config_, nh, true);  
-    kino_path_finder_->intialMap(&gridMap);
-    
-    kino_path_finder_2.reset(new path_searching::KinoAstar);
-    kino_path_finder_2->init(config_, nh);  
-    kino_path_finder_2->intialMap(&gridMap);
-
-
-
-}
-void PlanManager::warm(const ros::TimerEvent &){
-    neural_path_finder_->warm();
-    if(kino_path_finder_->use_network){
-        kino_path_finder_->warm();
-    }
-    return;
-}
-
-
-void PlanManager::odomCallback(const nav_msgs::OdometryPtr &msg)
-{
-    odom[0] = msg->pose.pose.position.x;
-    odom[1] = msg->pose.pose.position.y;
-    Eigen::Quaterniond q(msg->pose.pose.orientation.w,
-                msg->pose.pose.orientation.x,
-                msg->pose.pose.orientation.y,
-                msg->pose.pose.orientation.z);
-    Eigen::Matrix3d R(q);
-    odom[2] = atan2(R.col(0)[1],R.col(0)[0]);
-    hasOdom = true;
-    return;
-}
-void PlanManager::targetCallback(const geometry_msgs::PoseStamped &msg){
-    ROS_INFO("Recieved target!");
-    targetPose <<  msg.pose.position.x, msg.pose.position.y,
-                tf::getYaw(msg.pose.orientation);
-    std::cout<<"targetPose: "<<targetPose.transpose()<<std::endl;
-    hasTarget = true;
-    return;
-}
-void PlanManager::process(const ros::TimerEvent &){
-    if(!gridMap.has_map_()||pathid>=51) return;
-    // neural_path_finder_->intialMap(&gridMap);
-    gridMap.publishPCL();
-    // std::cout << "pcl number: "<<gridMap.getCloud().size()<<std::endl;
-    if(!hasTarget || !hasOdom) return;
-    ROS_WARN("Triggering------------------------------------ we begin to plan a trajectory!");
-    hasTarget = false;
-    for(startid=29551;startid<29600;startid++){
-        double flatObcs[30000] = {0};
-        std::ifstream fin(
-            ros::package::getPath("plan_manage") + "/testdata/"+scene+"/obcs/obc"+std::to_string(startid)+std::string(".dat"), std::ios::binary);
-        fin.read((char*)flatObcs, sizeof(double) * 30000);
-        fin.close();
-        pcl::PointCloud<pcl::PointXYZ> cloud_map;
-        cloud_map.points.clear();
-        for(int j = 0; j < 30000; j+=2){
-            pcl::PointXYZ pt;
-            pt.x = flatObcs[j];
-            pt.y = flatObcs[j+1];
-            pt.z = 0.0;
-            cloud_map.points.push_back(pt);
-        }
-        cloud_map.width = cloud_map.points.size();
-        cloud_map.height = 1;
-        cloud_map.is_dense = true;
-        sensor_msgs::PointCloud2 global_msg;
-        pcl::toROSMsg(cloud_map, global_msg);
-        global_msg.header.frame_id = "world";
-        gridMap.setEnv(global_msg);
-        neural_path_finder_->intialMap(&gridMap);
-        kino_path_finder_->intialMap(&gridMap);
-        kino_path_finder_2->intialMap(&gridMap);
-        
-//
-        for(pathid=5;pathid<51; pathid++){
-            double data[1210] = {0};
-            std::ifstream fin(ros::package::getPath("plan_manage") + "/testdata/"+scene + "/e"+std::to_string(startid)+"/path"+std::to_string(pathid)+".dat", std::ios::binary);
-            if (!fin.good()){
-                continue;
-            }//hzchzcasds
-            fin.read((char*)data, sizeof(double) * 1210);
-            fin.close();
-            targetPose << data[1205], data[1206], data[1207];
-            odom << data[11], data[12], data[13];
-            bool checkOcc = false;
-            gridMap.CheckIfCollisionUsingPosAndYaw(targetPose, &checkOcc, 0.3);
-            if(checkOcc)
-                continue;
-
-            gridMap.CheckIfCollisionUsingPosAndYaw(odom, &checkOcc, 0.3);
-            if(checkOcc)
-                continue;
-
-            if((odom-targetPose).head(2).norm() < 15)
-                continue;
-            gridMap.publishPCL();
-            for(int i = 0; i < 30; i++){
-                neural_path_finder_->warm();    
-            }
-            // std::cout << "mapid: "<<startid << "pathid: "<<pathid <<std::endl;
-
-            {
-                //neural path planning
-                neural_path_finder_->reset();
-                double t1 = ros::Time::now().toSec();
-                neural_path_finder_->search(odom, targetPose);
-                double t2 = ros::Time::now().toSec();
-                // ROS_INFO_STREAM("neural_path_finder time: "<<1000.0*(t2-t1)<<" ms");
-                
-                neural_path_finder_->display();  
-                //use neural network to get initial guess
-                path_searching::KinoTrajData nn_trajs_;
-                neural_path_finder_->getKinoNode(nn_trajs_);
-                int segnum = nn_trajs_.size();
-                std::vector<int> refined_singuals; refined_singuals.resize(segnum);
-                Eigen::VectorXd refined_rt; refined_rt.resize(segnum);//uniform piece-wise polynomial
-                std::vector<Eigen::MatrixXd> refined_inPs_container; refined_inPs_container.resize(segnum);
-                std::vector<Eigen::Vector2d> refined_gearPos;refined_gearPos.resize(segnum - 1);
-                std::vector<double> refined_angles; refined_angles.resize(segnum - 1);
-                double basetime = 0.0;
-                for(int i = 0; i < segnum; i++){
-                    double timePerPiece = pieceTime;
-                    path_searching::FlatTrajData nn_traj = nn_trajs_.at(i);
-                    refined_singuals[i] = nn_traj.singul;
-                    int piece_nums;
-                    double initTotalduration = nn_traj.duration;//hzchzc
-                    piece_nums = std::max(int(initTotalduration / timePerPiece + 0.5),1);
-                    double dt = initTotalduration / piece_nums; 
-                    refined_rt[i] = (initTotalduration / piece_nums);
-                    refined_inPs_container[i].resize(2, piece_nums - 1);
-                    for(int j = 0; j < piece_nums - 1; j++ ){
-                        double t = basetime + (j+1)*dt;
-                        Eigen::Vector3d pos = neural_path_finder_->evaluatePos(t);
-                        refined_inPs_container[i].col(j) = pos.head(2);
-                    }
-                    if(i >=1){
-                        Eigen::Vector3d pos = neural_path_finder_->evaluatePos(basetime);
-                        refined_gearPos[i-1] = pos.head(2);
-                        refined_angles[i-1] = pos[2];
-                    }
-                    basetime += initTotalduration;
-                }
-                iniState2d << odom[0], refined_singuals[0] * cos(odom[2]), 0.0,
-                            odom[1], refined_singuals[0] * sin(odom[2]), 0.0;
-
-                finState2d << targetPose[0], refined_singuals[segnum-1] * cos(targetPose[2]), 0.0,
-                            targetPose[1], refined_singuals[segnum-1] * sin(targetPose[2]), 0.0;
-
-
-                // ROS_INFO("begin to refine");
-                PolyTrajOpt::UgvTrajectory optTraj;
-                PolyTrajOpt::TrajOpt refinedOpt;
-
-                double t3 = ros::Time::now().toSec();
-                int flagSs = refinedOpt.OptimizeSe2Trajectory(
-                iniState2d, finState2d, refined_rt,
-                refined_inPs_container, refined_gearPos,
-                refined_angles, &gridMap,  config_, refined_singuals, vis_tool);
-                double t4 = ros::Time::now().toSec();
-
-                printf("\033[32mNeural Network path finder time!,time(ms)=%5.3f \n\033[0m", (t2-t1) * 1000.0-1.0);
-                printf("\033[32mBackend trajectory optimizer time!,time(ms)=%5.3f \n\033[0m", (t4-t3) * 1000.0);
-                
-            }
-            /*for benchmark hybridAstar+transformer*/
-            {
-                kino_path_finder_->reset();
-                Eigen::Vector4d iniFs, finFs;
-                Eigen::Vector2d initCtrl;
-                iniFs << odom, 1.0;
-                initCtrl.setZero();
-                finFs << targetPose, 1.0;
-                TicToc time_profile_tool_;
-                time_profile_tool_.tic();
-                path_searching::KinoTrajData kino_trajs_;
-                double model_time;
-                for(int i = 0; i < 20; i++){
-                    kino_path_finder_->warm();    
-                }
-                double t1 = ros::Time::now().toSec();
-                int status = kino_path_finder_->search(iniFs, initCtrl, finFs, model_time, true);
-                double t2 = ros::Time::now().toSec();
-                if(status!=0){
-                    kino_path_finder_->getKinoNode(kino_trajs_);
-                    std::vector<Eigen::Vector3d> visKinoPath;
-                    for(double t = 0.0; t < kino_path_finder_-> totalTrajTime; t += 0.01){
-                        Eigen::Vector3d pos;
-                        pos = kino_path_finder_->evaluatePos(t);
-                        pos[2] = 0.2;
-                        visKinoPath.push_back(pos);
-                    }
-                    vis_tool->visualize_path(visKinoPath, "/visualization/kinoPathNn");
-                    //use neural network to get initial guess？？？
-                    int segnum = kino_trajs_.size();
-                    std::vector<int> refined_singuals; refined_singuals.resize(segnum);
-                    Eigen::VectorXd refined_rt;
-                    refined_rt.resize(segnum);
-                    std::vector<Eigen::MatrixXd> refined_inPs_container;
-                    refined_inPs_container.resize(segnum);
-                    std::vector<Eigen::Vector2d> refined_gearPos;
-                    std::vector<double> refined_angles; 
-                    refined_gearPos.resize(segnum - 1); refined_angles.resize(segnum - 1);
-                    double basetime = 0.0;
-                    for(int i = 0; i < segnum; i++){
-                        double timePerPiece = pieceTime;
-                        path_searching::FlatTrajData kino_traj = kino_trajs_.at(i);
-                        refined_singuals[i] = kino_traj.singul;
-                        std::vector<Eigen::Vector3d> pts = kino_traj.traj_pts;
-                        int piece_nums;
-                        double initTotalduration = 0.0;
-                        for(const auto pt : pts){
-                            initTotalduration += pt[2];
-                        }
-                        piece_nums = std::max(int(initTotalduration / timePerPiece + 0.5),1);
-                        double dt = initTotalduration / piece_nums; 
-                        refined_rt[i] = (initTotalduration / piece_nums);
-
-                        refined_inPs_container[i].resize(2, piece_nums - 1);
-                        for(int j = 0; j < piece_nums - 1; j++ ){
-                            double t = basetime + (j+1)*dt;
-                            Eigen::Vector3d pos = kino_path_finder_->evaluatePos(t);
-                            refined_inPs_container[i].col(j) = pos.head(2);
-                        }
-                        if(i >=1){
-                            Eigen::Vector3d pos = kino_path_finder_->evaluatePos(basetime);
-                            refined_gearPos[i-1] = pos.head(2);
-                            refined_angles[i-1] = pos[2];
-                        }
-                        basetime += initTotalduration;
-                    }
-                    iniState2d << odom[0], refined_singuals[0] * cos(odom[2]), 0.0,
-                                odom[1], refined_singuals[0] * sin(odom[2]), 0.0;
-
-                    finState2d << targetPose[0], refined_singuals[segnum-1] * cos(targetPose[2]), 0.0,
-                                targetPose[1], refined_singuals[segnum-1] * sin(targetPose[2]), 0.0;
-                    // ROS_INFO("begin to refine");
-                    PolyTrajOpt::UgvTrajectory optTraj;
-                    PolyTrajOpt::TrajOpt refinedOpt;
-                    double t3 = ros::Time::now().toSec();
-                    int flagSs = refinedOpt.OptimizeSe2Trajectory(
-                    iniState2d, finState2d, refined_rt,
-                    refined_inPs_container, refined_gearPos,
-                    refined_angles, &gridMap,  config_, refined_singuals, vis_tool,"kinoastar");
-                    double t4 = ros::Time::now().toSec();
-                    printf("\033[32mKinoastar+transformer path finder time!,time(ms)=%5.3f \n\033[0m", (t2-t1) * 1000.0);
-                    printf("\033[32mBackend trajectory optimizer time!,time(ms)=%5.3f \n\033[0m", (t4-t3) * 1000.0);
-                 
-                }
-                else{
-                   ROS_ERROR("kinoastar+transformer failed!!!");
-                }
-            }
-            /*for benchmark hybridAstar*/
-            {
-                kino_path_finder_2->reset();
-                Eigen::Vector4d iniFs, finFs;
-                Eigen::Vector2d initCtrl;
-                iniFs << odom, 1.0;
-                initCtrl.setZero();
-                finFs << targetPose, 1.0;
-                TicToc time_profile_tool_;
-                time_profile_tool_.tic();
-                path_searching::KinoTrajData kino_trajs_;
-                double model_time;
-                double t1 = ros::Time::now().toSec();
-                int status = kino_path_finder_2->search(iniFs, initCtrl, finFs, model_time, true);
-                double t2 = ros::Time::now().toSec();
-                if(status!=0){
-                    kino_path_finder_2->getKinoNode(kino_trajs_);
-                    std::vector<Eigen::Vector3d> visKinoPath;
-                    for(double t = 0.0; t < kino_path_finder_2-> totalTrajTime; t += 0.01){
-                        Eigen::Vector3d pos;
-                        pos = kino_path_finder_2->evaluatePos(t);
-                        pos[2] = 0.2;
-                        visKinoPath.push_back(pos);
-                    }
-                    vis_tool->visualize_path(visKinoPath, "/visualization/kinoPath");
-                    //use neural network to get initial guess？？？
-                    int segnum = kino_trajs_.size();
-                    std::vector<int> refined_singuals; refined_singuals.resize(segnum);
-                    Eigen::VectorXd refined_rt;
-                    refined_rt.resize(segnum);
-                    std::vector<Eigen::MatrixXd> refined_inPs_container;
-                    refined_inPs_container.resize(segnum);
-                    std::vector<Eigen::Vector2d> refined_gearPos;
-                    std::vector<double> refined_angles; 
-                    refined_gearPos.resize(segnum - 1); refined_angles.resize(segnum - 1);
-                    double basetime = 0.0;
-                    for(int i = 0; i < segnum; i++){
-                        double timePerPiece = pieceTime;
-                        path_searching::FlatTrajData kino_traj = kino_trajs_.at(i);
-                        refined_singuals[i] = kino_traj.singul;
-                        std::vector<Eigen::Vector3d> pts = kino_traj.traj_pts;
-                        int piece_nums;
-                        double initTotalduration = 0.0;
-                        for(const auto pt : pts){
-                            initTotalduration += pt[2];
-                        }
-                        piece_nums = std::max(int(initTotalduration / timePerPiece + 0.5),1);
-                        double dt = initTotalduration / piece_nums; 
-                        refined_rt[i] = (initTotalduration / piece_nums);
-
-                        refined_inPs_container[i].resize(2, piece_nums - 1);
-                        for(int j = 0; j < piece_nums - 1; j++ ){
-                            double t = basetime + (j+1)*dt;
-                            Eigen::Vector3d pos = kino_path_finder_2->evaluatePos(t);
-                            refined_inPs_container[i].col(j) = pos.head(2);
-                        }
-                        if(i >=1){
-                            Eigen::Vector3d pos = kino_path_finder_2->evaluatePos(basetime);
-                            refined_gearPos[i-1] = pos.head(2);
-                            refined_angles[i-1] = pos[2];
-                        }
-                        basetime += initTotalduration;
-                    }
-                    iniState2d << odom[0], refined_singuals[0] * cos(odom[2]), 0.0,
-                                odom[1], refined_singuals[0] * sin(odom[2]), 0.0;
-
-                    finState2d << targetPose[0], refined_singuals[segnum-1] * cos(targetPose[2]), 0.0,
-                                targetPose[1], refined_singuals[segnum-1] * sin(targetPose[2]), 0.0;
-                    // ROS_INFO("begin to refine");
-                    PolyTrajOpt::UgvTrajectory optTraj;
-                    PolyTrajOpt::TrajOpt refinedOpt;
-                    double t3 = ros::Time::now().toSec();
-                    int flagSs = refinedOpt.OptimizeSe2Trajectory(
-                    iniState2d, finState2d, refined_rt,
-                    refined_inPs_container, refined_gearPos,
-                    refined_angles, &gridMap,  config_, refined_singuals, vis_tool,"kinoastar");
-                    double t4 = ros::Time::now().toSec();
-                    printf("\033[32mKinoastar path finder time!,time(ms)=%5.3f \n\033[0m", (t2-t1) * 1000.0);
-                    printf("\033[32mBackend trajectory optimizer time!,time(ms)=%5.3f \n\033[0m", (t4-t3) * 1000.0);
-                  
-                }
-                else{
-                   ROS_ERROR("kinoastar failed!!!");
-                }
-
-            }
-            //wait
-            ros::Duration(10.0).sleep();
-        }
-    }
-}

NeuralTraj/src/plan_manage/src/plan_node.cpp

@@ -1,19 +0,0 @@
-#include<plan_manage/plan_manage.h>
-#include <ros/ros.h>
-#include <ros/console.h>
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "global_planning_node");
-    ros::NodeHandle nh("~");
-    plan_manage::PlanManager manager;
-    manager.init(nh);
-    // ros::AsyncSpinner spinner(8); // Use4 threads
-
-    // spinner.start();
-
-    // ros::waitForShutdown();
-
-
-    ros::spin();
-    return 0;
-}