Fix segfault in some cases and reduce extrapolation to the future

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -309,8 +309,14 @@ SerestController::closest_obstacle_distance(
   .downsample(0.3);
   const auto & fused = view.as_points();
 
-  if (last_input_ts_ < view.get_latest_stamp()) {
-    last_input_ts_ = view.get_latest_stamp();
+  {
+    const auto view_latest = view.get_latest_stamp();
+    const rclcpp::Time view_latest_same_clock(
+      view_latest.nanoseconds(),
+      last_input_ts_.get_clock_type());
+    if (last_input_ts_ < view_latest_same_clock) {
+      last_input_ts_ = view_latest_same_clock;
+    }
   }
 
   double min_dist = std::numeric_limits<double>::infinity();
@@ -378,10 +384,10 @@ SerestController::fetch_required_inputs(
   odom = nav_state.get<nav_msgs::msg::Odometry>("robot_pose");
 
   if (rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) {
-    last_input_ts_ = path.header.stamp;
+    last_input_ts_ = rclcpp::Time(path.header.stamp, last_input_ts_.get_clock_type());
   }
   if (rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type()) > last_input_ts_) {
-    last_input_ts_ = odom.header.stamp;
+    last_input_ts_ = rclcpp::Time(odom.header.stamp, last_input_ts_.get_clock_type());
   }
 
   if (path.poses.empty()) {

controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp

@@ -111,7 +111,10 @@ SimpleController::update_rt(NavState & nav_state)
   const auto & pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
   const auto & goal_pose = path.poses.back().pose;
 
-  rclcpp::Time latest_stamp = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").header.stamp;
+  const auto clock_type = get_node()->get_clock()->get_clock_type();
+  rclcpp::Time latest_stamp(
+    nav_state.get<nav_msgs::msg::Odometry>("robot_pose").header.stamp,
+    clock_type);
   if (rclcpp::Time(path.poses.back().header.stamp,
       latest_stamp.get_clock_type()) > latest_stamp)
   {

planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp

@@ -157,12 +157,12 @@ void CostmapPlanner::update(NavState & nav_state)
 
   rclcpp::Time latest_stamp = nav_state.get<rclcpp::Time>("map_time");
   if (rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type()) > latest_stamp) {
-    latest_stamp = robot_pose.header.stamp;
+    latest_stamp = rclcpp::Time(robot_pose.header.stamp, latest_stamp.get_clock_type());
   }
   if (rclcpp::Time(goals.goals.front().header.stamp,
       latest_stamp.get_clock_type()) > latest_stamp)
   {
-    latest_stamp = goals.goals.front().header.stamp;
+    latest_stamp = rclcpp::Time(goals.goals.front().header.stamp, latest_stamp.get_clock_type());
   }
   current_path_.header.stamp = latest_stamp;
 

planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp

@@ -130,7 +130,8 @@ SimplePlanner::update(NavState & nav_state)
   const auto & goal = goals.goals.front().pose;
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
-  rclcpp::Time latest_stamp = robot_pose.header.stamp;
+  const auto clock_type = get_node()->get_clock()->get_clock_type();
+  rclcpp::Time latest_stamp(robot_pose.header.stamp, clock_type);
   if (rclcpp::Time(goals.goals.front().header.stamp,
       latest_stamp.get_clock_type()) > latest_stamp)
   {