Could not get pose information from robot for time

Charifou commented 1 month ago

Hi Muhammad and thanks for porting the package to ROS2. it will be very useful. I have tried to use it with a husky robot simulated in gazebo but I get this well known error:

[elevation_mapping]: Could not get pose information from robot for time 4.898000. Buffer empty?

I use as config files : "robots/husky.yaml", "elevation_maps/long_range.yaml", "sensor_processors/perfect.yaml", "postprocessing/postprocessor_pipeline.yaml"

husky.yaml is in the same format as ground_truth.yaml with "use_sim_time: true" in addition.

I have the same issue with a bag file. Do have any idea about where that might come from ? Thanks !

Charifou commented 1 month ago

robot_pose_with_covariance_topic: directly take a [nav_msgs/Odometry] msg topic not a [geometry_msgs/PoseWithCovarianceStamped.msg] like in ROS1.

Muhammad540 commented 1 month ago

Yes,

robot_pose_with_covariance_topic: directly take a [nav_msgs/Odometry] msg topic not a [geometry_msgs/PoseWithCovarianceStamped.msg] like in ROS1.

Yes you are right, i also experienced this issue but i forgot to add this in Readme, thanks for sharing this i will also update the Readme to point this issue.

Charifou commented 1 month ago

Yes,

robot_pose_with_covariance_topic: directly take a [nav_msgs/Odometry] msg topic not a [geometry_msgs/PoseWithCovarianceStamped.msg] like in ROS1.

Yes you are right, i also experienced this issue but i forgot to add this in Readme, thanks for sharing this i will also update the Readme to point this issue.

Do you know why the map is continuously reset after some time ? There is no aggregation during time; Is there a parameter I am not setting well ?

Muhammad540 commented 1 month ago

Yes,

robot_pose_with_covariance_topic: directly take a [nav_msgs/Odometry] msg topic not a [geometry_msgs/PoseWithCovarianceStamped.msg] like in ROS1.

Yes you are right, i also experienced this issue but i forgot to add this in Readme, thanks for sharing this i will also update the Readme to point this issue.

Do you know why the map is continuously reset after some time ? There is no aggregation during time; Is there a parameter I am not setting well ?

Do you mean the elevation values becomes 0 ? I did not experience this, however ensure that you are consistently getting the sensor data. Make sure that continuous cleanup is set to False. i am not sure at the moment the exact issue you are facing maybe if you provide more details, i can help.

Charifou commented 1 month ago

The values becomes nan, the map is erased even when continuous and visibility cleanup are set to false. In fact I realized that the aggregated map is published after post-processing. I don't know why the raw elevation map is continuously cleaned.

RyanCooper95 commented 1 month ago

Hello guys, since I am experiencing the same original problem about not gettin the pose properly from the robot I will explain my problem here, maybe I discovered another issue. Also, thank you @Muhammad540 for the porting, it is really helpful.

Anyway, I am trying to integrate the elevation_map package inside a Nav2 stack for a custom robot.

I am currently in simulation, specifically Gazebo classic.
The Nav2 stack uses AMCL as well.
I made a 2D map of the Gazebo world using SLAM_toolbox.
My odometry comes from the gz differential drive controller.
The 3D pointcloud comes from a simulated stereocamera, which I can see correctly on rviz.

Everything works well, I am even starting the elevation_mapping node (attached the gdb traceback):

However, the problem is that I can't see the elevation map in rviz, mostly because if I do an echo on the /elevation_map topic, it has always all nan values.

What i did up to now: My configuration parameters for the node (for now I pass them in the launch because for some reason the config files are not working for me) are the following:

