Hi, I am trying to write a code, which uses nav_msgs's OccupancyGrid

edit

Hi, I am trying to write a code, which uses nav_msgs's OccupancyGrid, to visualize the grid map like a potential field.

I'm inserting the probability of where an obstacle might move next into OccupancyGrid.data[]. data[] was declared as Int8 so I am inserting numbers from 0 to 100. I'm pretty sure that rviz should show each sell white or gray or black, but in my code some parts become red.

I want to add up the inserted data each time so that when multiple objects collide, the data[] becomes 100 and shows a black area. At the end of the program, I wrote a for loop so that when the value of data[] is over 100, that value becomes exactly 100.

Here's the part of the code where I'm having trouble with...

map.data[p + q_] += (int)(100 * (1 - sqrt((x-X)*(x-X) + (y-Y)*(y-Y))/R_d));

And to finalize the map.data[] I defined a global variable _LETHAL 100 and inserted it like below

if(map.data[i]>_LETHAL) map.data[i] = _LETHAL;

I apologize for making this confusing, it seems that I'm not able to upload my program or any image because I just registered today...

But will very appreciate for any help...! Thank you.