elevation_mapping rviz not displaying anything, TF_OLD_DATA ignoring data from the past issue

rviz not displaying anything, TF_OLD_DATA ignoring data from the past issue

Open dlee640 opened this issue 3 years ago • 3 comments

Hello.

I have collected actual data from quadruped (Unitree A1) & realsense D435i to test out elevation_mapping.

First of all, here is my bagfile info:

slambox@slambox-NUC10i7FNH:~$ rosbag info terrain.bag 
path:        terrain.bag
version:     2.0
duration:    39.6s
start:       Jun 03 2021 17:29:56.23 (1622755796.23)
end:         Jun 03 2021 17:30:35.80 (1622755835.80)
size:        1.5 GB
messages:    4700
compression: none [782/782 chunks]
types:       geometry_msgs/PoseWithCovarianceStamped [953b798c0f514ff060a53a3498ce6246]
             sensor_msgs/PointCloud2                 [1158d486dd51d683ce2f1be655c3c181]
             tf2_msgs/TFMessage                      [94810edda583a504dfda3829e70d7eec]
topics:      /camera/depth/color/points    781 msgs    : sensor_msgs/PointCloud2                
             /pose                         780 msgs    : geometry_msgs/PoseWithCovarianceStamped
             /tf                          3138 msgs    : tf2_msgs/TFMessage                      (4 connections)
             /tf_static                      1 msg     : tf2_msgs/TFMessage

As you can see, I have pointcloud topic, tf topic, and a pose topic that are in correct ros msg format.

