karlkurzer / path_planner Goto Github PK

I would like to do dynamic planning which uses online generated map. I am generating map using gmapping and set manual false. But I get empty path as result. Any idea on what is wrong?
Could you please an example(launch file etc.) for dynamic case?

Edit: I made it work. But it doesn't work when map origin is not (0,0).

Hi Karl,

I found that the voronoiTerm has been commented in commit 05190a6 with message “fixed a bunch of non critical warnings, and commented some unfinished code”.

I wonder if it is because this term has not been tuned to contribute to a good result or somehow?

Any instruction will be highly appreciated！Thanks!

hi, @karlkurzer
Many thanks for the nice code of your contribution.
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};
when the vehicle driving forward and turn left, dx ,dy and dt should be positive.But,in the situation i=1, the step parameters dy is negative. Can you provide some reasons of doing so?Thank you very much.

Dear Karl,
I've been trying to reach the state you've shown at video "Path Planning with a Hybrid A* Algorithm and Sensor Fusion", but haven't succeed.

At the begging I thought it would be enough to launch velodyne_arlanda.launch, then I tried to dig deeper and found out that in this package there ain't any subscriber to PointCloud or LaserScan. Could you provide names of missing nodes/packages?

The code often breaks out into Segmentation Faults when used with dynamic maps as the co-ordinates x and y may be negative; and when used while setting indices, they result in a negative value. This in turn causes segmentation faults.

A possible solution is to transform the x and y co-ordinates from World to Map before setting indices.

Hi karl:
Do you have more detailed code comments? I don't understand some code.
thank you very much。

Hi Karl,

Do you have a set of instructions to run the hybrid astar? What data does it operate on? Do you have performance numbers (run time) for the algorithm?

Thanks in advance!

Hello all,

I used the path_planner package for indoor robots, but when I'm running it on our maps it's not working, after debugging I finally came to understand that the resolution used inside the code is 1 for internal calculations, and the actual map resolution is not used anywhere for calculations, so I raised the issue who have faced a similar issue.

The modifications you need to do in the code. Go to src/collisiondetection.cpp

1. change "cX = (X + collisionLookup[idx].pos[i].x)" to "cX = (X + collisionLookup[idx].pos[i].x)/mapResolution", similarylfor cY,
   "cY = (Y + collisionLookup[idx].pos[i].y)/mapResolution".
   These are the x, y coordinates of the occupancy grid calculated from world coordinates. Since the map resolution was assumed 1 for calculations, there were issues in finding the path.

Please update on this if anyone has more info on this issue.

Everything is in the title

Hi, Karl,

thank you for your great work and sharing the code, i have read your paper and have some questions regard to how to choose this tiebreaker.
tiebreaker here is to break the admissible of heuristic function, you choose 0.01, but when i have a different turning radius, this value should also be changed? should this value be a function of turning radius or something else?

thank you.
Jialiang Han

Hi!
First of all thanks your work. It seems to be working really nice, however adjusting parameters is a drawback here :)
I tried to launch your node with following parameters:
static const float r = 1.0;
static const float cellSize = 0.05;
in include/constants.h
const float Node3D::dy[] = { 0, -0.00693155, 0.00693155};
const float Node3D::dx[] = { 0.1178097, 0.117537333, 0.117537333};
const float Node3D::dt[] = { 0, 0.1178097, -0.1178097};
in src/node3d.cpp

Also I resized map_demo.pmg 10x times and in file maps/map.yaml I set:
resolution: 0.05
Then at program start the error in the title occurs. Could you help me with this issue?

According to the description in the paper, shouldn't it be 5°?

// 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};

I found two mistakes in the code, I'm not sure whether you were aware of it,

1. As per the Dolgov Et al. paper, the formula for 'p1' in final optimization is,
   Xi.ort(-Xi1)/(Det(Xi)*Det(Xi1)), but in the Smoother::curvatureTerm function you used Det(ΔXi)*Det(ΔXi1).
2. As per the Dolgov Et al. paper, the σκ(∆φi/|∆xi | -kmax) is a quadratic penalty function which after differentiation, you get 2(∆φi/|∆xi | -kmax)*(dki/dxi), instead you just considered the dki/dxi for correction.
   I believe if you correct these parameters the algorithm should work fine with non-zero curvature weight.

This line is at the condition that i equals to 0, it sets dubinsNodes[0]'s predecessor as start node. However, checking dubinsNodes[0] == start would get true, which means the first dubin node will equal to is predecessor, and it brings node duplication at the connection of dubinShot and regular search.

