• 大小: 5.91MB
    文件类型: .zip
    金币: 1
    下载: 0 次
    发布日期: 2023-09-24
  • 语言: 其他
  • 标签: C/C  

资源简介

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个文件信息

评论

共有 条评论