anybotics/elevation_mapping

Robot-centric elevation mapping for rough terrain navigation

elevation_mapping's Issues

elevation_mapping process has died

This is very useful however I was getting stuck on the elevation_mapping node, when I run:
rosrun elevation_mapping elevation_mapping
the process simple died immediately... saying: core dumped...
Even I define each topic name using "simple_demo.launch", it still does not run.
Is it because I was missing part of the topic? I did provide pointcloud2 pose and tf though.

Any suggestion on what should I try next?
Thanks!

Udpated master to latest Kindr version

As of now, the master branch is not compatible with the Kindr master. We are working on a bigger update, so in the meanwhile please use the feature/obstacle_detection branch for compatibility.

How to convert pcl::pointcloud2 to grid map?

I acquired the pointcloud data from the Lidar, however, I am stucked when I wanna convert the pointcloud data to grid map?

Describe basic usage in README

how the kinect1 run this demo

when i run:
roslaunch elevation_mapping_demos simple_demo.launch
appear:
[ERROR] [1487917327.529496471]: Could not get pose information from robot for time 1487917327.529461. Buffer empty?
[ERROR] [1487917327.529546411]: Updating process noise failed.

and nothing in rviz
how can i run?

Visibility Cleanup Finds Zero Points

I am using the following parameters:
enable_visibility_cleanup: true
visibility_cleanup_rate: 1.0
I believe the rest of my configuration is correct, but when it goes to do visibility cleanup, I get the following:

Visibility cleanup has been performed in 0.045898 s (0 points).

This is even when I have dynamic objects moving across the map. They leave a trail and are never removed.

Is there a configuration parameter I am missing?

The elevation_mapping process has died when build type is Release

I'm running elevation_mapping on an Odroid XU4 with 16.04 and when I build with the default CMAKE_BUILD_TYPE I am able to visualize the fused map, but it was running quite slowly so I built with Release and when I try to visualize the fused map elevation_mapping dies.

Any suggestions/ideas for why this might be happening? Thanks!

NAN value for elevation map

Hello,

I've been working on a project of navigation on rough terrain. I am using Freenect + RTABMAP with kinect to map the environment and do localization. I have published all the required topics to the elevation_mapping node. However when I use my launch file as below, I always get NAN values from the elevation mapping node. Why is this happening?

There are by the way no errors thrown by any nodes. The TF-tree is published too.

<launch>
<node name="Elevation_Map" type="elevation_mapping" pkg="elevation_mapping" respawn="false" output="screen">
    <param name="point_cloud_topic" value="/rtabmap/mapData"/>
    <param name="robot_pose_topic" value="/rtabmap/odomwithcovariance"/>
    <param name="base_frame_id" value="/Robot_Base"/>
    <param name="track_point_frame_id" value="/Robot_Base"/>
    <param name="resolution" value="0.05"/>
</node>

</launch>

Kind Regards,
Jesse

sensor parameters

Hi,

I want to test the elevation mapping on a velodyne lidar.
Unfortunately, I couldn't understand what the parameters mean.

sensor_processor/min_radius: 0.018
sensor_processor/beam_angle: 0.0006
sensor_processor/beam_constant: 0.0015

Thank you

Cleanup Tasks

• Replace Eigen affine transform with Kindr transform
• Remove dependency on boost (?)
• Speed up with grid map layer accessors.
• Reduce console output.
• Cleanup parameter file structure.
• Seprate to ROS-dependent & independent libraries.
• Cleaning of point clouds in sensor processors.

How to save Elevation Map?

Is there a standard way to save and read an elevation map to a file and from a file?

Add submap service that fuses submap only

How to get PoseWithCovarianceStamped from a tf???

I get the odometry in a tf so I have transform it to PoseWithCovarianceStamped, but the problem is that i do not have any covariance. Is there any problem giving Pose withou covariance?

Segmentation fault on ARM due to oversized Eigen matrix

System

