OMPL学习笔记3

自建栅格地图

0 0 0 0 0 0 1 1 1 0 0 0 0 0 0
0 0 0 0 0 0 1 1 1 0 0 0 0 0 0
0 0 0 0 0 0 1 1 1 0 0 0 0 0 0
0 0 0 0 0 0 1 1 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
1 1 1 1 0 0 0 0 0 1 1 1 0 0 0
1 1 1 1 0 0 0 0 0 1 1 1 0 0 0
1 1 1 1 0 0 0 0 0 1 1 1 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0

如果是图像，可以转换成一些2值矩阵，1代表障碍区，0自由空间。

#include <ompl/geometric/SimpleSetup.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <cmath>
#include <ompl/base/SpaceInformation.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <iostream>
#include <fstream>
#include "boost/bind.hpp"
#include <vector>

namespace ob = ompl::base;
namespace og = ompl::geometric;


// Return true if the state is valid, false if the state is invalid
bool isStateValid(const ob::State *state)
{
  const ob::SE2StateSpace::StateType *state_2d = state->as<ob::SE2StateSpace::StateType>();
  const double &x(state_2d->getX()), &y(state_2d->getY());

   int grid[15][15];
  std::ifstream infile;
  infile.open("/home/ljq/ompl_tutorial/OMPL_Sample/GeomPlanningSE2/22.txt");
  while (infile.good()&& !infile.eof())
  {
     for(int i=0;i<15;i++)
     {
         for(int j=0;j<15;j++)
         {
             infile>>grid[i][j];
         }
     }
  }
  infile.close();
  if((grid[int(x)][int(y)]==1.0))
       return false;
   return true;

}

void planWithSimpleSetup(void)
{

  ob::StateSpacePtr space(new ob::SE2StateSpace());
  ob::RealVectorBounds bounds(2);
  bounds.setLow(0);
  bounds.setHigh(15);
  space->as<ob::SE2StateSpace>()->setBounds(bounds);

  // Instantiate SimpleSetup
  og::SimpleSetup ss(space);

  // Setup the StateValidityChecker


  // Setup Start and Goal
  ob::ScopedState<ob::SE2StateSpace> start(space);
  start->setXY(0,5);
  std::cout << "start: "; start.print(std::cout);

  ob::ScopedState<ob::SE2StateSpace> goal(space);
  goal->setXY(10,11);
  std::cout << "goal: "; goal.print(std::cout);

  ss.setStartAndGoalStates(start, goal);
  ob::PlannerPtr planner(new og::RRT(ss.getSpaceInformation()));
  ss.setPlanner(planner);
  ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
  std::cout << "----------------" << std::endl;

  // Execute the planning algorithm
  ob::PlannerStatus solved = ss.solve(1.0);

  // If we have a solution,
  if (solved)
  {
    std::cout << "----------------" << std::endl;
    std::cout << "Found solution:" << std::endl;
    ss.getSolutionPath().print(std::cout);
    std::ofstream ofs("../plot/path.dat");
    ss.getSolutionPath().printAsMatrix(ofs);
  }
  else
    std::cout << "No solution found" << std::endl;
}

int main()
{

  planWithSimpleSetup();
  return 0;
}

from mpl_toolkits.mplot3d import Axes3D
import numpy
import matplotlib.pyplot as plt
data = numpy.loadtxt('path.dat')
data1= numpy.loadtxt('22.txt')
#data1= numpy.loadtxt('obstacle.dat')

x=[]
y=[]
fig = plt.figure()
for i in range(15):
        for j in range(15):
                if (data1[i][j]==1):
                        x.append(i)
                        y.append(j)
                        
                

#ax = fig.gca(projection='3d')
plt.plot(data[:,0],data[:,1],'.-')
plt.hold('on')
plt.grid('on')

plt.scatter(x,y)
plt.hold('on')
plt.grid('on')
plt.show()