From 1479b915376822b226efeb8097b738bb75457e89 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 12 Dec 2024 19:31:31 +0900 Subject: [PATCH] fix(surround_obstacle_checker): use tf2::TimePointZero Signed-off-by: satoshi-ota --- .../include/surround_obstacle_checker/node.hpp | 3 +-- planning/surround_obstacle_checker/src/node.cpp | 12 ++++-------- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index a720de68acbcd..9978e41e356d4 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -99,8 +99,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node std::optional getNearestObstacleByDynamicObject() const; std::optional getTransform( - const std::string & source, const std::string & target, const rclcpp::Time & stamp, - double duration_sec) const; + const std::string & source, const std::string & target) const; bool isStopRequired(const bool is_obstacle_found, const bool is_stopped); diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 4c7d0589f843b..22e0c78248de8 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -424,8 +424,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCl return std::nullopt; } - const auto transform_stamped = - getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); + const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id); if (!transform_stamped) { return std::nullopt; @@ -468,8 +467,7 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic { if (!object_ptr_ || !use_dynamic_object_) return std::nullopt; - const auto transform_stamped = - getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); + const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link"); if (!transform_stamped) { return std::nullopt; @@ -522,14 +520,12 @@ std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamic } std::optional SurroundObstacleCheckerNode::getTransform( - const std::string & source, const std::string & target, const rclcpp::Time & stamp, - double duration_sec) const + const std::string & source, const std::string & target) const { geometry_msgs::msg::TransformStamped transform_stamped; try { - transform_stamped = - tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec)); + transform_stamped = tf_buffer_.lookupTransform(source, target, tf2::TimePointZero); } catch (tf2::TransformException & ex) { return {}; }