• Hardkernel Odroid XU4
• Ubuntu 14.04 LTS

Problem

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.

Where in the code the problem occurs

Using gdb leads to ElevationMap.cpp:286. At the core, the problem is that grid_map::EllipseIterator.cpp:32 is not initialized. This function is called by ElevationMap.cpp:285. Eigen allocates an immense amount of memory and finally throws std::bad_alloc() from Eigen/src/Core/util/Memory.h:362-367.

// Eigen/src/Core/util/Memory.h, lines 362 to 367
template<typename T>
EIGEN_ALWAYS_INLINE void check_size_for_overflow(size_t size)
{
  if(size > size_t(-1) / sizeof(T))
    throw_std_bad_alloc();
}

Hacky fix

A quick, hacky fix is to skip the map fusion process of a specific cell in case too much memory is allocated. This code is added between ElevationMap.cpp:285 and ElevationMap.cpp:286:

// If maxNumberOfCellsToFuse is too high, Eigen will throw a std::bad_alloc() error.
// Therefore, skip fusing in that case.
if (maxNumberOfCellsToFuse > (size_t(-1) / sizeof(float))) {
  // Skip fusing.
  fusedMap_.at("elevation", *areaIterator) = rawMapCopy.at("elevation", *areaIterator);
  fusedMap_.at("lower_bound", *areaIterator) = rawMapCopy.at("elevation", *areaIterator) - 2.0 * sqrt(rawMapCopy.at("variance", *areaIterator));
  fusedMap_.at("upper_bound", *areaIterator) = rawMapCopy.at("elevation", *areaIterator) + 2.0 * sqrt(rawMapCopy.at("variance", *areaIterator));
  fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);
  continue;
}

This is only a temporary fix. The issue should be fixed more permanently in grid_map.

Modifying Elevation_mapping to work with wheeled robots

I was wondering what files/methods I need to look at to modify Elevation Mapping to work with a wheeled robot? In your video, it shows red for any height change such as walking from floor to a plank. As well it shows blue for any flat surface. Is there a way to modify this so that the robot can be blue for minor slopes that it can climb?

Converting from grid_map_msgs:GridMap

The elevation_map type is of grid_map_msgs::GridMap. Is there a way to convert this to grid_map::GridMap? Can you recommend a good way to work with this data for visualizing as a 2d map?

Can the project support velodyne HDL-64?

I have recorded rosbag including pointcloud2 from velodyne-64, /tf for frame_id between the vehicle and the world and PoseWithCovarianceStampd(topic /ground_truth_pose) for world_frame_id. The How can I use the bag to have an elevation_mapping.
Now the elevation_mapping is filled with nan. Now I use velodyne_HDL-32E.yaml. The following is the result:

[ INFO] [1530867972.623350455]: Grid map visualization node started.
[ INFO] [1530867972.625583814]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_cloud'.
[ INFO] [1530867972.625621464]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'upper_bound_cloud'.
[ INFO] [1530867972.625649608]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'lower_bound_cloud'.
[ INFO] [1530867972.625865611]: grid_map_visualizations: Configured visualization of type 'map_region' with name 'map_region'.
[ INFO] [1530867972.628691825]: Grid map visualization initialized.
[ INFO] [1530867972.633750244]: Grid map visualization node started.
[ INFO] [1530867972.636420228]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_cloud'.
[ INFO] [1530867972.636457491]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'sigma_cloud'.
[ INFO] [1530867972.636695711]: grid_map_visualizations: Configured visualization of type 'map_region' with name 'map_region'.
[ INFO] [1530867972.639628355]: Grid map visualization initialized.
[ INFO] [1530867972.671558473]: Elevation mapping node started.
[ INFO] [1530867972.697715222]: Elevation map grid resized to 250 rows and 250 columns.
[ INFO] [1530867972.726483010]: Elevation mapping node initializing ... 
[ INFO] [1530867973.726852942]: Done.
[ INFO] [1530867973.738195697]: ElevationMap received a point cloud (110592 points) for elevation mapping.
[ INFO] [1530867983.521865152]: Raw map has been updated with a new point cloud in 5.122441 s.
[ INFO] [1530867983.535307001]: ElevationMap received a point cloud (110592 points) for elevation mapping.
[ INFO] [1530867984.018407364]: Elevation map has been fused in 9.291024 s.
[ INFO] [1530867993.337481506]: Raw map has been updated with a new point cloud in 5.124817 s.
[ INFO] [1530867993.340234993]: Visibility cleanup has been performed in 18.612875 s (0 points).
[ WARN] [1530867993.340339130]: Visibility cleanup duration is too high (current rate is 0.053726).

