From bf6108d328487667b686687c1ce912fd5acaef0c Mon Sep 17 00:00:00 2001 From: Robbin Baauw Date: Tue, 9 Nov 2021 14:14:07 +0100 Subject: [PATCH] fix(fake_perception): check for existence of map frame --- src/simulation/fake_perception/src/fake_perception.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/simulation/fake_perception/src/fake_perception.cpp b/src/simulation/fake_perception/src/fake_perception.cpp index 7862a695..1dba1769 100644 --- a/src/simulation/fake_perception/src/fake_perception.cpp +++ b/src/simulation/fake_perception/src/fake_perception.cpp @@ -4,7 +4,6 @@ using namespace std::chrono_literals; std::default_random_engine random_engine; - Perception::Perception() : Node("fake_perception"), buffer(get_clock()), transform_listener(buffer) { lidar_frame = declare_parameter("frames.lidar"); car_frame = declare_parameter("frames.car"); @@ -201,6 +200,14 @@ uint8_t Perception::get_noisy_type(uint8_t type, const ConfusionMatrix matrix) { } void Perception::generate_lidar_cone_data() { + // At the beginning, the right transforms will not have been published yet. + const auto tf_frames = buffer.getAllFrameNames(); + bool map_frame_exists = std::any_of(tf_frames.begin(), tf_frames.end(), [](const std::string &frame_name) { return frame_name == "map"; }); + if (!map_frame_exists) { + RCLCPP_INFO(get_logger(), "Transform from 'map' to '%s' not yet received", lidar_frame.c_str()); + return; + } + const auto map_to_lidar_transform = buffer.lookupTransform(lidar_frame, "map", rclcpp::Time(0), 1ns); // Start time of cone detection