(I am using these node results for velocity profile generation, thus duplicated node definitely affect the angle calculation)

Hello,
I am trying to intergrate this package with another suite that is built via catkin build.
Catkin build on this package does not work correctly (the hybrid_astar package is not created).

I am trying to assess whether it is easier to make my other package catkin_make compatible or make this catkin build compatible.

Can someone point me to how they would recommend making this package catkin build compatible? Related answer: https://answers.ros.org/question/277109/catkin_make-works-but-catkin-build-fails/

Thanks in advance.
Vinny

Hi,

There are some pretty nasty warning in smoother.cpp.
Smoother::voronoiTerm() seems to be broken (unused variables and no return).

Davide

Thank you for sharing, where do you modify the code for the visual search process?

Hey,
Most probably this was intentional but I just wanted to know the reason for keeping these weights 0(in file smoother.h) and what all impacts will it have on changing them.
If we are setting the weights to zero then it means that these terms won't have any effect in the final P then why are we consuming time in calculating the other terms as we know that Pvor and Pcur will turn out to be zero due to their weight being 0.
If I change the value of these weights will it have any effect on the computation time of the algorithm?
Can someone kindly brief on the results for the different value of these weights?

Thank You

Hi, thank you very much for the open-source code.I want to achieve the effect in the video, do you have any suggestions?Do you use local path planning?

Thank you for your code! It's very nice, and I'm interested in it.
But I have meet two problems.
One is How to set the start position and goal position manually? I used the "rostopic pub -1 ..." to publish the two topics. But after running one command, the command will latch for 3 seconds, then another command can be allowed to run. Is there a demo for setting the the start position and goal position manually?
The other problem is how to use a dynamical map? And is there a dynamical map demo for us to use? I want to simulate a scenario like a real car travelling. Is it suitable for this code to do the simulation?
Thank you for your guidance!

Hi,
Thank you very much. This package helps me a lot.
I have some question about the resolution for map-server.
If the resolution is 1, the planner works perfectly. However, the planner will do some error if the value of the resolution is 0.05.

image: ccc.png
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.1
free_thresh: 0.05
negate: 0

I have found the related issue but I dont know how to set value for Node3D::dy[] = { 0, -0.0415893, 0.0415893};

hi @karlkurzer ,
I want to visualize the process of planning but I don't know how to enable this utility.can you help me?
Thank you very much!!

Can you please let me know the model of your Heuristic Function, is it the same as the one in the DolgovEtal_2008 paper or you used something different? If it is the same as the one in the paper (using Voronoi Fields), then can you clarify Equation 1 in the "Path-Cost Function Using the Voronoi Field" section in the paper (PFA for the equation)?

Thanks in Advance,
Tschuss :)

As the title, thank you for your answer!

Hi,

For reference, I'm working on a hybrid A* implementation for the navigation stack here. I've been able to experimentally show that there's at minimum 4 mistakes in the smoother terms in the paper. Because I'm using a NLP solver, I get some debug information that lets me test and have some knowledge about "wow that messed things up" or "wow that helped". I was suspect of the derivations so I did them myself separately and had differences. I haven't deeply looked at this implementation, but I see them reflected here too. I bring this up for visibility and if anyone can independently verify my math, we can add it here too:

• In all derivatives, the term begins with -1 / || delta x||. That -1 should be a 1. I derived this formulation every which way and that's definitely positive.
• in dk/dx-1 the sign of the second term switches to positive because d (delta X) for xi-1 is going to include a -1 term because its (xi - xi-1).
• The second term denominator should be ||delta x|| ^ 2 not (delta x)^2. They're equal, but just watch out. delta x ^2 implies a vector output.
• In this implementation, it looks like you're using the partials of the same term to create the curvature. If its d k / d xi-1, then that should be used in the xi-1 point, not the xi point. I believe you need to evaluate the derivatives at d ki-1 / dxi+1 such that at the local d k / dxi, Xi-1's xi+1 is this terms' contribution. I am not 100% sure on that, but it seems to reason such that the derivative terms for the local neighborhood can impact their directions.
• In this implementation, I think there's an issue with the curvature derivative combination. I'm not completely sure the rigor behind doing 0.25 * kim1 + 0.5 * ki + 0.25 * kip1. This could be just me missing something. @karlkurzer I would absolutely love your input on that. I see it done here too (which seems to be a straight copy) and I can't reason myself through it. If you can help with that, I can upstream any other issues I find while implementing this.