[ INFO] [1530867993.351025602]: ElevationMap received a point cloud (110592 points) for elevation mapping.
[ INFO] [1530867993.816907762]: Elevation map has been fused in 9.796082 s.
[ INFO] [1530868003.225202005]: Raw map has been updated with a new point cloud in 5.208442 s.
[ INFO] [1530868003.227891645]: Visibility cleanup has been performed in 9.887359 s (0 points).
[ WARN] [1530868003.227953766]: Visibility cleanup duration is too high (current rate is 0.101139).

[ INFO] [1530868003.236804103]: ElevationMap received a point cloud (110592 points) for elevation mapping.
[ INFO] [1530868003.708482737]: Elevation map has been fused in 9.890168 s.
[ INFO] [1530868013.035706870]: Raw map has been updated with a new point cloud in 5.109959 s.
[ INFO] [1530868013.038773528]: Visibility cleanup has been performed in 9.810674 s (0 points).
[ WARN] [1530868013.038901000]: Visibility cleanup duration is too high (current rate is 0.101930).

[ INFO] [1530868013.047897045]: ElevationMap received a point cloud (110592 points) for elevation mapping.
[ INFO] [1530868013.548942611]: Elevation map has been fused in 9.839041 s.
[ INFO] [1530868022.799122446]: Raw map has been updated with a new point cloud in 5.141613 s.
[ INFO] [1530868022.801794363]: Visibility cleanup has been performed in 9.762718 s (0 points).

Wrong time conversion

In this line incorrect conversion from PCL timestamp to ROS time is done (uint64->double->ROS time), leading to Time is out of dual 32-bit range error with debug logger level.

elevation_mapping/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp

Line 130 in 060ae2f

ros::Time(pointCloudTransformed->header.stamp).toSec());

Instead, the next line can be used
timeStamp.toSec());
or fromNSec need to be used.

elevation_mapping received signal SIGSEGV, Segmentation fault.

Hey,

Can someone help me solve this crashing issue? It happens quite frequently while mapping. I've got a backtrace below. I'm running the master version of the code.

(gdb) bt
#0  0x00007ffff67dfe68 in grid_map::GridMap::at(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Array<int, 2, 1, 0, 2, 1> const&) const ()
   from /opt/ros/kinetic/lib/libgrid_map_core.so
#1  0x00007ffff67dffce in grid_map::GridMap::isValid(Eigen::Array<int, 2, 1, 0, 2, 1> const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&) const () from /opt/ros/kinetic/lib/libgrid_map_core.so
#2  0x00007ffff7ba007f in elevation_mapping::ElevationMap::fuse (this=this@entry=0x7fffffffc270, topLeftIndex=..., size=...) at /home/rayman/catkin_ws/src/elevation_mapping/src/ElevationMap.cpp:307
#3  0x00007ffff7ba1d32 in elevation_mapping::ElevationMap::fuseAll (this=this@entry=0x7fffffffc270) at /home/rayman/catkin_ws/src/elevation_mapping/src/ElevationMap.cpp:193
#4  0x00007ffff7b780f2 in elevation_mapping::ElevationMapping::publishFusedMapCallback (this=0x7fffffffb810) at /home/rayman/catkin_ws/src/elevation_mapping/src/ElevationMapping.cpp:335
#5  0x00007ffff786014b in ros::TimerManager<ros::Time, ros::Duration, ros::TimerEvent>::TimerQueueCallback::call() () from /opt/ros/kinetic/lib/libroscpp.so
#6  0x00007ffff78866f8 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/kinetic/lib/libroscpp.so
#7  0x00007ffff78880fb in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/kinetic/lib/libroscpp.so
#8  0x00007ffff7b7613a in elevation_mapping::ElevationMapping::runFusionServiceThread (this=0x7fffffffb810) at /home/rayman/catkin_ws/src/elevation_mapping/src/ElevationMapping.cpp:224
#9  0x00007ffff3abb5d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#10 0x00007ffff38946ba in start_thread (arg=0x7fffddffb700) at pthread_create.c:333
#11 0x00007ffff6d3d41d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Parameter to reduce/regulate verbosity level

