cpp-CppRoboticscpp实现机器人算法包括定位映射SLAM路径规划和控制

cpp implementation of robotics algorithms including localization, mapping, SLAM, path planning and control

/*************************************************************************
	> File Name: a_star.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Sat Jul 20 12:38:43 2019
 ************************************************************************/

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;


class Node{
public:
  int x;
  int y;
  float sum_cost;
  Node* p_node;

  Node（int x_ int y_ float sum_cost_=0 Node* p_node_=NULL）:x（x_） y（y_） sum_cost（sum_cost_） p_node（p_node_）{};
};


std::vector > calc_final_path（Node * goal float reso cv::Mat& img float img_reso）{
  std::vector rx;
  std::vector ry;
  Node* node = goal;
  while （node->p_node != NULL）{
    node = node->p_node;
    rx.push_back（node->x * reso）;
    ry.push_back（node->y * reso）;
    cv::rectangle（img
        cv::Point（node->x*img_reso+1 node->y*img_reso+1）
        cv::Point（（node->x+1）*img_reso （node->y+1）*img_reso）
        cv::Scalar（255 0 0） -1）;
  }
  return {rx ry};
}


std::vector > calc_obstacle_map（
    std::vector ox std::vector oy
    const int min_ox const int max_ox
    const int min_oy const int max_oy
    float reso float vr
    cv::Mat& img int img_reso）{

  int xwidth = max_ox-min_ox;
  int ywidth = max_oy-min_oy;

  std::vector > obmap（ywidth vector（xwidth 0））;

  for（int i=0; i    int x = i + min_ox;
    for（int j=0; j      int y = j + min_oy;
      for（int k=0; k        float d = std::sqrt（std::pow（（ox[k]-x） 2）+std::pow（（oy[k]-y） 2））;
        if （d <= vr/reso）{
          obmap[i][j] = 1;
          cv::rectangle（img
                        cv::Point（i*img_reso+1 j*img_reso+1）
                        cv::Point（（i+1）*img_reso （j+1）*img_reso）
                        cv::Scalar（0 0 0） -1）;
          break;
        }
      }
    }
  }
  return obmap;
}


bool verify_node（Node* node
                 vector > obmap
                 int min_ox int max_ox
                 int min_oy int max_oy）{
  if （node->x < min_ox || node->y < min_oy || node->x >= max_ox || node->y >= max_oy）{
    return false;
  }

  if （obmap[node->x-min_ox][node->y-min_oy]） return false;

  return true;
}


float calc_heristic（Node* n1 Node* n2 float w=1.0）{
  return w * std::sqrt（std::pow（n1->x-n2->x 2）+std::pow（n1->y-n2->y 2））;
}

std::vector get_motion_model（）{
  return {Node（1   0  1）
          Node（0   1  1）
          Node（-1   0  1）
          Node（0   -1  1）
          Node（-1   -1  std::sqrt（2））
          Node（-1   1  std::sqrt（2））
          Node（1   -1  std::sqrt（2））
          Node（1    1  std::sqrt（2））};
}

void a_star_planning（float sx float sy
                     float gx float gy
                     

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-07-29 06:44  CppRobotics-master\
     文件          40  2019-07-29 06:44  CppRobotics-master\.gitignore
     文件        2064  2019-07-29 06:44  CppRobotics-master\CMakeLists.txt
     文件        1074  2019-07-29 06:44  CppRobotics-master\LICENSE.md
     目录           0  2019-07-29 06:44  CppRobotics-master\gif\
     文件     1424868  2019-07-29 06:44  CppRobotics-master\gif\a_star.gif
     文件       41995  2019-07-29 06:44  CppRobotics-master\gif\csp.png
     文件     1965595  2019-07-29 06:44  CppRobotics-master\gif\dijkstra.gif
     文件      162704  2019-07-29 06:44  CppRobotics-master\gif\dwa.gif
     文件      548715  2019-07-29 06:44  CppRobotics-master\gif\ekf.gif
     文件      379287  2019-07-29 06:44  CppRobotics-master\gif\frenet.gif
     文件      186300  2019-07-29 06:44  CppRobotics-master\gif\lqr_full.gif
     文件      159746  2019-07-29 06:44  CppRobotics-master\gif\lqr_steering.gif
     文件      257280  2019-07-29 06:44  CppRobotics-master\gif\mpc.gif
     文件       26447  2019-07-29 06:44  CppRobotics-master\gif\mptg.gif
     文件     1160296  2019-07-29 06:44  CppRobotics-master\gif\pf.gif
     文件      110500  2019-07-29 06:44  CppRobotics-master\gif\rrt.gif
     文件      130987  2019-07-29 06:44  CppRobotics-master\gif\slp.gif
     目录           0  2019-07-29 06:44  CppRobotics-master\include\
     文件         556  2019-07-29 06:44  CppRobotics-master\include\cpprobotics_types.h
     文件        2279  2019-07-29 06:44  CppRobotics-master\include\csv_reader.h
     文件        4244  2019-07-29 06:44  CppRobotics-master\include\cubic_spline.h
     文件         809  2019-07-29 06:44  CppRobotics-master\include\frenet_path.h
     文件        4013  2019-07-29 06:44  CppRobotics-master\include\motion_model.h
     文件        1591  2019-07-29 06:44  CppRobotics-master\include\quartic_polynomial.h
     文件        1913  2019-07-29 06:44  CppRobotics-master\include\quintic_polynomial.h
     文件        5138  2019-07-29 06:44  CppRobotics-master\include\rrt.h
     文件        5817  2019-07-29 06:44  CppRobotics-master\include\trajectory_optimizer.h
     文件         745  2019-07-29 06:44  CppRobotics-master\include\visualization.h
     文件        2713  2019-07-29 06:44  CppRobotics-master\lookuptable.csv
     文件        5320  2019-07-29 06:44  CppRobotics-master\readme.md
............此处省略14个文件信息