Node(
            package='elevation_mapping',
            executable='elevation_mapping',
            name='elevation_mapping_node',
            output='screen',
            parameters=[{
                'inputs': ['stereocamera'],
                'use_sim_time': True,
                'input_sources': '/zed_camera_front_left_camera/points', 
                'stereocamera.type': 'pointcloud',
                'stereocamera.sensor_processor.type': 'perfect',
                'stereocamera.topic': '/zed_camera_front_left_camera/points',
                'stereocamera.publish_on_update': True,
                'stereocamera.queue_size': 1,
                'map_frame_id': "odom",
                'robot_base_frame_id': "base_link",
                'robot_pose_with_covariance_topic': "",  # Leaving this empty skips pose covariance updates.
                'robot_pose_cache_size': 1000,
                'track_point_frame_id': "base_link",
                'min_update_rate': 5.0,
                'track_point_x': 0.0,
                'track_point_y': 0.0,
                'track_point_z': 0.0,
                'length_in_x': 6.0,
                'length_in_y': 6.0,
                'position_x': 0.0,
                'position_y': 0.0,
                'resolution': 0.1,
                'min_variance': 0.000009,
                'max_variance': 0.01,
                'mahalanobis_distance_threshold': 2.5,
                'multi_height_noise': 0.0000009,
                'postprocessor_pipeline.filter1.name' : 'inpaint',
                'postprocessor_pipeline.filter1.type' : 'gridMapCv/InpaintFilter',
                'postprocessor_pipeline.filter1.params.input_layer' : 'elevation',
                'postprocessor_pipeline.filter1.params.output_layer' : 'elevation_inpainted',
                'postprocessor_pipeline.filter1.params.radius' : 0.05,
                'postprocessor_pipeline.filter2.name' : 'surface_normals',
                'postprocessor_pipeline.filter2.type' : 'gridMapFilters/NormalVectorsFilter',
                'postprocessor_pipeline.filter2.params.input_layer' : 'elevation_inpainted',
                'postprocessor_pipeline.filter2.params.output_layers_prefix' : 'elevation_inpainted',
                'postprocessor_pipeline.filter2.params.radius' : 0.1,
                'postprocessor_pipeline.filter2.params.normal_vector_positive_axis' : 'z',
            }],

            prefix=['xterm -e gdb -ex run --args']
        )

and my complete list of topics is the following:

/amcl/transition_event
/amcl_pose
/behavior_server/transition_event
/behavior_tree_log
/bond
/bt_navigator/transition_event
/clicked_point
/clock
/cmd_vel
/controller_server/transition_event
/diagnostics
/downsampled_costmap
/downsampled_costmap_updates
/dynamic_joint_states
/elevation_map
/elevation_map_raw_post
/global_costmap/costmap
/global_costmap/costmap_raw
/global_costmap/costmap_updates
/global_costmap/footprint
/global_costmap/global_costmap/transition_event
/global_costmap/published_footprint
/global_costmap/voxel_marked_cloud
/goal_pose
/imu/data
/initialpose
/joint_state_broadcaster/transition_event
/joint_states
/local_costmap/clearing_endpoints
/local_costmap/costmap
/local_costmap/costmap_raw
/local_costmap/costmap_updates
/local_costmap/footprint
/local_costmap/local_costmap/transition_event
/local_costmap/published_footprint
/local_costmap/voxel_grid
/local_costmap/voxel_marked_cloud
/local_plan
/map
/map_server/transition_event
/map_updates
/mobile_base/sensors/bumper_pointcloud
/odom
/parameter_events
/particle_cloud
/performance_metrics
/plan
/plan_smoothed
/planner_server/transition_event
/points
/robot_description
/robot_diff_drive/cmd_vel
/robot_diff_drive/odom
/robot_diff_drive/transition_event
/rosout
/scan
/smoother_server/transition_event
/speed_limit
/tf
/tf_static
/trajectories
/transformed_global_plan
/velocity_smoother/transition_event
/visibility_cleanup_map
/waypoint_follower/transition_event
/waypoints
/zed_camera_front_left_camera/camera_info
/zed_camera_front_left_camera/depth/camera_info
/zed_camera_front_left_camera/depth/image_raw
/zed_camera_front_left_camera/depth/image_raw/compressed
/zed_camera_front_left_camera/depth/image_raw/compressedDepth
/zed_camera_front_left_camera/depth/image_raw/theora
/zed_camera_front_left_camera/image_raw
/zed_camera_front_left_camera/image_raw/compressed
/zed_camera_front_left_camera/image_raw/compressedDepth
/zed_camera_front_left_camera/image_raw/theora
/zed_camera_front_left_camera/points
/zed_camera_front_left_raw_camera/camera_info
/zed_camera_front_left_raw_camera/image_raw
/zed_camera_front_left_raw_camera/image_raw/compressed
/zed_camera_front_left_raw_camera/image_raw/compressedDepth
/zed_camera_front_left_raw_camera/image_raw/theora
/zed_camera_front_right_camera/camera_info
/zed_camera_front_right_camera/image_raw
/zed_camera_front_right_camera/image_raw/compressed
/zed_camera_front_right_camera/image_raw/compressedDepth
/zed_camera_front_right_camera/image_raw/theora
/zed_camera_front_right_raw_camera/camera_info
/zed_camera_front_right_raw_camera/image_raw
/zed_camera_front_right_raw_camera/image_raw/compressed
/zed_camera_front_right_raw_camera/image_raw/compressedDepth
/zed_camera_front_right_raw_camera/image_raw/theora
/zed_imu_plugin/out

