Navigation package always fails to find path [ROS Hydro]

[ktiwari9]

Hi there, I am trying to use your navigation package to run A* to find a path using my occupancy grid, which is dynamically updated as and when the robot will move. I publish my occupancy_grid on /local_grid topic from the grid_construction_node and then I want to use this occupancy grid to start at current robot location and to go to /global_goal which is set as 5,0 by my obs_husky node. My rqt_graph is attached below:

I tried sending integral and double type goals but I have no idea why it always fails to find a path.

[ktiwari9]

My fixed frame in rviz is set to odom

[nwotero]

I should probably warn you that this navigation package was written by students for a school project, so I cant guarantee its stability. It might be better to use the standard navigation stack. Regardless, we left it in working condition so I am not sure why you are experiencing problems with it. There is probably some problem with the configuration, more so than can be debugged through the rqt graph. If you could post a bag file that would be helpful.

[ktiwari9]

Was it tested on ROS Hydro with a dynamic occupancy grid map ? In my case I have a dynamic Occupancy Grid that is constantly updated with new sensor readings and is advertised on nav_msgs::OccupancyGrid type topic called /local_grid and I do not have any other prior information or previous static environment map so I am running a SLAM Graph search. I am attaching a screencap of my setup for your reference.

[ktiwari9]

So in my case, uptill line 115 I think everything works, since my code gets the occupancy grid map, get the robot pose, start point and goal position so I guess the callbacks are fine. My map resolution is set to 0.5 so as for line 116, my start node is set to -49,2 whereas my start position is -5,1 and my goal is at 5,1 and then I get no path found error. My start cost is 20. So is the start node position creating a problem ?

[nwotero]

Let me see if I understand correctly: Your pose when running A* is (-5m, 1m) and your startNode is grid cell (-49, 2)? That certainly seems wrong. The start node should be grid cell (-10, 2) if what I just said was correct. I suggest uncommenting out some of the debug output that is in the code and following the execution of the algorithm. You should see the frontier expanding towards grid cell (10, 2).

And this package was developed in hydro, so there should be no problem there.

[nwotero]

My last comment made me think: there is no grid cell (-10, 2), since they must be positive numbers. The algorithm doesn't seem to account for nonzero map origins. If your map origin is not (0, 0), this may be your problem.

[ktiwari9]

Well the problem is that in gazebo my robot starts from -5,1 and proceeds along x axis but for some reason the axes in rviz are rotated and also the robot in rviz starts at 0,0 no matter what. How can I fix this ?

[ktiwari9]

And you are right the start node is -10,2 but then you mean I should always start my robot from 0,0 and go to positive co-ordinates only so that my start node also has positive co-ordinates?

[ktiwari9]

You were right about -ve co-ordiunates for start node. Now I go from 0,1 to 6.5,1 but the compiler complains about [ WARN] [1440730103.276218336, 1.529000000]: MSG to TF: Quaternion Not Properly Normalized from the executePath(). What is going wrong with this one here ?

[nwotero]

So the bug is indeed that the map origin is not used in the calculation forbthe start node. You should fix that in your version of the code.

The quaternion not being normalized is another strange one. I cant debug it without more details

[ktiwari9]

Hmmm I figured that i should use the map origin somehow but I still have no idea how to do this. As for the quaternion I found out that my robot orientation.w component was being set to 0 and it must always be 1 so I got rid of that bit. Now just the map origin problem.

[ktiwari9]

Quick Question about heuristics: heuristic[y][x] = (int) sqrt(pow(x - goalY, 2) + pow(y - goalY, 2)); --- Why is everything with respect to goalY and why is heuristic defined as [y][x] and not [x][y] ?

[ktiwari9]

And this is just a frontier based exploration right ? It does not take into account if there is an obstacle in the occupancy grid. It will generate the next frontier and will just go there. Correct ?

[ktiwari9]

In line 41 you generated a 2D Array I guess called mapData which as far as I understand must interpret the value of the grid if it is occupied or free but since I wasnt sure, I added this bit of code in your mapCallback()

