ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
1

Occupancy grid creation

asked 2022-05-16 05:21:21 -0600

dottant gravatar image

updated 2022-05-17 04:56:03 -0600

Hi guys, I'm trying to implement my own controller in ROS1 Melodic for a mobile robot and now I'm facing the obstacle mapping problem.
I'm starting from simple things such as the understanding of the nav_msgs::OccupancyGrid message and how to fill it, so I'm trying, for ex., to fill the grid with the -1 value right where there's the robot footprint during its movements. To do so, I developed a really simple node which gets odometry information to know the robot pose and publishes an updated OccupancyGrid. The part that I still have to understand better is the method to fill the OccupancyGrid message data.I tried to find something useful all over the web, but there's no explanation about this tasks
Here's the function that I would like to use

void OccupancyGridTest::SetRobotFreeCells()
{
  // the pose of the robot coming from Odom and based on the grid resolution
  int robotPoseGridX = static_cast<int>(robotPose_.x / gridResolution_);
  int robotPoseGridY = static_cast<int>(robotPose_.y / gridResolution_);

 // fills the full grid with 1 
 fill(myGrid_.data.begin(), myGrid_.data.end(), 1);

 // highlighting the robot footprint on grid space with -1
 for(int x = -((robotFootprint_ / gridResolution_)/2); x < ((robotFootprint_ / gridResolution_)/2); ++x)
  {
    for(int y = -((robotFootprint_ / gridResolution_)/2); y < ((robotFootprint_ / gridResolution_)/2); ++y)
    {
      int i = //MISSED CODE //
      myGrid_.data[i] = -1;
    }
  }
  myGrid_.header.seq++;  
  gridPub_.publish(myGrid_);
}


and here is how I initiate the grid

void OccupancyGridTest::InitGrid()
{
  myGrid_.header.seq = 1;
  myGrid_.header.frame_id = "/odom";
  myGrid_.info.resolution = gridResolution_;
  myGrid_.info.width = gridWidth_;
  myGrid_.info.height = gridHeigth_;
  myGrid_.info.origin.position.x = 0;
  myGrid_.info.origin.position.y = 0;
  myGrid_.info.origin.position.z = 0;
  myGrid_.info.origin.orientation.w = 1;
  myGrid_.info.origin.orientation.x = 0;
  myGrid_.info.origin.orientation.y = 0;
  myGrid_.info.origin.orientation.z = 0;
  myGrid_.data.resize(gridWidth_*gridHeigth_,1);
}

I would like to know how to create the index i and if there's some errors in the cycles and/or there are other information that I should have
Thank you very much in advance

UPDATE
I added these lines within the nested for loops

int i = (x + robotPoseGridX) + (gridWidth_ * (y + robotPoseGridY));
if(i>=0) myGrid_.data[i] = -1;

If anyone wanna write something more is more than welcome, thank you

UPDATE II
I updated the SetRobotFreeCells member function to the following

void OccupancyGridTest::SetRobotFreeCells()
{
  // the pose of the robot based on the grid resolution
  int robotPoseGridX = static_cast<int>(robotPose_.x / gridResolution_);
    int robotPoseGridY = static_cast<int>(robotPose_.y / gridResolution_);
  int index;

  fill(myGrid_.data.begin(), myGrid_.data.end(), 0);

  // highlightin the robot grid space
  if( (robotPoseGridX>=((robotFootprint_ / gridResolution_) / 2)) &&
        (robotPoseGridY>=((robotFootprint_ / gridResolution_) / 2)) )
  {
    for(int x = - ((robotFootprint_ / gridResolution_) / 2); x < ((robotFootprint_ / gridResolution_) / 2); ++x)
    {
      for(int y = - ((robotFootprint_ / gridResolution_) / 2); y < ((robotFootprint_ / gridResolution_) / 2); ++y)
      {        
        index = (x + robotPoseGridX) + (gridWidth_ * (y + robotPoseGridY));
        if(index>=0) myGrid_.data[index] = -1;
      }
    }
  }
  else if( (robotPoseGridX<((robotFootprint_ / gridResolution_) / 2)) &&
        (robotPoseGridY<((robotFootprint_ / gridResolution_) / 2)) )
  {
    for(int x = -robotPoseGridX; x < ((robotFootprint_ / gridResolution_) / 2); ++x)
    {
      for ...
(more)
edit retag flag offensive close merge delete

Comments

You could convert msg.data to a 2D array first. The other solution i see is that i = x*y

Alex-SSoM gravatar image Alex-SSoM  ( 2022-05-16 06:39:56 -0600 )edit

Thanks for the answer. The other solution can't be right cause the cell index must be related to the robot position

dottant gravatar image dottant  ( 2022-05-16 07:25:05 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-05-16 09:34:30 -0600

ljaniec gravatar image

updated 2022-05-16 09:43:36 -0600

By the OccupancyGrid.msg definition you know it is a row-major order 2D array, with a MapMetaData:

float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad].  This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin

You have already computed robotPoseGridX and robotPoseGridY (I assume it's correct), you know both dimensions of the map [cells], you can determine, using the integer divisions and modulo operations, the cells to change value around around (robotPoseGridX, robotPoseGridY) to -1.

image description

For the OX - use % width, for the OY - / width to translate between (robotPoseGridX, robotPoseGridY) <--> row-major order map (with relevant offset for the space the robot sweeps).

edit flag offensive delete link more

Comments

thank you so much. I updated the question before your answer. It would be great if you have a look thanks

dottant gravatar image dottant  ( 2022-05-16 09:52:02 -0600 )edit

Can you add a screenshot of your solution?

ljaniec gravatar image ljaniec  ( 2022-05-16 17:26:25 -0600 )edit
1

Updated with new code lines and pics

dottant gravatar image dottant  ( 2022-05-17 04:56:20 -0600 )edit

Really nice! It will be helpful for future readers :)

ljaniec gravatar image ljaniec  ( 2022-05-17 07:40:26 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2022-05-16 05:21:21 -0600

Seen: 126 times

Last updated: May 17 '22