Skip to content

Commit 1479b91

Browse files
committed
fix(surround_obstacle_checker): use tf2::TimePointZero
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent f8fed79 commit 1479b91

File tree

2 files changed

+5
-10
lines changed
  • planning/surround_obstacle_checker

2 files changed

+5
-10
lines changed

planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -99,8 +99,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
9999
std::optional<Obstacle> getNearestObstacleByDynamicObject() const;
100100

101101
std::optional<geometry_msgs::msg::TransformStamped> getTransform(
102-
const std::string & source, const std::string & target, const rclcpp::Time & stamp,
103-
double duration_sec) const;
102+
const std::string & source, const std::string & target) const;
104103

105104
bool isStopRequired(const bool is_obstacle_found, const bool is_stopped);
106105

planning/surround_obstacle_checker/src/node.cpp

+4-8
Original file line numberDiff line numberDiff line change
@@ -424,8 +424,7 @@ std::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByPointCl
424424
return std::nullopt;
425425
}
426426

427-
const auto transform_stamped =
428-
getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5);
427+
const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id);
429428

430429
if (!transform_stamped) {
431430
return std::nullopt;
@@ -468,8 +467,7 @@ std::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByDynamic
468467
{
469468
if (!object_ptr_ || !use_dynamic_object_) return std::nullopt;
470469

471-
const auto transform_stamped =
472-
getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5);
470+
const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link");
473471

474472
if (!transform_stamped) {
475473
return std::nullopt;
@@ -522,14 +520,12 @@ std::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByDynamic
522520
}
523521

524522
std::optional<geometry_msgs::msg::TransformStamped> SurroundObstacleCheckerNode::getTransform(
525-
const std::string & source, const std::string & target, const rclcpp::Time & stamp,
526-
double duration_sec) const
523+
const std::string & source, const std::string & target) const
527524
{
528525
geometry_msgs::msg::TransformStamped transform_stamped;
529526

530527
try {
531-
transform_stamped =
532-
tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec));
528+
transform_stamped = tf_buffer_.lookupTransform(source, target, tf2::TimePointZero);
533529
} catch (tf2::TransformException & ex) {
534530
return {};
535531
}

0 commit comments

Comments
 (0)