From 95fca20ed62ed0773ca2941aeed08a0a9204ee91 Mon Sep 17 00:00:00 2001 From: kirohy Date: Thu, 11 Apr 2024 13:39:28 +0900 Subject: [PATCH] [hrpsys_ros_bridge] avoid too many TF_REPEATED_DATA Warnings --- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 6 ++++-- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index a37e6923b..d53199a5d 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -1114,12 +1114,13 @@ void HrpsysSeqStateROSBridge::updateImu(tf::Transform &base, bool is_base_valid, if (not_nan) { std::map::const_iterator its = sensor_info.begin(); while ( its != sensor_info.end() ) { - if ( (*its).second.type_name == "RateGyro" ) { + if ( (*its).second.type_name == "RateGyro" && base_time > last_updated_imu_tf_stamp ) { boost::mutex::scoped_lock lock(tf_mutex); tf::StampedTransform imu_floor_stamped_transform(inv, base_time, (*its).first, "imu_floor"); geometry_msgs::TransformStamped imu_floor_tf; tf::transformStampedTFToMsg(imu_floor_stamped_transform, imu_floor_tf); tf_transforms.push_back(imu_floor_tf); + last_updated_imu_tf_stamp = base_time; break; } ++its; @@ -1159,9 +1160,10 @@ void HrpsysSeqStateROSBridge::updateSensorTransform(const ros::Time &stamp) ++its; } } - if (!sensor_tf_buffer.empty()) { + if (!sensor_tf_buffer.empty() && stamp > last_updated_sensor_tf_stamp) { boost::mutex::scoped_lock lock(tf_mutex); tf_transforms.insert(tf_transforms.end(), sensor_tf_buffer.begin(), sensor_tf_buffer.end()); + last_updated_sensor_tf_stamp = stamp; } } diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 4068ca1a3..d471cd853 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -100,9 +100,11 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl void updateOdometry(const hrp::Vector3 &trans, const hrp::Matrix33 &R, const ros::Time &stamp); // imu relatives + ros::Time last_updated_imu_tf_stamp; void updateImu(tf::Transform &base, bool is_base_valid, const ros::Time &stamp); // sensor relatives + ros::Time last_updated_sensor_tf_stamp; void updateSensorTransform(const ros::Time &stamp); std::map sensor_transformations; boost::mutex sensor_transformation_mutex;