改写代码地址
基于开学初用Windows下的vs2019的框架改了改，基本工程搭建过程见：vs2019+VREP 基本工程框架
借用vrep界面展示规划的路径，vrep主要是记录robot的轨迹,robot的位置由c++程序直接设置，vrep类的定义如下：
vrep.h
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31#pragma once #include"extApi.h" #include"simConst.h" #include"extApiPlatform.h" #include <math.h> #include <iostream> #include <vector> namespace vrepspace { class vrep { public: void vrep_connect(const char* ip); void vrep_start(); void vrep_stop(); void vrep_setpose2d(int handle, float x, float y); void getobjectpos(int handle, float* pos); int client_id; int robot_handle; int goal_handle; }; }
vrep.c
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51#include "vrep.h" namespace vrepspace { using namespace std; void vrep::vrep_connect(const char* ip) { int Port = 19997; client_id = simxStart((simxChar*)ip, Port, 1, 1, 1000, 5); vrep_stop(); extApi_sleepMs(100); if (client_id != -1) { cout << "V-rep connected."; simxGetObjectHandle(client_id, "robot", &robot_handle, simx_opmode_blocking); simxGetObjectHandle(client_id, "goal", &goal_handle, simx_opmode_blocking); } else { cout << "V-rep can't be connected."; } } void vrep::vrep_start() { simxStartSimulation(client_id, simx_opmode_oneshot); cout << "simulation started ...... " << endl; extApi_sleepMs(100); } void vrep::vrep_stop() { simxStopSimulation(client_id, simx_opmode_oneshot); } void vrep::vrep_setpose2d(int handle, float x, float y) { float position[3] = { x, y, 0 }; simxSetObjectPosition(client_id, (simxInt)handle, -1, position,simx_opmode_oneshot); } void vrep::getobjectpos(int handle, float* pos) { simxGetObjectPosition(client_id, handle, -1, pos, simx_opmode_blocking); extApi_sleepMs(50); } }
路径规划部分直接copy的ompl官网的 这个demo
里面的main函数当然要挪到自己的main.c里面去，另外Plane2DEnvironment类里面新增一个drawPath函数，在main.h里面定义，作用是在vrep里面移动位置
Plane2DEnvironment.h如下，路径规划只有一个头文件就够了
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161#pragma once #include <ompl/base/spaces/RealVectorStateSpace.h> #include <ompl/geometric/SimpleSetup.h> #include <ompl/geometric/planners/rrt/RRTstar.h> #include <ompl/geometric/planners/rrt/RRTConnect.h> #include <ompl/geometric/planners/prm/PRMstar.h> #include <ompl/util/PPM.h> #include <ompl/base/samplers/DeterministicStateSampler.h> #include <ompl/base/samplers/deterministic/HaltonSequence.h> #include <ompl/config.h> #include <boost/filesystem.hpp> #include <iostream> namespace ob = ompl::base; namespace og = ompl::geometric; class Plane2DEnvironment { public: Plane2DEnvironment(const char* ppm_file, bool use_deterministic_sampling = false) { bool ok = false; useDeterministicSampling_ = use_deterministic_sampling; try { ppm_.loadFile(ppm_file); ok = true; } catch (ompl::Exception& ex) { OMPL_ERROR("Unable to load %s.n%s", ppm_file, ex.what()); } if (ok) { auto space(std::make_shared<ob::RealVectorStateSpace>()); space->addDimension(0.0, ppm_.getWidth());//x轴长度即图片宽像素数 space->addDimension(0.0, ppm_.getHeight());//y轴长度即图片高像素数 maxWidth_ = ppm_.getWidth() - 1;//可行路径不能在边界上？ maxHeight_ = ppm_.getHeight() - 1; ss_ = std::make_shared<og::SimpleSetup>(space);//初始化采样空间即构型空间？状态空间 // set state validity checking for this space ss_->setStateValidityChecker([this](const ob::State* state) { return isStateValid(state); }); space->setup(); ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent()); // set the deterministic sampler // 2D space, no need to specify bases specifically if (useDeterministicSampling_) { // PRMstar can use the deterministic sampling ss_->setPlanner(std::make_shared<og::PRMstar>(ss_->getSpaceInformation()));//渐进最优的概率路线图*算法 space->setStateSamplerAllocator(std::bind(&Plane2DEnvironment::allocateHaltonStateSamplerRealVector, this, std::placeholders::_1, 2, std::vector<unsigned int>{2, 3})); } } } bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col) { if (!ss_) return false; ob::ScopedState<> start(ss_->getStateSpace()); start[0] = start_row; start[1] = start_col; ob::ScopedState<> goal(ss_->getStateSpace()); goal[0] = goal_row; goal[1] = goal_col; ss_->setStartAndGoalStates(start, goal); // generate a few solutions; all will be added to the goal; for (int i = 0; i < 10; ++i) { if (ss_->getPlanner()) ss_->getPlanner()->clear(); ss_->solve(); } const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount(); OMPL_INFORM("Found %d solutions", (int)ns); if (ss_->haveSolutionPath()) { if (!useDeterministicSampling_) ss_->simplifySolution(); og::PathGeometric& p = ss_->getSolutionPath(); if (!useDeterministicSampling_) { ss_->getPathSimplifier()->simplifyMax(p); ss_->getPathSimplifier()->smoothBSpline(p); } return true; } return false; } void drawPath(); void recordSolution() { if (!ss_ || !ss_->haveSolutionPath()) return; og::PathGeometric& p = ss_->getSolutionPath(); p.interpolate(); for (std::size_t i = 0; i < p.getStateCount(); ++i) { const int w = std::min(maxWidth_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0]); const int h = std::min(maxHeight_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1]); //得到路径上的点的坐标 ompl::PPM::Color& c = ppm_.getPixel(h, w); c.red = 255; c.green = 0; c.blue = 0; //在该坐标处画红点，设置vrep里面的robot位置原理与此一致 } } void save(const char* filename) { if (!ss_) return; ppm_.saveFile(filename); } private: bool isStateValid(const ob::State* state) const { const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_); const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_); const ompl::PPM::Color& c = ppm_.getPixel(h, w); return c.red > 127 && c.green > 127 && c.blue > 127; } ob::StateSamplerPtr allocateHaltonStateSamplerRealVector(const ompl::base::StateSpace* space, unsigned int dim, std::vector<unsigned int> bases = {}) { // specify which deterministic sequence to use, here: HaltonSequence // optionally we can specify the bases used for generation (otherwise first dim prime numbers are used) if (bases.size() != 0) return std::make_shared<ompl::base::RealVectorDeterministicStateSampler>( space, std::make_shared<ompl::base::HaltonSequence>(bases.size(), bases)); else return std::make_shared<ompl::base::RealVectorDeterministicStateSampler>( space, std::make_shared<ompl::base::HaltonSequence>(dim)); } og::SimpleSetupPtr ss_; int maxWidth_; int maxHeight_; ompl::PPM ppm_; bool useDeterministicSampling_; };
main.h 里面定义vrep里面robot移动的过程
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41#pragma once #include "vrep.h" #include "2dpointplanningdemo.h" using namespace std; namespace vp = vrepspace; vp::vrep m_vrep; void Plane2DEnvironment::drawPath() { float x, y; if (ss_->haveSolutionPath()) { if (!useDeterministicSampling_) ss_->simplifySolution(); og::PathGeometric& p = ss_->getSolutionPath(); if (!useDeterministicSampling_) { ss_->getPathSimplifier()->simplifyMax(p); ss_->getPathSimplifier()->smoothBSpline(p); } size_t length = p.getStateCount();//得到路径节点数 //p.interpolate(20); // 已经样条曲线平滑过了就不插补了 cout << "path length is : " << endl << length << endl; for (int i = 0; i < length; i++) { x = (p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0])/100; //注意像素距离转换到vrep里面的实际距离，差100倍 y = (p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1])/100; cout << "path point : " << i << ". x : " << x << ".y : " << y << endl; m_vrep.vrep_setpose2d(m_vrep.robot_handle, x, y); extApi_sleepMs(10); } } }
main.c 主函数
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50/********************************************************************* * @ Harbin Institute of Technology * * @author : HI_FORREST * * @date : 2021.12.27 * * *********************************************************************/ #include "main.h" int main() { m_vrep.vrep_connect("127.0.0.1"); // 连接到本机的vrep程序 m_vrep.vrep_start(); //开始仿真 std::cout << "OMPL version: " << OMPL_VERSION << std::endl; boost::filesystem::path path("E:/c++program/ompl/resources/resources/"); // ompl安装文件夹里复制出来的，D:vcpkg-masterbuildtreesomplsrc1.5.1-f5b2356dec.cleantestsresources 。用vcpkg安装的ompl，该文件夹在这里。 bool useDeterministicSampling = true; Plane2DEnvironment env((path/"ppm/floor.ppm").string().c_str(), useDeterministicSampling); float pos[3] = { 0,0,0 }; float target[3] = { 0,0,0 }; m_vrep.getobjectpos(m_vrep.robot_handle, pos); m_vrep.getobjectpos(m_vrep.goal_handle, target);//根据vrep里面的当做起点终点的dummy初始化路径规划的起终点。 int x = (int)(pos[0] * 100); int y = (int)(pos[1] * 100); int tx = (int)(target[0] * 100); int ty = (int)(target[1] * 100);// 该demo以图像像素数为距离进行规划，图片尺寸1895×1568，vrep里面的实际尺寸设为18.95米×15.68米。ppm图片可以用WPS打开！之前专门下了一个xnview mp 软件也可以，但是不如WPS方便。 cout << "start position : x : " << x << " . y : " << y << endl; cout << "goal position : tx : " << tx << " . ty : " << ty << endl; if (env.plan(x, y, tx, ty)) //如果路径规划成功执行以下内容 { env.recordSolution(); cout << "saving path as pictur ... " << endl; env.save("result_demo.ppm"); cout << "showing path in vrep ... " << endl; env.drawPath(); // 看main.h的定义 } return 0; }
ppm地图以左上角为原点，向下为y轴正方形，向右为x轴正方形，因此vrep里面的地图绕x轴转了180°，同时向右上方移动半张图，使原点位于地图左下角。
实际路径在ppm和vrep中如图：
十次规划均有解
最后
以上就是狂野小松鼠最近收集整理的关于【OMPL.demo.1】二维点到点的全部内容，更多相关【OMPL内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复