Request with zero size return full costmap #19
Conversation
|
@stonier |
| float subwindow_bottom_left_x = position.x() - geometry.x() / 2.0; | ||
| float subwindow_bottom_left_y = position.y() - geometry.y() / 2.0; | ||
| double subwindow_bottom_left_x = (position.x() - geometry.x()) * 0.5; | ||
| double subwindow_bottom_left_y = (position.y() - geometry.y()) * 0.5; |
There was a problem hiding this comment.
I also use floats where it's not important to be a double. While you don't see any difference on the PC, the difference is significant if using the code on an arm core.
I used to be the other way around...
| if((geometry.x() == original_size_x && geometry.y() == original_size_y) | ||
| || geometry.x() == 0.0 || geometry.y() == 0.0) { | ||
| is_subwindow = true; | ||
| costmap_subwindow = costmap_2d::Costmap2D(*(ros_costmap.getCostmap())); //be aware of some black magic in here |
There was a problem hiding this comment.
Is it actually necessary to do a copy in this case?
There was a problem hiding this comment.
I guess the tradeoff here is about whether it's large (e.g. global costmap) and the copy will be a large cost, or the or if the iteration over it locks the costmap for a significant time
There was a problem hiding this comment.
Actually the copy constructor does not do a copy (that's the black magic, yay navi stack) and I forgot to lock it farther downwards.
There was a problem hiding this comment.
Right, we are doing a copy anyway, so whether we do it early and lock around that, or do it late, and lock around the whole thing...much the same thing. So, let's just lock late.
Also as you pointed out, a block copy (memcopy style) might be much faster than iterating.
|
Is it intended that the costmap always has the full size but only copies the requested part? |
| double subwindow_bottom_left_y = (position.y() - geometry.y()) * 0.5; | ||
|
|
||
| double original_size_x = ros_costmap.getCostmap()->getSizeInMetersX(); | ||
| double original_size_y = ros_costmap.getCostmap()->getSizeInMetersX(); |
|
|
||
| // std::cout << "From ROS Costmap2D" << std::endl; | ||
| // std::cout << " Robot Pose : " << position.x() << "," << position.y() << std::endl; | ||
| // std::cout << " Bottom Left Corner: " << subwindow_bottom_left_x << "," << subwindow_bottom_left_y << std::endl; | ||
| // std::cout << " Resolution : " << resolution << std::endl; | ||
| // std::cout << " Size : " << geometry.x() << "x" << geometry.y() << std::endl; | ||
| // std::cout << " Original Size : " << original_size_x << "x" << original_size_y << std::endl; | ||
|
|
||
| bool is_subwindow = false; |
There was a problem hiding this comment.
Oh, this is a confusing name I wrote here...it's not about whether its a subwindow or not, but whether it lands inside the original boundaries or not.
There was a problem hiding this comment.
Maybe should be is_valid_window or something.
Cost map doesn't have the full size..it is determined by the passed in geometry. The copied part size is also determined by the passed in geometry. Should be the same. |
AlexReimann
force-pushed
the
fix_max_resolution_request
branch
5 times, most recently
from
7a522f9
to
19c3dfe
Compare
|
Not tested yet! |
AlexReimann
force-pushed
the
fix_max_resolution_request
branch
from
19c3dfe
to
c85ae31
Compare
|
Ready for checking. Already rebased / squashed |
|
Added some fixed for fromROSCostMap2D and toOccupancyGrid which basically just copies code from the occupancy grid converter from the original grid_map I'll add my test program tomorrow as it is a bit messy right now. |
AlexReimann
force-pushed
the
fix_max_resolution_request
branch
from
9fc652c
to
18c7b00
Compare
AlexReimann
force-pushed
the
fix_max_resolution_request
branch
from
18c7b00
to
665afa6
Compare
|
Fixed up the alignment test, but still a bit ugly ._. |
|
Ran out of time to check on this, but will push it through so you've something to work with. Keep the PR's coming as you need them - I'll keep pushing them and really go over it once I get a good day or two free :) |
Partly fixes issue #15.
This change is