Here is my modified `realsense_demo.launch' file. Two major changes here, adding the

<param name="/use_sime_time" value="true"/>

since I am playing the data from the bagfile, and adding the

<node pkg="tf" type="static_transform_publisher" name="base2camera" args="0 0 0 0 0 0 1  /base_link /camera_link  20"/>

, since the transform between camera_link and the robot base_link was missing in my original bag file /tf...:

<launch>
  <param name="/use_sime_time" value="true"/>

  <!-- 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/simple_demo_robot.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/remove_object.yaml" />
    <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/realsense_d435.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/postprocessing/postprocessor_pipeline.yaml" />
  </node>

  <!-- Launch visualizations for the resulting elevation map -->
  <include file="$(find elevation_mapping_demos)/launch/visualization.launch" />

  <!-- Launch RViz with the demo configuration -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find elevation_mapping_demos)/rviz/elevation_map_visualization_pointcloud.rviz" />

  <node pkg="tf" type="static_transform_publisher" name="base2camera" args="0 0 0 0 0 0 1  /base_link /camera_link  20"/>

</launch>

Here is my tf tree diagram for your information: Screenshot from 2021-06-04 16-53-00

Here is my modified simple_demo_robot.yaml, and remove_object.yaml. I did not need to change realsense config file and the postprocessing file:

--> simple_demo_robot.yaml

point_cloud_topic: "/camera/depth/color/points"
map_frame_id: "map"
robot_base_frame_id: "base_link"
robot_pose_with_covariance_topic: "/pose"
robot_pose_cache_size: 200
track_point_frame_id: "base_link"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

--> remove_object.yaml

point_cloud_topic: "/camera/depth/color/points"
map_frame_id: "map"
robot_base_frame_id: "base_link"
robot_pose_with_covariance_topic: "/pose"
robot_pose_cache_size: 200
track_point_frame_id: "base"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

SO, back to how I try to run this demo,

On terminal 1, I activate roscore On terminal 2, I roslaunch elevation_mapping_demo realsense_demo.launch Output:

slambox@slambox-NUC10i7FNH:~/elevation_ws$ roslaunch elevation_mapping_demos realsense_demo.launch 
... logging to /home/slambox/.ros/log/956c77a4-c55c-11eb-94c8-f8ac65d8d6d2/roslaunch-slambox-NUC10i7FNH-10137.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://slambox-NUC10i7FNH:36143/

SUMMARY
========

PARAMETERS
 * /elevation_map_fused_visualization/grid_map_topic: /elevation_mappin...
 * /elevation_map_fused_visualization/grid_map_visualizations: [{'type': 'point_...
 * /elevation_map_raw_visualization/grid_map_topic: /elevation_mappin...
 * /elevation_map_raw_visualization/grid_map_visualizations: [{'type': 'point_...
 * /elevation_mapping/enable_skip_lower_points: True
 * /elevation_mapping/fused_map_publishing_rate: 2.0
 * /elevation_mapping/length_in_x: 6.0
 * /elevation_mapping/length_in_y: 6.0
 * /elevation_mapping/mahalanobis_distance_threshold: 2.5
 * /elevation_mapping/map_frame_id: map
 * /elevation_mapping/max_variance: 0.05
 * /elevation_mapping/min_update_rate: 2.0
 * /elevation_mapping/min_variance: 0.0001
 * /elevation_mapping/multi_height_noise: 0.01
 * /elevation_mapping/point_cloud_topic: /points
 * /elevation_mapping/position_x: 0.0
 * /elevation_mapping/position_y: 0.0
 * /elevation_mapping/postprocessor_pipeline: [{'type': 'gridMa...
 * /elevation_mapping/remove_penetrated_points_rate: 5.0
 * /elevation_mapping/resolution: 0.01
 * /elevation_mapping/robot_base_frame_id: base_link
 * /elevation_mapping/robot_motion_map_update/covariance_scale_rotation_x: 0.0
 * /elevation_mapping/robot_motion_map_update/covariance_scale_rotation_y: 0.0
 * /elevation_mapping/robot_motion_map_update/covariance_scale_rotation_z: 0.0
 * /elevation_mapping/robot_motion_map_update/covariance_scale_translation_x: 0.0
 * /elevation_mapping/robot_motion_map_update/covariance_scale_translation_y: 0.0
 * /elevation_mapping/robot_motion_map_update/covariance_scale_translation_z: 0.0
 * /elevation_mapping/robot_pose_cache_size: 500
 * /elevation_mapping/robot_pose_with_covariance_topic: /pose
 * /elevation_mapping/scanning_duration: 0.01
 * /elevation_mapping/sensor_processor/cutoff_max_depth: 3.25
 * /elevation_mapping/sensor_processor/cutoff_min_depth: 0.2
 * /elevation_mapping/sensor_processor/ignore_points_above: 0.4
 * /elevation_mapping/sensor_processor/lateral_factor: 0.01576
 * /elevation_mapping/sensor_processor/normal_factor_a: 0.000611
 * /elevation_mapping/sensor_processor/normal_factor_b: 0.003587
 * /elevation_mapping/sensor_processor/normal_factor_c: 0.3515
 * /elevation_mapping/sensor_processor/normal_factor_d: 0
 * /elevation_mapping/sensor_processor/normal_factor_e: 1
 * /elevation_mapping/sensor_processor/type: structured_light
 * /elevation_mapping/time_tolerance: 0.5
 * /elevation_mapping/track_point_frame_id: base_link
 * /elevation_mapping/track_point_x: 0.0
 * /elevation_mapping/track_point_y: 0.0
 * /elevation_mapping/track_point_z: 0.0
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /use_sime_time: True

NODES
  /
    base2camera (tf/static_transform_publisher)
    elevation_map_fused_visualization (grid_map_visualization/grid_map_visualization)
    elevation_map_raw_visualization (grid_map_visualization/grid_map_visualization)
    elevation_mapping (elevation_mapping/elevation_mapping)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[elevation_mapping-1]: started with pid [10220]
process[elevation_map_fused_visualization-2]: started with pid [10221]
process[elevation_map_raw_visualization-3]: started with pid [10222]
process[rviz-4]: started with pid [10223]
process[base2camera-5]: started with pid [10224]
[ WARN] [1622840350.850984846]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
[ WARN] [1622840350.851588941]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
[ WARN] [1622840350.851610377]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
[ INFO] [1622840350.852195450]: Elevation mapping node started.
[ INFO] [1622840350.859068350]: Grid map visualization node started.
[ INFO] [1622840350.860989480]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_cloud'.
[ INFO] [1622840350.861018451]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'upper_bound_cloud'.
[ INFO] [1622840350.861033921]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'lower_bound_cloud'.
[ INFO] [1622840350.861050930]: grid_map_visualizations: Configured visualization of type 'map_region' with name 'map_region'.
[ INFO] [1622840350.862520506]: Grid map visualization node started.
[ INFO] [1622840350.863130264]: Elevation map grid resized to 600 rows and 600 columns.
[ INFO] [1622840350.865432893]: Grid map visualization initialized.
[ INFO] [1622840350.866067055]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_cloud'.
[ INFO] [1622840350.866088287]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'sigma_cloud'.
[ INFO] [1622840350.866107723]: grid_map_visualizations: Configured visualization of type 'map_region' with name 'map_region'.
[ INFO] [1622840350.867998181]: Grid map visualization initialized.
[ WARN] [1622840350.875219243]: Could not load the input sources configuration from parameter
 /elevation_mapping/input_sources, are you sure it was pushed to the parameter server? Assuming
 that you meant to leave it empty. Not subscribing to any inputs!

[ WARN] [1622840350.875392714]: Parameter 'point_cloud_topic' is deprecated, please use 'input_sources' instead.
[ INFO] [1622840350.879457211]: Elevation mapping node initializing ... 
[ INFO] [1622840351.880023386]: Done initializing.

On terminal 3, I rosbag play myfile.bag that contains appropriate topics..

Then on rviz, I try to visualize all the topics that are supposedly output from elevation_mapping package, but I see nothing!!

Screenshot from 2021-06-04 16-56-47

When I try to play the bagfile again since my bagfile is kind of short, this error starts printing on the roslaunch terminal (terminal 2):

Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame base_link at time 1.62276e+09 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame base_link at time 1.62276e+09 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame odom at time 1.62276e+09 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame odom at time 1.62276e+09 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame laser at time 1.62276e+09 according to authority unknown_publisher
Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
         at line 277 in /tmp/binarydeb/ros-melodic-tf2-0.6.5/src/buffer_core.cpp
Warning: TF_OLD_DATA ignoring data from the past for frame laser at time 1.62276e+09 according to authority unknown_publisher

I am not sure why nothing is showing on my rviz despite following all the procedures. Which topic do I need to visualize? When I rostopic echo /elevation_mapping/elevation_map I get nan responses, but I seem to get responses for elevation_map_fused_visualization/elevation_cloud.

slambox@slambox-NUC10i7FNH:~$ rostopic echo elevation_map_fused_visualization/elevation_cloud
header: 
  seq: 253
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "map"
height: 1
width: 0
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "upper_bound"
    offset: 12
    datatype: 7
    count: 1
  - 
    name: "lower_bound"
    offset: 16
    datatype: 7
    count: 1
  - 
    name: "rgb"
    offset: 20
    datatype: 7
    count: 1
  - 
    name: "uncertainty_range"
    offset: 24
    datatype: 7
    count: 1
is_bigendian: False
point_step: 28
row_step: 0
data: []
is_dense: False
---
header: 
  seq: 254
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "map"
height: 1
width: 0
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "upper_bound"
    offset: 12
    datatype: 7
    count: 1
  - 
    name: "lower_bound"
    offset: 16
    datatype: 7
    count: 1
  - 
    name: "rgb"
    offset: 20
    datatype: 7
    count: 1
  - 
    name: "uncertainty_range"
    offset: 24
    datatype: 7
    count: 1
is_bigendian: False
point_step: 28
row_step: 0
data: []
is_dense: False
---

If someone can spot my error, that would be appreciated. Thank you!

Jun 04 '21 21:06 dlee640

I have similar problems

Jun 14 '21 09:06 Miracle-qi

Hey, I think your issue is here:

[ WARN] [1622840350.875219243]: Could not load the input sources configuration from parameter
 /elevation_mapping/input_sources, are you sure it was pushed to the parameter server? Assuming
 that you meant to leave it empty. Not subscribing to any inputs!

You should extend the configuration with:

  input_sources:
   input_front:
     type: pointcloud
     topic: /camera/depth/color/points
     queue_size: 1
     publish_on_update: true
     sensor_processor:
       ignore_points_above: .inf
       ignore_points_below: -.inf
       type: structured_light
       cutoff_min_depth: 0.1
       cutoff_max_depth: 1.5
       normal_factor_a: 0.001063
       normal_factor_b:  0.003949
       normal_factor_c: 0.0
       normal_factor_d: 0.0008278
       normal_factor_e: 1
       lateral_factor: 0.01576 # approx 1 deg

Jun 30 '21 09:06 maximilianwulf

I have same question. And I want to know how to add the input_sources file and which type is it？

Dec 14 '21 15:12 Zhangwenyao1

elevation_mapping elevation_mapping copied to clipboard

rviz not displaying anything, TF_OLD_DATA ignoring data from the past issue

elevation_mapping
elevation_mapping copied to clipboard