OMPL学习笔记2

最新推荐文章于 2024-07-05 11:39:03 发布

找不到工作的我

最新推荐文章于 2024-07-05 11:39:03 发布

阅读量1.7k

点赞数 2

分类专栏： ompl

本文链接：https://blog.csdn.net/ljq31446/article/details/79714728

版权

ompl 专栏收录该内容

5 篇文章 12 订阅

订阅专栏

本文介绍使用OMPL库进行机器人路径规划，并通过定义不可行区域来检测障碍物。代码示例展示了如何设置状态空间、有效性检查器、起始和目标状态，以及规划并简化解决方案路径。同时，通过Python读取路径文件并可视化路径与障碍物。

摘要由CSDN通过智能技术生成

在1的基础上，加入障碍物。

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

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());
  // State is invalid when it is inside a 1x1 box
  // centered at the origin:
  if(std::fabs(x)<0.5 && std::fabs(y)<0.5)
    return false;
  // Otherwise, the state is valid:
  return true;
}

void planWithSimpleSetup(void)
{
  // Construct the state space where we are planning
  ob::StateSpacePtr space(new ob::SE2StateSpace());

  ob::RealVectorBounds bounds(2);
  bounds.setLow(-1);
  bounds.setHigh(1);
  space->as<ob::SE2StateSpace>()->setBounds(bounds);

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

  // Setup the StateValidityChecker
  ss.setStateValidityChecker(boost::bind(&isStateValid, _1));

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

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

  ss.setStartAndGoalStates(start, goal);

  std::cout << "----------------" << std::endl;

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

  // If we have a solution,
  if (solved)
  {
    // Simplify the solution
    ss.simplifySolution();
    std::cout << "----------------" << std::endl;
    std::cout << "Found solution:" << std::endl;
    // Print the solution path to screen
    ss.getSolutionPath().print(std::cout);

    // Print the solution path to a file
    std::ofstream ofs("../plot/path.dat");
    ss.getSolutionPath().printAsMatrix(ofs);
  }
  else
    std::cout << "No solution found" << std::endl;
}

int main()
{
  planWithSimpleSetup();
  return 0;
}

在代码18-21加入一个不可行区域的定义，-0.5<x<0.5,-0.5<y<0.5,期望工作：童过检测图片像素来获取障碍物信息。

python

from mpl_toolkits.mplot3d import Axes3D
import numpy
import matplotlib.pyplot as plt
data = numpy.loadtxt('path.dat')
data1= numpy.loadtxt('obstacle.dat')
fig = plt.figure()
#ax = fig.gca(projection='3d')
plt.plot(data[:,0],data[:,1],'.-')
plt.hold('on')
plt.grid('on')
plt.fill(data1[:,0],data1[:,1],'.-')
plt.hold('on')
plt.grid('on')
plt.show()