Is there a parameter to reduce the verbosity level of the elevation mapping node (e.g. removing all the ROS_INFO "elevation map has been fused ...")?

Thanks,
Marco.

Time stamp is wrong

I am using a bag file to publish the pose and point cloud topics, but this is the error I get from the elevation mapping topic.

[ERROR] [1543392165.165991458]: Requested update with time stamp 1472902300.261383, but time of last update was 1543392164.813431.
[ERROR] [1543392165.166066639]: Updating process noise failed.

I have tried to set the parameter of use_sim_time to True, but if I do that, the elevation mapping node does not initialize.So can you help me?

nan Value for elevation map

The elevation map node is subscribed to /depth/points from asus xtion, tf and pose with covariance stamped, but rostopic echo elevation_mapping/elevation_map returns NAN.

Updating map in real-time

I'm having trouble getting the map to update as soon as it has scanned an area. I've modified the various updating and publishing parameters to 1 hz but when viewed in Rviz it appears to update at random times, almost 10-20 seconds after scanning an area. In your video with the 4 legged robot over the rubble scene, the map appears to be saving and updating in realtime. What can you suggest is a good way to do that?

ERROR: cannot launch node of type [elevation_mapping/elevation_mapping]: can't locate node [elevation_mapping] in package [elevation_mapping]

I'm using ROS kinetic and the compilation worked. What could be causing this?

LookupTransform argument target_frame in tf2 frame_ids cannot be empty

Hi,
I am publishing a point cloud from velodyne VLP-16 and run elevation_mapping_demo using the ground_truth yaml as follows:

point_cloud_topic: "/velodyne_points"
map_frame_id: "map"
sensor_frame_id: "velodyne"
robot_base_frame_id: "map"
robot_pose_with_covariance_topic: "/ground_truth_pose"
robot_pose_cache_size: 500
track_point_frame_id: "map"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

As tf and pose I use a SLAM algorithm to this purpose. Although, the tf is tf2 type.
I got the following error:

[ERROR] [1541700130.680266861, 1455209754.533483445]: Invalid argument passed to lookupTransform argument target_frame in tf2 frame_ids cannot be empty
Can anyone point me out what am I doing wrong?
Thanks in advance,
Bruno

Update README with latest changes.

Is it can use hokuyo 2d pointcloud build elevation map?

I use hokuyo utm30lx laserscan transform to pointcloud2,and use rovio odometry as 6d pose.but the console display " can not get msg from sensor".And i have been success in kinect pointcloud.
I want to know whether is it can use hokuyo 2d pointcloud2 to build EM.

Only a part of defined map region calculated

I have been trying to use your code for visualizing elevation map of an environment set up in Gazebo.
I can get the pose of robot and point cloud data properly but still when I visualize the elevation map on rviz, I can only see a portion of map within the defined map area. The other area within map boundary remains empty (say for example for a 3x3 map size, I can see only 1x3 map on the left side).
These are the params I am using:
length_in_x: 3.0
length_in_y: 3.0
point_cloud_topic: "/firefly/vi_sensor/camera_depth/depth/points"
robot_base_frame_id: "firefly/base_link"
robot_pose_with_covariance_topic: "/firefly/vi_sensor/ground_truth/pose_with_covariance"
track_point_frame_id: "firefly/base_link"

