Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[hrpsys_ros_bridge] remove duplications of the imu TF and the sensor TFs #13

Merged
merged 1 commit into from
Jun 13, 2024
Merged

[hrpsys_ros_bridge] remove duplications of the imu TF and the sensor TFs #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1114,12 +1114,13 @@ void HrpsysSeqStateROSBridge::updateImu(tf::Transform &base, bool is_base_valid,
if (not_nan) {
std::map<std::string, SensorInfo>::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;
Expand Down Expand Up @@ -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;
}
}

Expand Down
2 changes: 2 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string, geometry_msgs::Transform> sensor_transformations;
boost::mutex sensor_transformation_mutex;
Expand Down