资源简介
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个文件信息
- 上一篇:云计算环境下分布式存储关键技术综述
- 下一篇:无敌邮件营销软件V8.1.zip
相关资源
- Go-一个golang的编写的情感分析小程序
- WS2812库 arduino测试通过
- VC全景图拼接算法源码毕业设计+论文
- 超市信息管理系统课程设计.docx
- zlib动态链接库x86&64;
- openssl1.1.1源码
- vc做的小游戏 —— 彩色泡泡
- linux+QT下基于RTP协议的实时视频传输客
- cmake-3.17.2-win64-x64.zip
- C用Easyx图形库编写贪吃蛇.zip
- 图像细化算法
- 北斗模块GPS定位显示
- cpp-zzzJSON是一个纯C实现的最快JSON解析
- cpp-室外基于GPS的无人机分布式编队避
- dotnet-Util是一个netcore平台下的应用框
- 西电-面向对象课件褚华老师所著
- cpp-基于MXNetC框架的CPU实时人脸识别
- gsl-2.6.7z
- Go-基于go语言和beego框架搭建的go博客
- 《编译原理第二版》完整版
-
swift-iOS的Fireba
se快速入门 - vim配置成强大的IDE
- K66_逐飞.zip
- 山东大学软件学院_操作系统实验1
- mingw-w64压缩包
- ImageFusion.zip
- STM32Cube_fw_F4_V1.24.1 CubeMX的F4系列的封装
- 未来教育考试系统.zip
- 西安科技大学 程序设计大赛总决赛原
- 矩阵乘法及两种求逆方法
评论
共有 条评论