problems about the heuristics and the choose of dx dy dt

hi, @karlkurzer Many thanks for the nice code of your contribution. I have read about the paper and also the code you wrote. In the original paper:

Dolgov, D., Thrun, S., Montemerlo, M., & Diebel, J. (2008). Practical search techniques in path planning for autonomous driving. Ann Arbor, 1001(48105), 18-80.

The authors reported that they used two heuristics: the “non-holonomic-without-obstacles”, and the “holonomic-with-obstacles heuristic”. I saw that the code of the first heuristic (algorithm.cpp, updateH() ) :

1 the “non-holonomic-without-obstacles”:

    ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r);
    State* rsStart = (State*)reedsSheppPath.allocState();
    State* rsEnd = (State*)reedsSheppPath.allocState();
    rsStart->setXY(start.getX(), start.getY());
    rsStart->setYaw(start.getT());
    rsEnd->setXY(goal.getX(), goal.getY());
    rsEnd->setYaw(goal.getT());
    reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd);

the "authors augmented that it can be fully pre-computed of and then simply translated and rotated to match the current goal"

However, I found that the current implementation does not pre-computed the cost. It always computes it every time the updateH()function is called". This does not correspond to the original paper. Can you provide some ideas for improving?

2 and the code of the “holonomic-with-obstacles heuristic”:

  // if twoD heuristic is activated determine shortest path
  // unconstrained with obstacles
  if (Constants::twoD && !nodes2D[(int)start.getY() * width + (int)start.getX()].isDiscovered()) {
    //    ros::Time t0 = ros::Time::now();
    // create a 2d start node
    Node2D start2d(start.getX(), start.getY(), 0, 0, nullptr);
    // create a 2d goal node
    Node2D goal2d(goal.getX(), goal.getY(), 0, 0, nullptr);
    // run 2d astar and return the cost of the cheapest path for that node
    nodes2D[(int)start.getY() * width + (int)start.getX()].setG(
      aStar(goal2d, start2d, nodes2D, width, height, configurationSpace, visualization)
      );
    //    ros::Time t1 = ros::Time::now();
    //    ros::Duration d(t1 - t0);
    //    std::cout << "calculated 2D Heuristic in ms: " << d * 1000 << std::endl;
  }

  if (Constants::twoD) {
    // offset for same node in cell
    twoDoffset = sqrt(((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) * ((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) +
                      ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())) * ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())));
    twoDCost = nodes2D[(int)start.getY() * width + (int)start.getX()].getG() - twoDoffset;

  }

The authors said that they used the obstacle map to compute the shortest distance to the goal by performing dynamic programming in 2D. I found that the map used in this project is fixed. If I fully understand the algorithm, in the first heuristic the algorithm uses a statistic map for calculation, and then it uses a dynamic map (including moving object) for calculation. Can you provide some reasons of doing so?

3 My next question refers to the child-node expansions: in the original paper, Reed-Shepp model is used to generate the child node, and

"for some nodes, an additional child is generated by computing an optimal Reed-and-Shepp path from the current state to the goal (assuming an obstacle-free environment)"

"For computational reasons, it is not desirable to apply the Reed-Shepp expansion to every node (especially far from the goal, where most such paths are likely to go through obstacles). In our implementation, we used a simple selection rule, where the Reed-Shepp expansion is applied to one of every N nodes, where N decreases as a function of the cost-to-goal heuristic "

And in the current implementation, the child node is created in "algorithm.cpp":

   nSucc = nPred->createSuccessor(i)

where the data type nPred may be Node2D*or Node3D*, and their implementation:

Node2D:

Node2D* Node2D::createSuccessor(const int i) {
  int xSucc = x + Node2D::dx[i];
  int ySucc = y + Node2D::dy[i];
  return new Node2D(xSucc, ySucc, g, 0, this);
}

the step parameters:

const int Node2D::dx[] = { -1, -1, 0, 1, 1, 1, 0, -1 };
const int Node2D::dy[] = { 0, 1, 1, 1, 0, -1, -1, -1 };

Node3D:

Node3D* Node3D::createSuccessor(const int i) {
  float xSucc;
  float ySucc;
  float tSucc;

  // calculate successor p ositions forward
  if (i < 3) {//前向 Successor
    xSucc = x + dx[i] * cos(t) - dy[i] * sin(t);
    ySucc = y + dx[i] * sin(t) + dy[i] * cos(t);
    tSucc = Helper::normalizeHeadingRad(t + dt[i]);
  }
  // backwards
  else {//后向 Successor
    xSucc = x - dx[i - 3] * cos(t) - dy[i - 3] * sin(t);
    ySucc = y - dx[i - 3] * sin(t) + dy[i - 3] * cos(t);
    tSucc = Helper::normalizeHeadingRad(t - dt[i - 3]);
  }

  return new Node3D(xSucc, ySucc, tSucc, g, 0, this, i);
}

and the step parameters:

// R = 6, 6.75 DEG
const float Node3D::dy[] = { 0,        -0.0415893,  0.0415893};
const float Node3D::dx[] = { 0.7068582,   0.705224,   0.705224};
const float Node3D::dt[] = { 0,         0.1178097,   -0.1178097};

// R = 3, 6.75 DEG
//const float Node3D::dy[] = { 0,        -0.0207946, 0.0207946};
//const float Node3D::dx[] = { 0.35342917352,   0.352612,  0.352612};
//const float Node3D::dt[] = { 0,         0.11780972451,   -0.11780972451};

//const float Node3D::dy[] = { 0,       -0.16578, 0.16578};
//const float Node3D::dx[] = { 1.41372, 1.40067, 1.40067};
//const float Node3D::dt[] = { 0,       0.2356194,   -0.2356194};

So my problem is how to calculate the dx , dy , dt? Is there any equations of calculating these values? Additionally, how to correctly choose an adaptive parameters (like R=?, DEG=?) for a given scene? also, if I want to implement "the Reed-Shepp expansion is applied to one of every N nodes", can you provide some ideas for it?

Many thanks.

karlkurzer / path_planner

problems about the heuristics and the choose of dx dy dt #27