@facontidavide I see you've been active on this repo too, you run into any of this or just testing it out?

here:

Whenever I try for a 500x500 image/matrix the hybrid_a_star crashes within a few seconds but it works for 450x450 , I am not able to find the problem due to which this is occurring. What should I do to make it work for the a 500x500 grid .

Steps to reproduce the error

1. Take a 500x500 image and put it in the maps folder
2. Put the name of the image in maps.yaml
3. roslaunch as given in readme
4. Set the markers ... after 2-3 markers set the hybrid_a_star node should crash

Thank You

Want to clarify it is a simulation only project for now.
I am building a mobile robot-trailer system for indoor environments. My setup includes a Clearpath Robotics Ridgeback robot towing a custom cart. I have changed the Ridgeback to a normal differential drive from its Mecanum drive system. I would like to incorporate this path_planner to the system, is there a way I can model my systems' kinematic constraints to the planner?

Another issue is, I am using Hector SLAM to map the environment which outputs a 2048 x 2048 map image (pgm) with a resolution of 0.05. I am not able to figure out how to tune this planner to work with that map. I tried changing the resolution and vehicle size constants but the it didn't work. The hybrid_astar node crashes as soon as I add a initial_pose and nav goal.

Also getting the following error - [ERROR] [1611054999.409444557]: Ignoring transform for child_frame_id "map" from authority "unknown_publisher" because of a nan value in the transform (nan 0.000000 0.500000) (0.000000 0.000000 0.000000 1.000000)

What should be done?

The dubinsShot function in algorithm.cpp only check whether the node isTraversable, and it doesn't check if the node isOnGrid or not, which may result to node out of map

I am trying to implement your hybrid A* on a map that is of size 1100 x 423, and for some reason, the search crashes after correctly finding the path to the end goal. Is there a work around in the code for this? Or is there a line that we need to account for?

Hey, I want to replace the global_planner in move_base package with this algorithm. Have u done this before? Could u please give me some suggestions? Thanks a lot!

The cost estimation for a 3D node (getC) uses a g value that is a 3D cost and an h value that is from the 2D A*. The 3D cost is always higher than the 2D cost since it has more restrictions. This encourages the algorithm to plan from an early path which results in a breath first search.

By adding a weight to the value of g in node3d.hpp:

/// get the total estimated cost
float getC() const
{
    return 0.8 * g + h;
}

The results become more interesting:

Left is the original and right is after the change. This isn't a complete fix as making the costs equivalent is hard but improves the behavior a lot.

hello!
when I in navigation, I find the goal pose and the path end pose are not the same point,so sometimes the car can't run to goal pose.
can you help me?
thanks!

Smoother::curvatureTerm 还some problem can't get right result

请问这个问题是什么原因？
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "_control" with any
of the following names:

_controlConfig.cmake
_control-config.cmake

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.

"sudo apt install libompl-dev" should be replaced with "sudo apt install ros-kinetic-ompl" instead.

Hi,sometimes,this error will happen, what should i do to change this? thanks
**Error: TF_NAN_INPUT: Ignoring transform for child_frame_id "map" from authority "unknown_publisher" because of a nan value in the transform (nan 0.000000 0.500000) (0.000000 0.000000 0.000000 1.000000)
**

Hi Karl, thx for sharing your codes, they help me a lot!

I got some problems by using maps with resolution of 0.05. I've seen this problem in many other issues, so I changed dy[] dx[] in class node2D and class node3D, I just multiply them by 20 and I also changed Constants::cellSize to 0.05, still it doesn't work! The results are wired, besides it took very long time. Do you have some ideas where I can fix it?

isOnGrid function only performs the car center node check, it doesn't perform any car area check, which may result issue like picture below.

To solve this issue, we can simply do some modifications in collisionDetection.cpp for CollisionDetection::configurationTest function:
-- letting the collision check return false if any car area is out of the map

if (cX >= 0 && (unsigned int)cX < grid->info.width && cY >= 0 && (unsigned int)cY < grid->info.height) {
      if (grid->data[cY * grid->info.width + cX]) {
        return false;
      }
    }
   // modification below:
    else{
      return false;
    }

Thanks a lot for this fantastic work! Currently I'm working on integrating this package on my own robot platform. Since you mentioned this package was tested on your own vehicle, could you please give some suggestions on how this package receives occupancy map which is updated online by other perception packages, and how to interact with the ROS navigation stack for navigation? It would be better if you can provide detailed instructions!

