From 47d74205fec6a50c581f85a4c63f927e8fce8129 Mon Sep 17 00:00:00 2001 From: Rafael Perez-Segui Date: Wed, 4 Dec 2024 17:01:40 +0100 Subject: [PATCH] Remove tf_timeout_threshold --- .../as2_platform_dji_psdk/as2_platform_dji_psdk.hpp | 1 - src/as2_platform_dji_psdk.cpp | 11 +---------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/include/as2_platform_dji_psdk/as2_platform_dji_psdk.hpp b/include/as2_platform_dji_psdk/as2_platform_dji_psdk.hpp index a81d98e..e0ff73b 100644 --- a/include/as2_platform_dji_psdk/as2_platform_dji_psdk.hpp +++ b/include/as2_platform_dji_psdk/as2_platform_dji_psdk.hpp @@ -90,7 +90,6 @@ class DJIMatricePSDKPlatform : public as2::AerialPlatform // Gimbal as2::tf::TfHandler tf_handler_; - std::chrono::nanoseconds tf_timeout_; bool enable_gimbal_; psdk_interfaces::msg::GimbalRotation gimbal_command_msg_; rclcpp::Time last_gimbal_command_time_; diff --git a/src/as2_platform_dji_psdk.cpp b/src/as2_platform_dji_psdk.cpp index 2ee533e..468cfef 100644 --- a/src/as2_platform_dji_psdk.cpp +++ b/src/as2_platform_dji_psdk.cpp @@ -111,15 +111,6 @@ void DJIMatricePSDKPlatform::configureSensors() sensor_odom_ptr_ = std::make_unique>("odom", this); RCLCPP_INFO(this->get_logger(), "DJIMatricePSDKPlatform odometry sensor configured"); - // Read tf_timeout_threshold - double tf_timeout_threshold; - this->declare_parameter("tf_timeout_threshold", 0.5); - this->get_parameter("tf_timeout_threshold", tf_timeout_threshold); - tf_timeout_ = std::chrono::duration_cast( - std::chrono::duration(tf_timeout_threshold)); - RCLCPP_INFO( - this->get_logger(), "DJIMatricePSDKPlatform tf_timeout_threshold: %f", tf_timeout_threshold); - // Initialize the camera streaming service this->declare_parameter("enable_camera", false); bool enable_camera; @@ -396,7 +387,7 @@ void DJIMatricePSDKPlatform::gimbalControlCallback( desired_earth_orientation = desired_base_link_orientation; std::string target_frame = "earth"; // Earth frame - if (!tf_handler_.tryConvert(desired_earth_orientation, target_frame, tf_timeout_)) { + if (!tf_handler_.tryConvert(desired_earth_orientation, target_frame)) { RCLCPP_ERROR( this->get_logger(), "Could not transform desired gimbal orientation to earth frame"); return;