search

ANYbotics / elevation_mapping

Robot-centric elevation mapping for rough terrain navigation
BSD 3-Clause "New" or "Revised" License
1.37k stars 446 forks source link

Layers of elevation_map full of 'nan' #131

Open h3ct0r opened 4 years ago

h3ct0r commented 4 years ago

Hi! First, thanks for making this package available :)

I'm trying to make the package work with a custom turtle-bot based robot, and I'm trying to figure out where is the missing piece to make the package work.

The turtlesim3_waffle_demo.launch worked normally, but with my particular configuration, the map is only outputting nan values, and I cannot see any error in the console even in DEBUG mode.

What could this problem be? I'm using a Kinect sensor. Thanks!

My YAML config (myrobot.yaml) looks like this:

# Robot.
map_frame_id:                               odom
robot_base_frame_id:                        base_footprint
sensor_frame_id:                            camera_depth_optical_frame
robot_pose_with_covariance_topic:           /odom_combined
robot_pose_cache_size:                      200
point_cloud_topic:                          /camera/depth/points
track_point_frame_id:                       base_footprint
track_point_x:                              0.0
track_point_y:                              0.0
track_point_z:                              0.0
min_update_rate:                            2.0
time_tolerance:                             1.0
time_offset_for_point_cloud:                0.0
sensor_processor/ignore_points_above:       0.4
robot_motion_map_update/covariance_scale:   0.01

# Map.
length_in_x:                                4.0
length_in_y:                                4.0
position_x:                                 0.0
position_y:                                 0.0
resolution:                                 0.08
min_variance:                               0.00009
max_variance:                               0.01
mahalanobis_distance_threshold:             2.5
multi_height_noise:                         0.002
surface_normal_positive_axis:               z
fused_map_publishing_rate:                  0.5
enable_visibility_cleanup:                  false
visibility_cleanup_rate:                    1.0
scanning_duration:                          1.0

# Init submap
initialize_elevation_map:                   false
initialization_method:                      0
length_in_x_init_submap:                    1.0
length_in_y_init_submap:                    1.0
margin_init_submap:                         0.3
init_submap_height_offset:                  0.01
target_frame_init_submap:                   base_footprint

The transformation between the camera frame and the robot exists:

rosrun tf tf_echo base_footprint camera_depth_optical_frame
At time 0.000
- Translation: [-0.087, 0.013, 0.297]
- Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500]
            in RPY (radian) [-1.571, -0.000, -1.571]
            in RPY (degree) [-90.000, -0.000, -90.000]
rosrun tf tf_echo odom base_footprint 
At time 1497.461
- Translation: [-6.271, -0.156, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.989, -0.149]
            in RPY (radian) [0.001, -0.001, -2.843]
            in RPY (degree) [0.030, -0.039, -162.888]

The pose with covariance is given by the odom + IMU with the robot_pose_ekf package:

rostopic info /odom_combined
Type: geometry_msgs/PoseWithCovarianceStamped

