Ask Your Question
0

Seeking clarity the Costmap2D::worldToMapEnforceBounds() function

asked 2021-11-17 13:22:31 -0600

updated 2021-11-17 13:22:41 -0600

The docs define the costmap_2d::Costmap2D::worldToMapEnforceBounds(double wx, double wy, int &mx, int &my) function as follows -

Convert from world coordinates to map coordinates, constraining results to legal bounds.

How does one set the legal bound?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-11-17 13:42:52 -0600

Hi @skpro19

if you look at the source code of this function it's basically checking the bounds against origin_x_, origin_y and resolution_ that are values that are created with an instance of Costmap2D, they also get updated in the event map gets resized. Not a definition of a legal bound, but at least you can see how it's interpreted

00228 void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const
00229 {
00230   // Here we avoid doing any math to wx,wy before comparing them to
00231   // the bounds, so their values can go out to the max and min values
00232   // of double floating point.
00233   if (wx < origin_x_)
00234   {
00235     mx = 0;
00236   }
00237   else if (wx >= resolution_ * size_x_ + origin_x_)
00238   {
00239     mx = size_x_ - 1;
00240   }
00241   else
00242   {
00243     mx = (int)((wx - origin_x_) / resolution_);
00244   }
00245 
00246   if (wy < origin_y_)
00247   {
00248     my = 0;
00249   }
00250   else if (wy >= resolution_ * size_y_ + origin_y_)
00251   {
00252     my = size_y_ - 1;
00253   }
00254   else
00255   {
00256     my = (int)((wy - origin_y_) / resolution_);
00257   }
00258 }
edit flag offensive delete link more

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: 2021-11-17 13:22:31 -0600

Seen: 10 times

Last updated: Nov 17 '21