Also there wasn't any info about the param named "sensor_frame_id" (in SensorProcessorBase.cpp) in the readme, I had to change it to accordingly to even make the map appear .
For now I have set it with this:
sensor_frame_id: "/firefly/vi_sensor/base_link"
I think this param has something to do with the problem.

Clear map service call causes EM to stop outputing maps

Running Anymal in simulation, Elevation mapper stops working (but doesn't crash) when a clear_map service call is made.

I ran the following with the latest Anymal release:
roslaunch anymal_nav_sim sim.launch
this starts to publish RealSense elevation maps.

Then a call to this service causes the elevation mapping node to stop publishing:
rosservice call /elevation_mapping/clear_map

Any tips on how to reproduce more minimally? (also seen when EM is configured to use actuated lidar input)

Error: Could not get pose information from robot

Hi,

I am getting an error where it is not detecting my pose. I sample it from /odom/pose. This contains the pose and covariance information. Not sure what else to do. Any help would be appreciated.

<node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" respawn="false" output="screen">
    <param name="point_cloud_topic" value="/camera/depth/points"/>
    <param name="robot_pose_topic" value="/odom/pose"/>
    <param name="base_frame_id" value="/base_link"/>
    <param name="map_frame_id" value="/map"/>
    <param name="track_point_frame_id" value="/base_footprint"/>
    <param name="resolution" value="0.05"/>
  </node>

Eigen fails assert after map is fused

Hello,

I am trying to run simple_demo.launch on ROS Kinetic. I believe all of my configs, topics, and transforms have been set up correctly.

When I start up the software, I can see laser scans being incorporated in RVIZ. However, when the map is fused and published, I get the following error:

elevation_mapping: /usr/include/eigen3/Eigen/src/Eigenvalues/EigenSolver.h:347: Eigen::EigenSolver<_MatrixType>::EigenvectorsType Eigen::EigenSolver<_MatrixType>::eigenvectors() const [with _MatrixType = Eigen::Matrix<double, 2, 2>; Eigen::EigenSolver<_MatrixType>::EigenvectorsType = Eigen::Matrix<std::complex, 2, 2>; typename Eigen::NumTraits::Real = double]: Assertion `m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."' failed.

A GDB trace shows that this is coming from the following line:

const double ellipseRotation(atan2(solver.eigenvectors().col(maxEigenvalueIndex).real()(1), solver.eigenvectors().col(maxEigenvalueIndex).real()(0)));

Any idea what could be causing this? It always happens after ROS_INFO prints for the first time that the map has been fused.

Thanks,
d-rohr

nan value for elevation map

Hi,
I am trying to use Intel Realsense D435 for elevation mapping. In order to get the essentials working, I omitted the advanced features.
The node is running and doesn't print any errors. The published messages contain nan only.

For tracking I use external Vicon system, which publishes the pose with 200 Hz. For simplicity the covariance matrix has been set to identity matrix. The world frame corresponds to the Vicon system frame and the base, map and sensor frame all correspond to the local frame of the tracked object in Vicon.

I copied and adjusted the launch and yaml files as described in the installation guide. Surprisingly the starleth_odometry is mentioned in the console even though it is not in the launch/yaml files. But setting this frame equal to the base frame solves this error.

Do you know, what could raise the nan problem? Can it be linked to the starleth_odometry transformation?

Thanks in advance

Time Synchronization Issues

I am using a bag file to publish the pose and point cloud topics, but this is the error I get from the elevation mapping topic.

[ERROR] [1492450225.324741178]: Requested update with time stamp 0.000000, but time of last update was 1492450225.073660.

I have tried to set the parameter of use_sim_time to True, but if I do that, the elevation mapping node does not initialize. Is there any work around for this issue, or is there a way I can help out to add this feature if it does not already exist?

Cases when elevation_mapping crashes due to compiler settings

In this issue I'd like to collect the cases when elevation_mapping segfaults because of incompatible compiler settings (because I've found at least two settings that make it crash). Feel free to add a reference or comment to your segfaults...

1. ROS Indigo, Ubuntu 14.04, PCL 1.7 installed by system, building in Debug mode: the built binary crashes at startup because of incompatibilities between c++11 support and PCL 1.7.
   • Solution: Build in Release or RelWithDebInfo modes
   • Solution: Compile PCL on your own
   • Possibly relevant issues: #33, #49
   • Possible fix in elevation_mapping: As stated in #33, this probably can't be fixed for Indigo.
2. ROS Indigo, Ubuntu 14.04, building with gcc 4.9 with optimizations (Release or RelWithDebInfo): the built binary crashes when fusing maps (in a call to GridMap::add). For some reason, the call to add is performed with uninitialized values for some vector lengths, resulting in an attempt to allocate a vector of random size, which often doesn't fit into available memory.
   • Solution: Build with gcc 4.8 or 5+.
   • Possibly relevant issues: #77, #42
   • Possible fix in elevation_mapping: I still haven't digged deep enough to find the culprit why gcc 4.9 fails. So I'd suggest checking the compiler version in CMakeLists.txt and failing the build when gcc 4.9 is detected. OpenMesh does something like this, though I haven't found any description of the gcc bug they faced.

Elevation mapping doesn't move along with Robot

Hi,
I published the elevation map with the following parameters:

point_cloud_topic: "/velodyne_points"
map_frame_id: "/map"
sensor_frame_id: "/velodyne"
robot_base_frame_id: "/camera_init"
robot_pose_with_covariance_topic: "/ground_truth_pose"
robot_pose_cache_size: 100
track_point_frame_id: "/camera_init"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

Although, the elevation map is not being built along with the robot movement.

Anyone can provide feedback what is it happening?
Regards,
Bruno

The /elevation_mapping/elevation_map topic has NAN value

I'm using "Ubuntu 16.04" and "ROS-Kinetic" branches, the only problem at compiling was "Kindr" package which I got it from GitHub and make it separately.
I have set every parameter of this package in the launch file in elevation_mapping_demos and also I have set parameters in config files in both elevation_mapping and elevation_mapping_demos.
I have also checked the topics which this node subscribes and every one of them has correct values also, the rate of publishing data has been checked.
The only topic that I can visualise is the /map_region which has a correct value.
I have noticed some part of the code has //TODO, I don't know what is the problem.
Is there any help to figure it out?
I'll appreciate any help

Verify/revise frame id handling in GetGridMap request

Currently z-component is ignored when determining the map positions with respect to the requested frame, might want to transform maps

source_frame does not exist

I am using a VLP-16 lidar sensor.
These are the configurations for elevation_mapping node:

point_cloud_topic: "/velodyne_points"
map_frame_id: "world"
robot_base_frame_id: "top_plate_link"
robot_pose_with_covariance_topic: "/pose/gazebo"
robot_pose_cache_size: 100
track_point_frame_id: "top_plate_link"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0
length_in_x: 2.5
length_in_y: 2.5
position_x: 0.0
position_y: 0.0
resolution: 0.01
min_variance: 0.000009
max_variance: 0.01
mahalanobis_distance_threshold: 2.5
multi_height_noise: 0.0000009
sensor_processor/type: laser
sensor_processor/min_radius: 0.018
sensor_processor/beam_angle: 0.0006
sensor_processor/beam_constant: 0.0015

And I get this outcomes:

[ WARN] [1518015771.987702851, 1340.760000000]: Visibility cleanup duration is too high (current rate is 0.163138).
[ INFO] [1518015771.988841483, 1340.760000000]: ElevationMap received a point cloud (17629 points) for elevation mapping.
[ERROR] [1518015775.070534283, 1341.760000000]: "sensor" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [1518015775.070570182, 1341.760000000]: Point cloud could not be processed.
[ INFO] [1518015775.071199457, 1341.760000000]: ElevationMap received a point cloud (17629 points) for elevation mapping.

I am trying to convert the point cloud to elevation map. I saw that world frame is published with a higher frequency than other frames and I can not see elevation map on rviz. What can be the problem? What am I doing wrong?

Missing dependency on 'grid_map_rviz_plugin'

Hi all,

after I cleaned my workspace and build it again I noticed that rviz could not visualize the elevation map anymore. I believe we have a missing dependency on "grid_map_rviz_plugin". I suggest we include it here. If that's okay with you I'll open a PR for this repo.

What do you think @pfankhauser @remod?

Underlying map error

Fix issue when current map is bigger than underlying map.

kindr building

Having an issue with building kindr. I cannot use catkin_make to compile a package for elevation_mapping. Terminal shows that kindr and other packages are non-homogenous

Question: What is the argument for choosing the pose interface?

Elevation mapping now listens to a pose topic with message type geometry_msgs/PoseWithCovarianceStamped. What is the argument for choosing this? Another option would be to enable the user to provide the pose using a nav_msgs/Odometry (maybe as an alternative), as this also contains the pose and covariance. A nice perk of an Odometry interface is easy integration with robot localization.

More verbose error descriptions in _Map_base::at()

Improve error descriptions (exception::what()) for improved debugging of ROS node
Current output: "Exception thrown while processing service call: _Map_base::at" when accessing a non-existing cell type

Stereo camera parameter descriptions missing/unclear

The stereo camera sensor model includes the following parameters:

• p_1
• p_2
• p_3
• p_4
• p_5
• lateral_factor
• depth_to_disparity_factor

However, there is no explanation of what these parameters mean. Can anyone help me with this?

Nan value for elevation map, also terminated due to a 'std::bad_alloc'

I was turning in "velodyne_points", "pose" and "tf" to the elevation_mapping node, however didn't get a valid elevation map. When I do
rostopic echo "elevation_map"
It shows a lot of Nan values looks like the following:

nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]
outer_start_index: 109

inner_start_index: 61

and the elevation_map node got the following error message:

terminate called after throwing an instance of 'std::bad_alloc'
what(): std::bad_alloc

Any suggestions?
Thanks!

Add example dataset to test installation

Title "Test Installation on Basic Dataset" in README

Wrong paths for the ground truth demo

I noticed the paths for the ground truth demo are wrong and the launch file does not work.

This is what is currently written:

<launch>
    <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
         <rosparam command="load" file="$(find elevation_mapping)/parameters/robots/ground_truth.yaml" />
         <rosparam command="load" file="$(find elevation_mapping)/parameters/elevation_maps/long_range.yaml" />
         <rosparam command="load" file="$(find elevation_mapping)/parameters/sensor_processors/perfect.yaml" />
    </node>
</launch>

This is what is should be:

<launch>
    <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
         <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/ground_truth_demo.yaml" />
         <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/long_range.yaml" />
         <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/perfect.yaml" />
    </node>
</launch>

Finish definition of GetSubmap service

Segmentation fault in PCL

I am trying to run elevation mapping on an Ubuntu 14 system with ROS Indigo, using the system packages for libpcl (1.7.2) and boost (1.54) and I keep getting a segmentation fault. The segmentation fault happens inside PCL, is caused by using incompatible C++ versions and is quite common (see here, here or here). It can be usually resolved by compiling PCL with C++11 or by compiling elevation mapping without C++11.

Is there a branch that doesn't require C++11? What pcl/boost versions do you use on ROS systems with elevation mapping to avoid such conflicts?

Any path planning ROS package compatible with this grid map?

This work is excellent for legged robots, however I can't find any path planning ROS package working with the grid map, Any suggestion will be appreciated. I known the grid map can be converted to pointcloud2 or occupied map.