Publishers: 
 * /robot_pose_ekf (http://computer:35329/)

Subscribers: 
 * /elevation_mapping (http://computer:38809/)
 * /rviz (http://computer:42720/)

My launch file looks like this:

  <!-- Elevation mapping node -->
  <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/myrobot.yaml" />
     <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/kinect_nguyen_et_al.yaml" />
  </node>

And the output with DEBUG flags looks like this:

[ INFO] [1581555088.648572068]: Elevation mapping node started.
[ INFO] [1581555088.690070893]: Elevation map grid resized to 50 rows and 50 columns.
[ INFO] [1581555088.773560937]: Elevation mapping node initializing ... 
[ INFO] [1581555089.872467718, 1061.134000000]: Done initializing.
[ INFO] [1581555089.880065807, 1061.138000000]: First corresponding point cloud and pose found, elevation mapping started. 
[DEBUG] [1581555089.894130976, 1061.154000000]: ElevationMap received a point cloud (307200 points) for elevation mapping.
[DEBUG] [1581555089.894198556, 1061.154000000]: Elevation map is checked for relocalization.
[DEBUG] [1581555089.894462649, 1061.154000000]: Elevation map has been moved to position (-6.240000, -0.160000).
[DEBUG] [1581555089.894487675, 1061.154000000]: Updating map with latest prediction from time 0.000000.
[ERROR] [1581555089.894507424, 1061.154000000]: Could not get pose information from robot for time 1061.083000. Buffer empty?
[ERROR] [1581555089.894524581, 1061.154000000]: Updating process noise failed.
[DEBUG] [1581555089.937094472, 1061.196000000]: ElevationMap received a point cloud (307200 points) for elevation mapping.
[DEBUG] [1581555089.937280483, 1061.196000000]: Elevation map is checked for relocalization.
[DEBUG] [1581555089.937375384, 1061.196000000]: Updating map with latest prediction from time 1061.160000.
[DEBUG] [1581555089.945017191, 1061.204000000]: Point cloud transformed to frame camera_depth_optical_frame for time stamp 1061132000.000000.
[DEBUG] [1581555089.951781006, 1061.211000000]: cleanPointCloud() reduced point cloud to 115740 points.
[DEBUG] [1581555089.955785833, 1061.215000000]: Limiting point cloud to the height interval of [-inf, 0.400000] relative to the robot base.
[DEBUG] [1581555089.961000827, 1061.220000000]: removePointsOutsideLimits() reduced point cloud to 84178 points.
[DEBUG] [1581555090.116506230, 1061.369000000]: Raw map has been updated with a new point cloud in 0.115524 s.
[DEBUG] [1581555090.126854044, 1061.377000000]: ElevationMap received a point cloud (307200 points) for elevation mapping.
[DEBUG] [1581555090.127061399, 1061.377000000]: Elevation map is checked for relocalization.
[DEBUG] [1581555090.127213671, 1061.377000000]: Updating map with latest prediction from time 1061.160000.
[DEBUG] [1581555090.146006097, 1061.394000000]: Limiting point cloud to the height interval of [-inf, 0.400000] relative to the robot base.
[DEBUG] [1581555090.148020359, 1061.395000000]: removePointsOutsideLimits() reduced point cloud to 84178 points.
[DEBUG] [1581555090.280338873, 1061.517000000]: Raw map has been updated with a new point cloud in 0.098854 s.
[DEBUG] [1581555090.287962711, 1061.524000000]: ElevationMap received a point cloud (307200 points) for elevation mapping.
[DEBUG] [1581555090.288007030, 1061.524000000]: Elevation map is checked for relocalization.
[DEBUG] [1581555090.288051888, 1061.524000000]: Updating map with latest prediction from time 1061.461000.
[DEBUG] [1581555090.306167399, 1061.535000000]: Limiting point cloud to the height interval of [-inf, 0.400000] relative to the robot base.
[DEBUG] [1581555090.308592203, 1061.537000000]: removePointsOutsideLimits() reduced point cloud to 84178 points.
^C[DEBUG] [1581555090.438599661, 1061.657000000]: Raw map has been updated with a new point cloud in 0.098192 s.

When echoing the contents of /elevation_mapping/elevation_map I'm seeing the layers full of nan like in the attached picture: echo_elevation_mapping

maximilianwulf commented 4 years ago

Hi @h3ct0r,

could you run your setup with DEBUG messages to see if elevation mapping adds points from the point cloud?

Can you also plot the pose values? Some points of the point cloud get filtered out of the elevation map because of the following messages:

[DEBUG] [1581555090.146006097, 1061.394000000]: Limiting point cloud to the height interval of [-inf, 0.400000] relative to the robot base.
[DEBUG] [1581555090.148020359, 1061.395000000]: removePointsOutsideLimits() reduced point cloud to 84178 points.

It could be that only points remain that are outside of the defined elevation map area.