anybotics / grid_map Goto Github PK

Hi,
I started using the grid_map_core pkg recently aiming at incrementally building a GridMap. However it seems there are no possibility to resize 'on the fly' a GridMap (resize while preserving the existing data).
The setGeometry functions internally call clearAll thus aren't suited for incremental building. I came across both addDataFrom & extendToInclude functions which both require as input argument another GridMap which is a bit of an overshoot - instantiation of a second GridMap object, internal copy of the current object etc...
Did I miss something or is it a missing functionality ??

Replace modified files with template traits (http://wiki.ros.org/roscpp/Overview/MessagesTraits).

Elevation mapping throws std::bad_alloc() as soon as the map is supposed to be moved. Adding point clouds to a map fixed in position works.

The underlying problem is probably in grid_maps, namely in EllipseIterator.cpp:32.

See https://github.com/ethz-asl/elevation_mapping/issues/42 for details.

Hi, I can't 100% reproduce this bug, but it seems to me that if one (or more?) of the vertices of the polygon used for the PolygonIterator is outside of the map, for some reason my code just hangs.

It does not happen all the time when I have a polygon that's not fully inside the map, but it happens a lot, and if i reexecute some scenario that got stuck it always gets stuck again.

Anyone else ever experienced something like this?

When I run catkin_make, I got following:

ExpressionFilter.cpp:56:88: error: no matching function for call to ‘grid_map::SlidingWindowMathExpressionFilter<grid_map::GridMap>::getParam(std::string, bool&)’
if (!FilterBase<T>::getParam(std::string("compute_empty_cells"), isComputeEmptyCells_)) {
                                                                                        ^
/home/cutcat/catkin_ws_hujun/src/grid_map/grid_map_filters/src/SlidingWindowMathExpressionFilter.cpp:56:88: note: candidates are:
In file included from /home/cutcat/catkin_ws_hujun/src/grid_map/grid_map_filters/include/grid_map_filters/SlidingWindowMathExpressionFilter.hpp:15:0,
                 from /home/cutcat/catkin_ws_hujun/src/grid_map/grid_map_filters/src/SlidingWindowMathExpressionFilter.cpp:9:
opt/ros/indigo/include/filters/filter_base.h:121:8: note: bool filters::FilterBase<T>::getParam(const string&, std::string&) [with T = grid_map::GridMap; std::string = std::basic_string<char>]
   bool getParam(const std::string& name, std::string& value)
...

My ros version is Indigo 1.11.20.
It seems like the filter_base.h file in Indigo 1.11.20 have no override function for getParam(string&, bool ).
Also I found filter_base.h has getParam(string&, bool ) in Indigo 1.11.21 on my friend's computer, so should I build this pakage in higher Indigo version like 1.11.21?

I tried converting a floating point cv::Mat to a layer and noted that the grid map ends up having extremely small entries (magniture ~E-37). The cv image however has entries in the range [0, 31.0]. Investigating I found the following line: https://github.com/ethz-asl/grid_map/blob/master/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp#L96
Setting the max by casting to (float)std::numeric_limits<Type_>::max(); works for small integer types, but not if Type is a float (or double).

I'll do my own small conversion for the moment, but two action items might be useful:

• Document that float images are not really supported by the function
• Possibly add a converter that can directly convert float cv::Mat to GridMap without scaling the entries

Line 127:
for (grid_map::GridMapIterator iterator(map); !iterator.isPassedEnd(); ++iterator) {

should be:
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {

Special case where both circles are the same is not working

I think there's a bug with GridMapCvProcessing.

If I have a map of fixed area at high resolution (say, 4 x 4 meters, 1 cm) and I want the same area at low resolution (say 4 x 4 meters, 4 cm) by applying the changeResolution method, this line causes segfault if the starting index of the original map at high resolution is out of bounds of the target map.

The new starting index should be somehow scaled as well, to find the closest cell to the original in the low resolution map.

the config folder from grid_map_demos is not present in the indigo .deb:

roslaunch grid_map_demos tutorial_demo.launch

error loading tag:
file does not exist [/opt/ros/indigo/share/grid_map_demos/config/tutorial_demo.yaml]
XML is
The traceback for the exception was written to the log file

Have you'll considered adding the "Select" feature to the RViz plugin for users to be able to click on a GridMap cell and get it local & global coordinates. It would also be great to get the list of layer values for the selected grid cell. I, personally, do not have much experience writing RViz plugins so was wondering if this a planned feature or if you have thoughts on how easy/ hard it would be to implement something like this. Thanks!

In the iterators demo, the iteration is using a string key based lookup to the layers to pull data at every index, which can be slow if you're just wanting to do full iterations over the data.

My first take was just to pull the data matrices for the layers and iterate over them directly, however after understanding a bit better what is happening with the internal storage, I gather this is a bad idea since a move will have shifted your sense of where the start is in that data matrix.

So instead, I've taken to really using the grid map iterators to pull the indices and operate on the matrix directly as follows:

grid_map::Matrix& data_layer_one = my_grid_map.get("layer_one");
grid_map::Matrix& data_layer_two = my_grid_map.get("layer_two");
for (grid_map::GridMapIterator iterator(my_grid_map); !iterator.isPastEnd(); ++iterator) {
  int i = (*iterator)(0);
  int j = (*iterator)(1);
  // do something with data_layer_one(i, j);
  // do something with data_layer_two(i, j);
}

Is this the recommended way? If it is, it would be good to have that in the iterators demo to show how you might iterate over a grid map with speed.

I find the ROS wiki package documentation quite useful when learning about new code. So far only grid_map had a page and so I have started adding empty wiki pages for the other packages as well, e.g. http://wiki.ros.org/grid_map_core).

Now, each package looks quite lonely, since there is no stack/metapackage linking them. Hence, I like to suggest adding a metapackage, which links to all grid_map packages. Doing so will enable the ROS wiki to link between the packages via a stack page. This page is also well suited to be a landing page from which one can point to the other important packages.

Also installing the metapackage will automatically pull in the other packages.

An example is the rocon metapackage and the related wiki page (notice the list of packages below the release buttons).

http://wiki.ros.org/rosdistro/Tutorials/Indexing%20Your%20ROS%20Repository%20for%20Documentation%20Generation

When running the iterator demo, the program terminates after a while with the following message:

$ roslaunch grid_map_demos iterators_demo.launch
# ...
[ INFO] [1472111866.193859810]: Running polygon iterator demo.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Time is out of dual 32-bit range

The links to the API documentations hit a 404

Hi everyone,
When scaling the height in addLayerFromImage, shouldn't the denominator be depth - 1?

As far as I can tell grid_map cannot transform according to the z value or orientation in GridMapInfo.msg

• doesn't use 3D values in the message
• setGeometry accepts only x and y
  It only uses the x,y value

We have our own heightmapping tool which creates a grid relative to a frame.

... note that the large grid in the 'director' UI is not aligned with the heightmap grid. also note that the gridmap is aligned with the robot's orientation, by design.

I suppose the solution would be to add our frame to TF and set the x,y values to zero. But I wanted to ask why a full 3d pose is included in the ROS message but not used?

I am using the 1.4.1 version (apt-get install of ubuntu packages).

I run the iterator_demo, the iterator goes in column-major order, however the gif image of this github repo shows that the iterator goes in row-major order.

I am confused and want to ask that which one is correct ?

I have a question: from the RViz I can see that the gridmap is visualized with some sort of normal to orient the cell/mesh. This means that for each cell a normal is computed. Is it available from the gridmap object or it is computed by the RViz plugin from the neighbors?

Is there a method to get the normals automatically?

Regarding to GridMap::add(const std::string& layer, const double value), we can add layers with initial value. However, GridMap::resize(const Eigen::Array2i& size) only resize all layers without filling any data.

If I want to extend all layers with their initial value, is there any way to do that?

Thanks.

See branch fix/polygon_iterator.

Hi there,
first of all, thanks for this easy-to-use and well documented library!

There however seem to be alignment issues with the Eigen datatypes when compiling the library and software including its headers, which can lead to segmentation faults with compiling with vectorization (SSE) enabled. This probably is the case as you use Eigen datatypes like Eigen::Vector2d as a member of a structs/classes or in stl containers, like here:
https://github.com/ethz-asl/grid_map/blob/master/grid_map_core/include/grid_map_core/Polygon.hpp#L206

See https://eigen.tuxfamily.org/dox-devel/group__DenseMatrixManipulation__Alignement.html for more information on the particular issues.

As far as I can see:

• best would be to adapt the code to use the Eigen allocators with correct alignment for vectorization
• second best to use Eigen::DontAlign datatypes to prevent alignment issues
• third best to at least warn the user to turn off Eigen vectorization as otherwise it can lead to hard-to-debug segmentation faults
  (see Eigen docs linked above for details on each of the three options)

ros-perception/vision_opencv@8b5bbcb

When converting a GridMap to an OpenCV image with the method grid_map:GridMapCvConverter::toImage, the cv::Mat is initialized with zeros (black pixels) (here) then, only finite values are copied from the GridMap to the cv::Mat (here).

In other words the NaN values are automatically translated into black pixels.

For my use case (processing a map during a SLAM), I have a lot of NaN values and I need that information in OpenCV.
So maybe, setting the value of the pixels to the mid value between lowerValue and upperValue is more interesting than forcing to 0 by default.

While compiling grid_map_rviz_plugin master branch on Ubuntu 14 with a bunch of compilation flag set I got the following compilation error :

grid_map_rviz_plugin/src/GridMapVisual.cpp:73:18: error: suggest parentheses around '&&' within '||' [-Werror=parentheses]

Standard ROS navigation uses CostMap2D as a cost map representation and when implementing a nav_core::BaseGlobalPlanner plugin that uses grid_map as a back-end, it would be desirable to have a converter that allows initalizing the internal grid_map from a CostMap2D.
Are there any plans for implementing this (or even some implementation out there that I missed)?

I've been using grid_map for a while. I found LineIterator occasionally crash if I initialize it with two 'Position's, and access the grid map with

LineIterator iter(*grid_map_, pos1, pos2);
for (; !iter.isPastEnd(); ++iter)
{
  grid_map_->at("map", *iter) = foo;
}

This is most likely when both 'pos1' and 'pos2' are out of valid range.

I looked into the source code of 'LineIterator' and found that the constructor will NOT initialize anything if 'getIndexLimitedToMapRange()' returns false. In this case, when you invoke 'isPastEnd()', it'll probably return false because it actually returns 'iCell_ > nCells_'. NOTE the 'iCell_' and 'nCells_' were not initialized properly.

Not sure if it is actually an issue but when overlaying an occupancy grid and a grid map, the two plots mismatch - see attached screenshot. Visually it looks like grid map is one cell short.
I noticed that in the GridMapVisual class cells are iterated up to rows\cols -1.

Is this the intended behavior ? What is the rational behind this ?

Simple 50x50 square fully marked as occupied. The occupancy grid overflow on each side (black border). Moreover, the grid map displayed is only 49x49.

I've been running into a problem with maps not matching up that I suspect could be related to mismatching sizes. So what I have is the following: A static_map_ containing standard occupancy data that is fixed wrt to world. I also have a "moving" elevation map local_elevation_map_. I want to fuse the local elevation data to static_map_, adding obstacles based on elevation data.

To this end, I do the following:

  const grid_map::Length& local_length = local_elevation_map_.getLength();
  const grid_map::Position& local_position = local_elevation_map_.getPosition();

  static_map_.extendToInclude(local_elevation_map_);

  bool submap_create_success;
  grid_map::GridMap static_cut = this->static_map_.getSubmap(local_position, local_length, submap_create_success);

  grid_map::Matrix& elev_data   = local_elevation_map_["elevation"];
  grid_map::Matrix& static_cut_data = static_cut["occupancy"];

  for (grid_map::GridMapIterator iterator(local_elevation_map_); !iterator.isPastEnd(); ++iterator) {
      const grid_map::Index index(*iterator);

      // According to some rule, add obstacle information to the map
      if ( std::abs( robot_elevation - elev_data(index(0), index(1)) ) > 0.38 ){
         static_cut_data(index(0), index(1)) = 100.0;
      }
  }
  static_map_.addDataFrom(static_cut, true, true, true);

So essentially, my goal was to create a subMap with exactly the same size and origin as the local elevation map and then iterate over/compare them using the same index. This works in the sense that I get data out, but it looks transformed incorrectly (i.e. obstacles in the elevation map show up in a different place in the static_map_ when visualizing them). With some debugging I found that the length of my local_elevation_map_ is 6, but that of the generated submap static_cut is 6.05. Now I can see how this can happen due to discretization, but it's highly inconvenient :)

I thus have a request and a question:

It would probably be good to indicate in documentation that the actual size of the returned submap might be different from what was requested. Alternatively, if possible it could be made to actually always have the requested size, but I think this might be tricky due to the discrete nature.

What would be the "proper" way of doing what I want (add data from a local map to a world fixed one based on some rule)?

In toCvImage the order of the index access (1,0) into the cv::Mat (code) is exactly in opposite order to access in addLayerFromImage (0,1) (code).

This means that when creating a cv::Mat from a grid map layer, performing some operations on it and then adding it back as another layer using both conversion methods mentioned above, the new layer will be rotated by 90 degrees. Looks like a bug to me, or is there some kind of rationale for this?

Useful if you want to fling around grid map messages in nodelets without copying data.

I can probably contribute this when I start optimising.

Moving the discussion here from email so there will be a visible, relevant history.

Proposal : find a way to have GridMap and CostMap objects use the same message object.

As grid_map core does not need ROS I would add a custom Cmake file without catkin references for those who don't need ROS and don't have it installed.

I started playing around with this library and as a first test I implemented a "round trip conversion": nav_msgs::OccupancyGrid-> grid_map_msgs::GridMap -> grid_map::GridMap -> nav_msgs::OccupancyGrid.

The first conversion is done by my own implementation (couldn't find an implementation in this library), the second is using grid_map::GridMapRosConverter::fromMessage and the last grid_map::GridMapRosConverter::toOccupancyGrid.

The result looks fine, except 1) the costmap is rotated and 2) there is an offset in the origin's position.

Starting with the first issue, the original origin's quaternion is:

    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

The quaternion of the intermediate grid map is the same:

    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

But the final quaternion is different:

    orientation: 
      x: 0.0
      y: 0.0
      z: 1.0
      w: 0.0

Looking at the code, I found this part:

occupancyGrid.info.origin.orientation.x = 0.0;
occupancyGrid.info.origin.orientation.y = 0.0;
occupancyGrid.info.origin.orientation.z = 1.0;  // yes, this is correct.
occupancyGrid.info.origin.orientation.w = 0.0;

I don't understand why this change is necessary. Could someone please explain?

Looking at the second issue about the position offset the situation is similar. The original origin position in my example is:

    position: 
      x: -50.0
      y: -50.0
      z: 0.0

The position of the grid map's origin is the same. But during the conversion into a occupancy grid the origin is shifted/adjusted to:

    position: 
      x: 7.45058059692e-07
      y: 7.45058059692e-07
      z: 0.0

The reference frame for the origin of the occupancy grid in ROS (e.g. when published by move_base) seems to be one of the corners. When looking into the [conversion code (part 1, part 2) it seems to be assumed that the reference frame is the centre. What's the reason behind this?

Only other grid_map's can be seen if a grid_map is set to transparent (alpha<1). Robot model and other markers are not visible through transparent grid_map's.

Currently, there's an error when Eigen is not loaded at first from grid_map_core because of the Eigen Plugins.

Getting ready to release the cost map counterparts to this library and just realised the debs aren't up to date with the api I'm making use of here in the master branch.

@pfankhauser any chance you can bump a new release on these?

The recent moving of the include file: ethz-asl@787747e in #96 is breaking downstream packages expecting the header.

A backwards compatibility header with a deprecation warning would be appreciated.

http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__cost_map_ros__ubuntu_trusty_amd64__binary/8/console

00:07:47.321 /tmp/binarydeb/ros-indigo-cost-map-ros-0.3.1/src/lib/converter.cpp:23:47: fatal error: grid_map_ros/Costmap2DConverter.hpp: No such file or directory
00:07:47.321  #include <grid_map_ros/Costmap2DConverter.hpp>
00:07:47.321    

Without actually changing the lengths, nor the position of the grid_map window.

We've been playing around with something in cost_map/pull-requests/17.

Thoughts?

hello guys, I want to build my own grid map on ROS Indigo with a sensor (e.g. kinect or Realsense camera) but without using a robot and navigation, just building the local grid map that should represent the obstacles detected by the sensor. What should I do with this grid map library? Do I need to work with the node "elevation mapping" in addition? Thanks!

Like #51, another idea to bring closer together the GridMap and CostMap implementations.

I was thinking of a CRTP class, much like Eigen does for it's matrices.

This would have several advantages:

• Move alot of geometry/structural related code that is common to both into the base class.
• Define templatised virtuals in the base class that XxxMap implementations require.
• Child implementations can exist without being template classes themselves

The third advantage is good. I'm not really thinking about providing support for a variety of child implementations (that starts to go down the road of YAGNI), however it does make it easier for my colleagues to dive into building/maintaining the CostMap class if it itself isn't a massively templatised (or specialisation of a template) itself.

I'd like to have a crack at this one before #51 as it covers a significantly larger amound of overlap than the messages do.