bug fix about issue #5

[Tasaki-Lab]

Bug fix

Fixed bug

I fix the bug about issue 5.
#5

Fix applied

Grids of the cost map generated by costmap_generator are not placed correctlly.
For example, maybe, we want the point's position (0.3, 0.5) in the costmap coordinate to be placed cost map index (0, 0).

In the case of previous code:
the position (0.3, 0.5) -> cost map index (1, 1)

New feature implementation

the position (0.3, 0.5) -> cost map index (0, 0)

Implemented feature

int mapped_x_ind = std::ceil(mapped_x);
-->
int mapped_x_ind = (int)mapped_x;

[JWhitleyWork]

@Tasaki-Lab It appears that this change intermittently causes a segmentation fault. Can you please try keeping the std::ceil call and wrapping it in static_cast<int>?

[Tasaki-Lab]

I think isValidInd has a bug.

In GridMap::setGeometry, the size of cost map is defined as:
00059 size(0) = static_cast(round(length(0) / resolution)); // There is no round() function in Eigen.
00060 size(1) = static_cast(round(length(1) / resolution));

So, I modify the code. (Tested by Moriyama data)

[JWhitleyWork]

@Tasaki-Lab Where are you seeing the code that you're talking about? Can you provide a link? I don't see any round function used in costmap_generator at all.

[Tasaki-Lab]

@Tasaki-Lab Where are you seeing the code that you're talking about? Can you provide a link? I don't see any round function used in costmap_generator at all.

Autoware uses grid_map_ros provided by ROS package, right?

costmap_generator.cpp (line 124) uses setGeometry provided by the ROS package.
In the ROS package code (line 00059-00060), GridMap::setGeometry uses the round function.
http://docs.ros.org/jade/api/grid_map_core/html/GridMap_8cpp_source.html#l00051