catch Extrapolation Exception when a bad map frame location.

[Cryst4L9527]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of turtlebot

---

Description of contribution in a few bullet points

A simple exception catch for robusting the code with some malformed messages, just like #2511.

The detailed crash info is following:

[INFO] [1664191509.857372862] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[INFO] [1664191510.357362318] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[INFO] [1664191510.857486959] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 4.486000 but the earliest data is at time 4.776000, when looking up transform from frame [base_link] to frame [map]
[INFO] [1664191511.357613276] [global_costmap.global_costmap]: start
[INFO] [1664191525.125322336] [global_costmap.global_costmap_rclcpp_node]: Message Filter dropping message: frame 'base_scan' at time 6.000 for reason 'Unknown'
terminate called after throwing an instance of 'tf2::ExtrapolationException'
  what():  Lookup would require extrapolation into the past.  Requested time 8.376000 but the earliest data is at time 8.396000, when looking up transform from frame [base_scan] to frame [map]

It seems map_server and navigator got bad observations and failed to get a good map-odom tf in some timestamps, so that base_link-map tf was not available when extrapolation, which is caught by the function in robot_utils.cpp, as following:

bool transformPoseInTargetFrame(
  const geometry_msgs::msg::PoseStamped & input_pose,
  geometry_msgs::msg::PoseStamped & transformed_pose,
  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
  const double transform_timeout)
{
  static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
  try {
    transformed_pose = tf_buffer.transform(
      input_pose, target_frame,
      tf2::durationFromSec(transform_timeout));
    return true;
  } catch (tf2::LookupException & ex) {
    ...
  } catch (tf2::ExtrapolationException & ex) {
    RCLCPP_ERROR(
      logger,
      "Extrapolation Error looking up target frame: %s\n", ex.what());
  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(
      logger, "Failed to transform from %s to %s",
      input_pose.header.frame_id.c_str(), target_frame.c_str());
  }
  return false;
}

and for the globalization of local obseration in observation_buffer.cpp, a base_scan-map tf is needed to transform the local observation to global space and might be also unavailable for the above reason, the exception is just like the TransformException, can be caught in the same way:

try {
    // given these observations come from sensors...
    // we'll need to store the origin pt of the sensor
    geometry_msgs::msg::PointStamped local_origin;
    local_origin.header.stamp = cloud.header.stamp;
    local_origin.header.frame_id = origin_frame;
   ...
    sensor_msgs::msg::PointCloud2 global_frame_cloud;
    // transform the point cloud
    tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_);
   ...
     } catch (tf2::TransformException & ex) {
    // if an exception occurs, we need to remove the empty observation from the list
    observation_list_.pop_front();
    RCLCPP_ERROR(
      logger_,
      "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
      sensor_frame_.c_str(),
      cloud.header.frame_id.c_str(), ex.what());
    return;
  }

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@Cryst4L9527, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@Cryst4L9527, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[Cryst4L9527]

Well sorry, I understand from the CI build info that the exception tf2::ExtrapolationException should be caught by the tf2::TransformException first, but I still get this crash in my environment, it seems to be because of the version of something like tf2. Maybe this pull can be regarded as a clarification of the crash reason and more compatibility with diverse versions?

[codecov]

Codecov Report

Base: 82.29% // Head: 82.20% // Decreases project coverage by -0.08% ⚠️

Coverage data is based on head (1e46ccf) compared to base (987fe90).
Patch coverage: 50.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3252      +/-   ##
==========================================
- Coverage   82.29%   82.20%   -0.09%     
==========================================
  Files         341      341              
  Lines       15537    15539       +2     
==========================================
- Hits        12786    12774      -12     
- Misses       2751     2765      +14     

Impacted Files	Coverage Δ
nav2_costmap_2d/src/observation_buffer.cpp	88.15% <50.00%> (-2.39%)	⬇️
..._planner/include/nav2_smac_planner/node_hybrid.hpp	92.85% <0.00%> (-7.15%)	⬇️
...stmap_2d/plugins/costmap_filters/binary_filter.cpp	94.80% <0.00%> (-2.60%)	⬇️
..._amcl/src/sensors/laser/likelihood_field_model.cpp	95.00% <0.00%> (-2.50%)	⬇️
...ostmap_2d/plugins/costmap_filters/speed_filter.cpp	95.18% <0.00%> (-2.41%)	⬇️
nav2_navfn_planner/src/navfn_planner.cpp	89.17% <0.00%> (-1.04%)	⬇️
...tree/include/nav2_behavior_tree/bt_action_node.hpp	80.70% <0.00%> (-0.88%)	⬇️
nav2_amcl/src/amcl_node.cpp	69.33% <0.00%> (-0.42%)	⬇️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[AlexeyMerzlyakov]

In my experiments the ExtrapolationException is also being covered by the TransformException. ExtrapolationException is being a child of TransformException since 2010, so I am not sure how could you catch this exception separately. It looks like the the setup or CMake cache problem, rather than real issue.

[Cryst4L9527]

I'm confused too, it seems that all the place where runs a transform function have an exception catch, I'll try to find if somewhere else needs a exception catch.