【OMPL.demo.1】二维点到点
改写代码地址
基于开学初用Windows下的vs2019的框架改了改，基本工程搭建过程见：vs2019+VREP 基本工程框架
借用vrep界面展示规划的路径，vrep主要是记录robot的轨迹,robot的位置由c++程序直接设置，vrep类的定义如下：
vrep.h
#pragma once
#include"extApi.h"
#include"simConst.h"
#include"extApiPlatform.h"
#include vrep.c
#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如下，路径规划只有一个头文件就够了
#pragma once
#include main.h 里面定义vrep里面robot移动的过程
#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 主函数
/*********************************************************************
* @ 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-master\buildtrees\ompl\src\1.5.1-f5b2356dec.clean\tests\resources 。用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中如图：
十次规划均有解