nav_msgs::GridCells obstacles; obstacles.header.stamp = map->header.stamp; obstacles.header.frame_id = map->header.frame_id; obstacles.cell_width = map->info.resolution; obstacles.cell_height = map->info.resolution;

for (unsigned int i = 0 ; i < map->info.height; ++i) { for(unsigned int j = 0; j < map->info.width; ++j) { if(map->data[imap->info.height+j] == 100) { geometry_msgs::Point obstacle_coordinates; obstacle_coordinates.x = (j * obstacles.cell_height) + map->info.origin.position.x + (map->info.resolution/2.0); obstacle_coordinates.y = (i \ obstacles.cell_width) + map->info.origin.position.y + (map->info.resolution/2.0); obstacle_coordinates.z = 0; obstacles.cells.push_back(obstacle_coordinates); } } } so now I have obstacle co-ordinates but how I can use this information while generating the next frontier ? If can somehow check if the obstacle.cells[i].x ==100 then the grid is occupied and should not be sampled as the next frontier instead some other grid must be used as the next neighbour. Can you help me design this part so that the Occupancy grid can be checked for obstacles before sampling the next frontier ?

[nwotero]

" Quick Question about heuristics: heuristic[y][x] = (int) sqrt(pow(x - goalY, 2) + pow(y - goalY, 2)); --- Why is everything with respect to goalY and why is heuristic defined as [y][x] and not [x][y] ? "

This is a bug. It should be pow(x - goalX, 2)

" And this is just a frontier based exploration right ? It does not take into account if there is an obstacle in the occupancy grid. It will generate the next frontier and will just go there. Correct ? "

There are two things you can mean by this, and Im not sure which one you are going for. A* pushes unvisited nodes to a frontier and then "visits" them to accumulate a cost. So it is not frontier exploration as in the robot will navigate to unknown parts of the map, as is the case for the usual sense of the term. Furthermore, as I noted previously, this was created for a school project, and in that project we made a simplifying assumption that we would not need to do obstacle avoidance. If you want to add it in, simply don't add a node to the frontier if the map data says there is an obstacle there.

As for the map origin, you only need it when you are converting your start and end points from meters to grid cells. Everything else should be fine.

[ktiwari9]

So while generaring the next node to visit, how can i read the map data to say if the map data for that index ==100, add another node or something. Can you help me with this bit of code ?

[ktiwari9]

So as far as I understand, in line 41, mapData[y][x] = map_msg->data[i++]; mapData[y][x] will already store the obstacle information. So for example, if the obstacle is in the grid [2][10] then mapData[2][10]=100 and for others it would be zero or something. So, while obtaining the next node to move to, the node should never be [2][10], but how do I fit this logic in the createPath () ?

[nwotero]

Line 141, if (mapData[curY][curX] == 100) continue;

This will still visit the grid cell and mark it as such, but it will not look at the neighbors of the grid cell. That should be it.

[ktiwari9]

Wait, there is nothing on Line 141. Do you mean I add if (mapData[curY][curX] == 100) continue; before the 2 nested for loops from line 144-158 ?

[nwotero]

I was saying, add that code to line 141. There is nothing there right now, use that space for the if statement

[ktiwari9]

Hmm I did that but the robot still keeps pushing the obstacle rather than avoiding it.

[ktiwari9]

Okay so I printed out the mapdata[y][x] into a text file using the for loops from the mapcallback() and I see that the values are initially -1, -1 (UNKNOWN), 0 (FREE) or 100 (OCCUPIED). But when I write the condition we discussed in line 141;

if (mapData[cur_y][cur_x] == 100) { ROS_INFO("*****OBSTACLE DETECTED"); cout<<mapData[cur_y][cur_x]; continue; } the ros_info and cout is never printed. I have no clue why because 100 is being entered into the text file so I am sure it can read an obstacle but it never goes inside this condition.

[ktiwari9]

I think this condition is read only in the beginning. So of there was no obstacle when it was planning the next node and then if we place an obstacle it does not account for this becausw it has already planned the path through its next nodes.