add front_end_deploy

STPP_DEPLOY/NeuralTraj/.catkin_workspace

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

STPP_DEPLOY/NeuralTraj/.vscode/c_cpp_properties.json

@@ -0,0 +1,21 @@
+{
+  "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
+}

STPP_DEPLOY/NeuralTraj/run.sh

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

STPP_DEPLOY/NeuralTraj/src/CMakeLists.txt

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

STPP_DEPLOY/NeuralTraj/src/path_search/CMakeLists.txt

@@ -0,0 +1,73 @@
+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
+)
+

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

@@ -0,0 +1,289 @@
+#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

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

@@ -0,0 +1,32 @@
+#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

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

@@ -0,0 +1,95 @@
+#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

STPP_DEPLOY/NeuralTraj/src/path_search/package.xml

@@ -0,0 +1,28 @@
+<?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>
+

STPP_DEPLOY/NeuralTraj/src/path_search/src/nnPath.cpp

@@ -0,0 +1,422 @@
+#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);
+        }
+        }
+    }
+
+
+}

STPP_DEPLOY/NeuralTraj/src/plan_manage/CMakeLists.txt

@@ -0,0 +1,71 @@
+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}
+)

STPP_DEPLOY/NeuralTraj/src/plan_manage/config/planning_forest.yaml

@@ -0,0 +1,68 @@
+# 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

STPP_DEPLOY/NeuralTraj/src/plan_manage/config/planning_office.yaml

@@ -0,0 +1,70 @@
+# 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

STPP_DEPLOY/NeuralTraj/src/plan_manage/config/planning_ruin.yaml

@@ -0,0 +1,68 @@
+# 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

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

@@ -0,0 +1,129 @@
+#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

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

@@ -0,0 +1,838 @@
+#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

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

@@ -0,0 +1,801 @@
+/* 
+ * 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

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

@@ -0,0 +1,54 @@
+#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

STPP_DEPLOY/NeuralTraj/src/plan_manage/launch/plan_node.launch

@@ -0,0 +1,11 @@
+<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>

STPP_DEPLOY/NeuralTraj/src/plan_manage/launch/plan_node2.launch

@@ -0,0 +1,11 @@
+<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>

STPP_DEPLOY/NeuralTraj/src/plan_manage/launch/plan_node3.launch

@@ -0,0 +1,11 @@
+<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>

STPP_DEPLOY/NeuralTraj/src/plan_manage/package.xml

@@ -0,0 +1,36 @@
+<?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>
+

STPP_DEPLOY/NeuralTraj/src/plan_manage/rviz/default.rviz

@@ -0,0 +1,691 @@
+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

STPP_DEPLOY/NeuralTraj/src/plan_manage/src/plan_manage.cpp

@@ -0,0 +1,429 @@
+#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();
+        }
+    }
+}

STPP_DEPLOY/NeuralTraj/src/plan_manage/src/plan_node.cpp

@@ -0,0 +1,19 @@
+#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;
+}