How to extract max(x-axis) and min(x-axis) from costmap

asked 2014-06-23 23:06:58 -0500

Ken_in_JAPAN gravatar image

updated 2014-06-24 15:03:51 -0500

I want to know maximum value of x Axis in costmap. How can I get it with C++? In old code, I found a next expression.

map_upperbound = costmap_->getSizeInMetersX() - costmap_->getOriginX();
map_lowerbound = map_upperbound - costmap_->getSizeInMetersX();
bounds.setHigh(0, map_upperbound);
bounds.setLow(0, map_lowerbound);
ROS_DEBUG("Setting uper bound and lower bound of map x-coordinate to (%f, %f).", map_upperbound, map_lowerbound);

I think that this is wrong. or I'm wrong? As dornhege says, this code is correct. I understand that getSizeInMetersX() gets a width of X axis on a map. So, minimum value of x-axis is 0 and maximum value of x-axis is getSizeInMetersX().
At once, I edited a code like a following.

map_upperbound = costmap_->getSizeInMetersX() - 0.0;
map_lowerbound = -map_upperbound;

So, RRT planner began to plan.

[ INFO] [1403639024.861823324]: odom received!
Debug:   RRT: Planner range detected to be 113.437104
Info:    RRT: Starting with 1 states
Info:    RRT: Created 7 states
Info:    Solution found in 0.021853 seconds
Info:    Path simplification took 0.000126 seconds
edit retag flag offensive close merge delete

Comments

This might work if the origin has an orientation of 0. Orientations other than 0 are not supported currently anyways.

dornhege gravatar image dornhege  ( 2014-06-24 04:55:02 -0500 )edit

Thanks @dornhege: if getOriginX() is 0, map_upperbound = getSizeInMetersX() and map_lowerbound = 0. I think so. I expected map_lowerbound = - value. My thought is wrong?

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-24 11:44:59 -0500 )edit

Well, that depends on what those values are supposed to be used for. Are those min/max x coordinates in the map frame or maybe something for display or offset for an image. I would expect [0, getSizeInMetersX()] for an origin of 0 if nothing is specified.

dornhege gravatar image dornhege  ( 2014-06-24 13:47:56 -0500 )edit

@dornhege That's right! I have thought that an origin is a center of map. If the origin is an edge of map, this code is correct. Thanks!

Ken_in_JAPAN gravatar image Ken_in_JAPAN  ( 2014-06-24 14:12:17 -0500 )edit

Yes. You can think of the origin as the coordinate frame that the "map image" resides in (i.e including orientation)

dornhege gravatar image dornhege  ( 2014-06-25 06:02:07 -0500 )edit