while the rqt_graph and the tf tree of my robot are the following: rosgraph

I tried several combinations of configuration parameters in terms of frames and whatever. Also, I am not using in my current configuration the robot_pose_with_covariance_topic but I tried launch the node with /odom, /robot_diff_drive/odom (which are both nav_msgs/msg/Odometry type) and /amcl_pose (which is geometry_msgs/PoseWithCovarianceStamped type).

My guess: the robot is indeed not receiving the poses and this does not allow to trigger the lidarcallback (i cant see on the GDB the log messages i put inside the code for debugging:

// Check if point cloud has corresponding robot pose at the beginning
  if (!receivedFirstMatchingPointcloudAndPose_) {

    RCLCPP_INFO(nodeHandle_->get_logger(), "Before conditional check: receivedFirstMatchingPointcloudAndPose_ = %s", receivedFirstMatchingPointcloudAndPose_ ? "true" : "false");

    const double oldestPoseTime = robotPoseCache_.getOldestTime().seconds();
    const double currentPointCloudTime = rclcpp::Time(pointCloudMsg->header.stamp,RCL_ROS_TIME).seconds();

     RCLCPP_INFO(nodeHandle_->get_logger(), "Oldest Pose Time: %f, Current Point Cloud Time: %f", oldestPoseTime, currentPointCloudTime);

    if (currentPointCloudTime < oldestPoseTime) {
      auto clock = nodeHandle_->get_clock();
      RCLCPP_WARN_THROTTLE(nodeHandle_->get_logger(), *(clock), 5, "No corresponding point cloud and pose are found. Waiting for first match. (Warning message is throttled, 5s.)");
      return;
    } else {
      RCLCPP_INFO(nodeHandle_->get_logger(), "First corresponding point cloud and pose found, elevation mapping started. ");
      receivedFirstMatchingPointcloudAndPose_ = true;
      RCLCPP_INFO(nodeHandle_->get_logger(), "After setting: receivedFirstMatchingPointcloudAndPose_ = %s", receivedFirstMatchingPointcloudAndPose_ ? "true" : "false");
    }

My final guesses are the following:

I am doing something terribly wrong in the config params.
The tfs for some reason are not reaching the elevation_map node
Synchronization issues since now and then I get this message on the terminal

I tried to be the more detailed possible. Your help or even some suggestions are highly appreciated. Thank you!

Muhammad540 commented 1 month ago

Hello guys, since I am experiencing the same original problem about not gettin the pose properly from the robot I will explain my problem here, maybe I discovered another issue. Also, thank you @Muhammad540 for the porting, it is really helpful.

Anyway, I am trying to integrate the elevation_map package inside a Nav2 stack for a custom robot.

I am currently in simulation, specifically Gazebo classic.
The Nav2 stack uses AMCL as well.
I made a 2D map of the Gazebo world using SLAM_toolbox.
My odometry comes from the gz differential drive controller.
The 3D pointcloud comes from a simulated stereocamera, which I can see correctly on rviz.

Everything works well, I am even starting the elevation_mapping node (attached the gdb traceback):

However, the problem is that I can't see the elevation map in rviz, mostly because if I do an echo on the /elevation_map topic, it has always all nan values.

What i did up to now: My configuration parameters for the node (for now I pass them in the launch because for some reason the config files are not working for me) are the following:

Node(
            package='elevation_mapping',
            executable='elevation_mapping',
            name='elevation_mapping_node',
            output='screen',
            parameters=[{
                'inputs': ['stereocamera'],
                'use_sim_time': True,
                'input_sources': '/zed_camera_front_left_camera/points', 
                'stereocamera.type': 'pointcloud',
                'stereocamera.sensor_processor.type': 'perfect',
                'stereocamera.topic': '/zed_camera_front_left_camera/points',
                'stereocamera.publish_on_update': True,
                'stereocamera.queue_size': 1,
                'map_frame_id': "odom",
                'robot_base_frame_id': "base_link",
                'robot_pose_with_covariance_topic': "",  # Leaving this empty skips pose covariance updates.
                'robot_pose_cache_size': 1000,
                'track_point_frame_id': "base_link",
                'min_update_rate': 5.0,
                'track_point_x': 0.0,
                'track_point_y': 0.0,
                'track_point_z': 0.0,
                'length_in_x': 6.0,
                'length_in_y': 6.0,
                'position_x': 0.0,
                'position_y': 0.0,
                'resolution': 0.1,
                'min_variance': 0.000009,
                'max_variance': 0.01,
                'mahalanobis_distance_threshold': 2.5,
                'multi_height_noise': 0.0000009,
                'postprocessor_pipeline.filter1.name' : 'inpaint',
                'postprocessor_pipeline.filter1.type' : 'gridMapCv/InpaintFilter',
                'postprocessor_pipeline.filter1.params.input_layer' : 'elevation',
                'postprocessor_pipeline.filter1.params.output_layer' : 'elevation_inpainted',
                'postprocessor_pipeline.filter1.params.radius' : 0.05,
                'postprocessor_pipeline.filter2.name' : 'surface_normals',
                'postprocessor_pipeline.filter2.type' : 'gridMapFilters/NormalVectorsFilter',
                'postprocessor_pipeline.filter2.params.input_layer' : 'elevation_inpainted',
                'postprocessor_pipeline.filter2.params.output_layers_prefix' : 'elevation_inpainted',
                'postprocessor_pipeline.filter2.params.radius' : 0.1,
                'postprocessor_pipeline.filter2.params.normal_vector_positive_axis' : 'z',
            }],

            prefix=['xterm -e gdb -ex run --args']
        )

and my complete list of topics is the following:

/amcl/transition_event
/amcl_pose
/behavior_server/transition_event
/behavior_tree_log
/bond
/bt_navigator/transition_event
/clicked_point
/clock
/cmd_vel
/controller_server/transition_event
/diagnostics
/downsampled_costmap
/downsampled_costmap_updates
/dynamic_joint_states
/elevation_map
/elevation_map_raw_post
/global_costmap/costmap
/global_costmap/costmap_raw
/global_costmap/costmap_updates
/global_costmap/footprint
/global_costmap/global_costmap/transition_event
/global_costmap/published_footprint
/global_costmap/voxel_marked_cloud
/goal_pose
/imu/data
/initialpose
/joint_state_broadcaster/transition_event
/joint_states
/local_costmap/clearing_endpoints
/local_costmap/costmap
/local_costmap/costmap_raw
/local_costmap/costmap_updates
/local_costmap/footprint
/local_costmap/local_costmap/transition_event
/local_costmap/published_footprint
/local_costmap/voxel_grid
/local_costmap/voxel_marked_cloud
/local_plan
/map
/map_server/transition_event
/map_updates
/mobile_base/sensors/bumper_pointcloud
/odom
/parameter_events
/particle_cloud
/performance_metrics
/plan
/plan_smoothed
/planner_server/transition_event
/points
/robot_description
/robot_diff_drive/cmd_vel
/robot_diff_drive/odom
/robot_diff_drive/transition_event
/rosout
/scan
/smoother_server/transition_event
/speed_limit
/tf
/tf_static
/trajectories
/transformed_global_plan
/velocity_smoother/transition_event
/visibility_cleanup_map
/waypoint_follower/transition_event
/waypoints
/zed_camera_front_left_camera/camera_info
/zed_camera_front_left_camera/depth/camera_info
/zed_camera_front_left_camera/depth/image_raw
/zed_camera_front_left_camera/depth/image_raw/compressed
/zed_camera_front_left_camera/depth/image_raw/compressedDepth
/zed_camera_front_left_camera/depth/image_raw/theora
/zed_camera_front_left_camera/image_raw
/zed_camera_front_left_camera/image_raw/compressed
/zed_camera_front_left_camera/image_raw/compressedDepth
/zed_camera_front_left_camera/image_raw/theora
/zed_camera_front_left_camera/points
/zed_camera_front_left_raw_camera/camera_info
/zed_camera_front_left_raw_camera/image_raw
/zed_camera_front_left_raw_camera/image_raw/compressed
/zed_camera_front_left_raw_camera/image_raw/compressedDepth
/zed_camera_front_left_raw_camera/image_raw/theora
/zed_camera_front_right_camera/camera_info
/zed_camera_front_right_camera/image_raw
/zed_camera_front_right_camera/image_raw/compressed
/zed_camera_front_right_camera/image_raw/compressedDepth
/zed_camera_front_right_camera/image_raw/theora
/zed_camera_front_right_raw_camera/camera_info
/zed_camera_front_right_raw_camera/image_raw
/zed_camera_front_right_raw_camera/image_raw/compressed
/zed_camera_front_right_raw_camera/image_raw/compressedDepth
/zed_camera_front_right_raw_camera/image_raw/theora
/zed_imu_plugin/out

while the rqt_graph and the tf tree of my robot are the following: rosgraph

I tried several combinations of configuration parameters in terms of frames and whatever. Also, I am not using in my current configuration the robot_pose_with_covariance_topic but I tried launch the node with /odom, /robot_diff_drive/odom (which are both nav_msgs/msg/Odometry type) and /amcl_pose (which is geometry_msgs/PoseWithCovarianceStamped type).

My guess: the robot is indeed not receiving the poses and this does not allow to trigger the lidarcallback (i cant see on the GDB the log messages i put inside the code for debugging:

// Check if point cloud has corresponding robot pose at the beginning
  if (!receivedFirstMatchingPointcloudAndPose_) {

    RCLCPP_INFO(nodeHandle_->get_logger(), "Before conditional check: receivedFirstMatchingPointcloudAndPose_ = %s", receivedFirstMatchingPointcloudAndPose_ ? "true" : "false");

    const double oldestPoseTime = robotPoseCache_.getOldestTime().seconds();
    const double currentPointCloudTime = rclcpp::Time(pointCloudMsg->header.stamp,RCL_ROS_TIME).seconds();

     RCLCPP_INFO(nodeHandle_->get_logger(), "Oldest Pose Time: %f, Current Point Cloud Time: %f", oldestPoseTime, currentPointCloudTime);

    if (currentPointCloudTime < oldestPoseTime) {
      auto clock = nodeHandle_->get_clock();
      RCLCPP_WARN_THROTTLE(nodeHandle_->get_logger(), *(clock), 5, "No corresponding point cloud and pose are found. Waiting for first match. (Warning message is throttled, 5s.)");
      return;
    } else {
      RCLCPP_INFO(nodeHandle_->get_logger(), "First corresponding point cloud and pose found, elevation mapping started. ");
      receivedFirstMatchingPointcloudAndPose_ = true;
      RCLCPP_INFO(nodeHandle_->get_logger(), "After setting: receivedFirstMatchingPointcloudAndPose_ = %s", receivedFirstMatchingPointcloudAndPose_ ? "true" : "false");
    }

My final guesses are the following:

I am doing something terribly wrong in the config params.
The tfs for some reason are not reaching the elevation_map node
Synchronization issues since now and then I get this message on the terminal

I tried to be the more detailed possible. Your help or even some suggestions are highly appreciated. Thank you!

I apologize for a late response, i was also experiencing this "nan" issue and for me it was due to very high sensor noise variance, for some reason. The pose topic in "nav_msgs/Odometry" is necessary for the computation of the elevation map because if you go through the paper published by anybotics they use this to compute the uncertainty in the elevation map, i would suggest ensuring that you provide the correct pose info. Also inside the "add" method in elevation_map.cpp file , observe your variance values while running the elevation map, any irregularities in those can infact lead to "nan" values in downstream calculations.

RyanCooper95 commented 1 month ago

@Muhammad540 dont worry, actually thank you for the answer.

I am double checking the code and the node configuration again. From my findings it seems that athough the pointcloudcallback is correctly registered, it is never triggered. According to what you were saying:

Also inside the "add" method in elevation_map.cpp file , observe your variance values while running the elevation map, any irregularities in those can infact lead to "nan" values in downstream calculations.

do you think that variance can be also a cause for not triggering the callback? i don't think so, but it is just to double-check if I am wrong or not.

Charifou commented 1 month ago

As I said, in my case I can visualize the map on the "/elevation_map_raw_post" topic not in "/elevation_map". In this second one, the map of course is erased and values become NaN.

RyanCooper95 commented 1 month ago

Yeah after these modifications I think I am aligned also with you @Charifou. I can see the raw costmap and I get your same error

Steps performed:

Depth camera update rate: passed from 15 Hz to 30 Hz.
Started the gazebo
no AMCL, no Nav
Set up the PointCloud2 visualized in Gazebo.
Fire up the elevation mapping node
Setup the gridmap visualizer on the raw costmap

Muhammad540 / elevation_mapping

Could not get pose information from robot for time #1