Sorry, this is my first time to implement code in ROS. The catkin_ws/src floder was been created and the code was clone into src. But when I try to catkin_make in the root folder, it gives me errors say cannot find package.xml. So I copied package.xml to root floder(under src), but when I catkin_make it again.
I get below error:

CMake Error at CMakeLists.txt:76 (add_executable):
Cannot find source file:

src/main.cpp

Tried extensions .c .C .c++ .cc .cpp .cxx .m .M .mm .h .hh .h++ .hm .hpp
.hxx .in .txx

CMake Error at CMakeLists.txt:57 (add_library):
Cannot find source file:

/home/garychian/catkin_ws_path_planner/src/src/algorithm.cpp

Tried extensions .c .C .c++ .cc .cpp .cxx .m .M .mm .h .hh .h++ .hm .hpp
.hxx .in .txx

CMake Error at CMakeLists.txt:73 (add_executable):
Cannot find source file:

src/tf_broadcaster.cpp

Tried extensions .c .C .c++ .cc .cpp .cxx .m .M .mm .h .hh .h++ .hm .hpp
.hxx .in .txx

I am so confused right now, the CMakeLists.txt was auto generated by ROS(I think), so what about the CMakeLists.txt under path_planner floder?

anyone can give me some suggestions? Thanks in advantage!

Hi,

Is there any plan to port the project to ROS2 ?
If not, I might try to sync it with either Foxy, Galactic or Rolling.

Let me know :)

Hi All,
When launching manual.launch with the rosparam “ use sim time “ set to true, the path planner no longer works.
Does anyone have any insight why this may be?

I have tried replacing the tf broadcaster with a static transform, but that did not solve this issue.
Any help would be much appreciated.

Cheers,
Vinny

Hi karl:
I changed a map, but the program has no output path points.
This is my modified yaml file：
image: willo.pgm
resolution: 0.05
origin: [-26.600000, -28.200000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
Can you tell me how to modify the program?Thank you！

inline bool isCusp(const std::vector<Node3D>& path, int i) {
    // the author 
    bool revim2 = path[i - 2].getPrim() > 3 ;
    bool revim1 = path[i - 1].getPrim() > 3 ;
    bool revi   = path[i].getPrim() > 3 ;
    bool revip1 = path[i + 1].getPrim() > 3 ;
    //  bool revip2 = path[i + 2].getPrim() > 3 ;

    return (revim2 != revim1 || revim1 != revi || revi != revip1);
}

considering change >3 to >=3 to refer a reverse motion, which is called a cusp point?

Hey Karl,

I am relatively new to ROS and have a question regarding output data. I would like to find out the driving direction of the vehicle. In the visualization tool a change of direction is clearly visible, but it is not reflected in the topics (for example in /sPathVehicle). Is there a possibility that I am overlooking?

As I understand the code a direction change is taken into account in the path selection, however the selected direction is then not published.

Thanks a lot for your help.

With kind regards

Hey Karl,
I like your planner very much and I want to use your planner in my turtlebot. I need move base to run turtlebot. So I wish to put this planner as plugin of global planner into move base.
I followed a tutorials to create plugin: . And I add initialized and makeplan to your code. Also, I changed setMap function to set costmap2D. But it still cant work.
[move_base-10] process has died [pid 17783, exit code -11, cmd /opt/ros/kinetic/lib/move_base/move_base cmd_vel:=navigation_velocity_smoother/raw_cmd_vel odom:=odom scan:=scan __name:=move_base __log:=/home/lin/.ros/log/8e6b9c90-a6bb-11e8-a582-705a0f38cc62/move_base-10.log].
Do you know how to fix it?

algorithm.cpp in line 201 - 208

               if (iPred == iSucc && nSucc->getC() > nPred->getC() + Constants::tieBreaker) {
                  delete nSucc;
                  continue;
                   }
                // if successor is in the same cell and the C value is lower, set predecessor to predecessor of predecessor
             else if (iPred == iSucc && nSucc->getC() <= nPred->getC() + Constants::tieBreaker) {
                  nSucc->setPred(nPred->getPred());
  }

Hi, thank you very much for the open-source code. I am confused about the judgment of the same cell in the code above. The index iPred and iSucc here are the three-dimensional indexes related to the heading angle, which will result in almost no judgment as the same cell as long as the heading angle changes. I think it is not necessary to consider the heading angle when judging whether it is the same cell, just check whether there is already a waypoint in the current grid in the 2D grid. What do you think?