From 0fbd5cc81a929b8891a6dd722ddff2b90e7d1690 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 11 Apr 2022 09:59:47 +0200 Subject: [PATCH 1/5] (image_buffer) convert to tf2 --- ed_sensor_integration/CMakeLists.txt | 1 + .../include/ed/kinect/image_buffer.h | 11 ++++-- ed_sensor_integration/package.xml | 1 + .../src/kinect/image_buffer.cpp | 39 ++++++++++--------- 4 files changed, 30 insertions(+), 22 deletions(-) diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index 8483906c..dc078cd5 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS ed ed_sensor_integration_msgs geolib2 + geometry_msgs image_geometry sensor_msgs tf2 diff --git a/ed_sensor_integration/include/ed/kinect/image_buffer.h b/ed_sensor_integration/include/ed/kinect/image_buffer.h index ba905abb..92a3dc44 100644 --- a/ed_sensor_integration/include/ed/kinect/image_buffer.h +++ b/ed_sensor_integration/include/ed/kinect/image_buffer.h @@ -2,9 +2,13 @@ #define ED_SENSOR_INTEGRATION_IMAGE_BUFFER_H_ #include -#include + #include +#include + +#include + #include #include #include @@ -15,7 +19,7 @@ namespace rgbd class Client; } -namespace tf +namespace tf2_ros { class TransformListener; } @@ -66,7 +70,8 @@ class ImageBuffer std::unique_ptr rgbd_client_; - std::unique_ptr tf_listener_; + tf2_ros::Buffer tf_buffer_; + std::unique_ptr tf_listener_; /** * @brief Newer images should be pushed on the front. This will result in the front being the most recent and the back being the oldest diff --git a/ed_sensor_integration/package.xml b/ed_sensor_integration/package.xml index d256900b..b1f48eec 100644 --- a/ed_sensor_integration/package.xml +++ b/ed_sensor_integration/package.xml @@ -16,6 +16,7 @@ ed ed_sensor_integration_msgs geolib2 + geometry_msgs image_geometry sensor_msgs tf2 diff --git a/ed_sensor_integration/src/kinect/image_buffer.cpp b/ed_sensor_integration/src/kinect/image_buffer.cpp index 1b4d27e7..d0fe6ad2 100644 --- a/ed_sensor_integration/src/kinect/image_buffer.cpp +++ b/ed_sensor_integration/src/kinect/image_buffer.cpp @@ -1,13 +1,17 @@ #include "ed/kinect/image_buffer.h" +#include + +#include + #include #include -#include -#include + +#include // ---------------------------------------------------------------------------------------------------- -ImageBuffer::ImageBuffer() : rgbd_client_(nullptr), tf_listener_(nullptr), shutdown_(false) +ImageBuffer::ImageBuffer() : rgbd_client_(nullptr), tf_buffer_(), tf_listener_(nullptr), shutdown_(false) { } @@ -32,7 +36,7 @@ void ImageBuffer::initialize(const std::string& topic, const std::string& root_f rgbd_client_->initialize(topic); if (!tf_listener_) - tf_listener_ = std::make_unique(); + tf_listener_ = std::make_unique(tf_buffer_); worker_thread_ptr_ = std::make_unique(&ImageBuffer::workerThreadFunc, this, worker_thread_frequency); } @@ -70,8 +74,8 @@ bool ImageBuffer::waitForRecentImage(rgbd::ImageConstPtr& image, geo::Pose3D& se // - - - - - - - - - - - - - - - - - - // Wait until we have a tf - if (!tf_listener_->canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()))) // Get the TF when it is available now - if (!tf_listener_->waitForTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_end - ros::Time::now())) + if (!tf_buffer_.canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()))) // Get the TF when it is available now + if (!tf_buffer_.canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_end - ros::Time::now())) return false; // - - - - - - - - - - - - - - - - - - @@ -79,11 +83,10 @@ bool ImageBuffer::waitForRecentImage(rgbd::ImageConstPtr& image, geo::Pose3D& se try { - tf::StampedTransform t_sensor_pose; - tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose); - geo::convert(t_sensor_pose, sensor_pose); + geometry_msgs::TransformStamped t_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp())); + geo::convert(t_sensor_pose.transform, sensor_pose); } - catch(tf::TransformException& ex) + catch(tf2::TransformException& ex) { ROS_ERROR_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what()); return false; @@ -149,20 +152,18 @@ bool ImageBuffer::getMostRecentImageTF() rgbd::ImageConstPtr& rgbd_image = *it; try { - tf::StampedTransform t_sensor_pose; - tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose); - geo::convert(t_sensor_pose, sensor_pose); + geometry_msgs::TransformStamped t_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp())); + geo::convert(t_sensor_pose.transform, sensor_pose); } - catch (tf::ExtrapolationException& ex) + catch (tf2::ExtrapolationException& ex) { try { // Now we have to check if the error was an interpolation or extrapolation error (i.e., the image is too old or // to new, respectively). If it is too old, discard it. - tf::StampedTransform latest_sensor_pose; - tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0), latest_sensor_pose); + geometry_msgs::TransformStamped latest_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0)); // If image time stamp is older than latest transform, the image is too old and the tf data is not available anymore - if ( latest_sensor_pose.stamp_ > ros::Time(rgbd_image->getTimestamp()) ) + if ( latest_sensor_pose.header.stamp > ros::Time(rgbd_image->getTimestamp()) ) { ROS_DEBUG_STREAM_NAMED("image_buffer", "[IMAGE_BUFFER] Image too old to look-up tf. Deleting all images older than timestamp: " << std::fixed @@ -178,14 +179,14 @@ bool ImageBuffer::getMostRecentImageTF() continue; } } - catch (tf::TransformException& ex) + catch (tf2::TransformException& ex) { ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what()); ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what()); continue; } } - catch (tf::TransformException& ex) + catch (tf2::TransformException& ex) { ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what()); ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what()); From a1d052b3fe224e105784abb1e6c1af32bb65d371 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 11 Apr 2022 10:04:49 +0200 Subject: [PATCH 2/5] (laser) small cleanup of tf2 stuff --- ed_sensor_integration/CMakeLists.txt | 1 - ed_sensor_integration/package.xml | 1 - ed_sensor_integration/src/laser/laser_plugin.cpp | 4 +++- ed_sensor_integration/src/laser/laser_plugin.h | 6 +++++- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ed_sensor_integration/CMakeLists.txt b/ed_sensor_integration/CMakeLists.txt index dc078cd5..7362f2e5 100644 --- a/ed_sensor_integration/CMakeLists.txt +++ b/ed_sensor_integration/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs image_geometry sensor_msgs - tf2 tf2_ros tue_config tue_filesystem diff --git a/ed_sensor_integration/package.xml b/ed_sensor_integration/package.xml index b1f48eec..ae842d1f 100644 --- a/ed_sensor_integration/package.xml +++ b/ed_sensor_integration/package.xml @@ -19,7 +19,6 @@ geometry_msgs image_geometry sensor_msgs - tf2 tf2_ros tue_config tue_filesystem diff --git a/ed_sensor_integration/src/laser/laser_plugin.cpp b/ed_sensor_integration/src/laser/laser_plugin.cpp index aed11d15..7a28f595 100644 --- a/ed_sensor_integration/src/laser/laser_plugin.cpp +++ b/ed_sensor_integration/src/laser/laser_plugin.cpp @@ -12,12 +12,14 @@ #include #include +#include + #include #include #include -#include "tf2/transform_datatypes.h" +#include #include diff --git a/ed_sensor_integration/src/laser/laser_plugin.h b/ed_sensor_integration/src/laser/laser_plugin.h index dc44e972..83bbd933 100644 --- a/ed_sensor_integration/src/laser/laser_plugin.h +++ b/ed_sensor_integration/src/laser/laser_plugin.h @@ -11,13 +11,17 @@ #include #include #include -#include #include #include #include #include +namespace tf2_ros +{ +class TransformListener; +} + class LaserPlugin : public ed::Plugin { From 6a9af315cfaabb4d232fbfc2667383e78a38f345 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 13 Apr 2022 15:00:28 +0200 Subject: [PATCH 3/5] add check for within bounds --- ed_sensor_integration/include/ed/kinect/fitter.h | 4 ++-- ed_sensor_integration/src/kinect/fitter.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ed_sensor_integration/include/ed/kinect/fitter.h b/ed_sensor_integration/include/ed/kinect/fitter.h index e93cbac9..f53f0785 100644 --- a/ed_sensor_integration/include/ed/kinect/fitter.h +++ b/ed_sensor_integration/include/ed/kinect/fitter.h @@ -233,11 +233,11 @@ class Fitter /** * @brief checkExpectedBeamThroughEntity checks if the expected center beam passes through the entity. If - * not: something went wrong + * not: throw a fitter error * @param model_ranges * @param entity * @param sensor_pose_xya - * @param expected_center_beam + * @param expected_center_beam expected index of the beam through the center of the object. range: any int. indices outside bounds will also throw an error. */ void checkExpectedBeamThroughEntity(const std::vector &model_ranges, ed::EntityConstPtr entity, const geo::Pose3D &sensor_pose_xya, const int expected_center_beam) const; diff --git a/ed_sensor_integration/src/kinect/fitter.cpp b/ed_sensor_integration/src/kinect/fitter.cpp index 0f5dadf5..04d30228 100644 --- a/ed_sensor_integration/src/kinect/fitter.cpp +++ b/ed_sensor_integration/src/kinect/fitter.cpp @@ -463,7 +463,7 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector& model_ran std::vector expected_identifiers(nr_data_points_, 0); renderEntity(entity, sensor_pose_xya, 1, expected_ranges, expected_identifiers); - if (expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model + if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_ || expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model throw FitterError("Expected beam does not go through entity"); } From 7b82885ee9d04ef93fcac1f2769129d2030ffbd1 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 13 Apr 2022 15:05:48 +0200 Subject: [PATCH 4/5] separate range check --- ed_sensor_integration/src/kinect/fitter.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ed_sensor_integration/src/kinect/fitter.cpp b/ed_sensor_integration/src/kinect/fitter.cpp index 04d30228..3aa6892d 100644 --- a/ed_sensor_integration/src/kinect/fitter.cpp +++ b/ed_sensor_integration/src/kinect/fitter.cpp @@ -462,8 +462,9 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector& model_ran expected_ranges = model_ranges; std::vector expected_identifiers(nr_data_points_, 0); renderEntity(entity, sensor_pose_xya, 1, expected_ranges, expected_identifiers); - - if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_ || expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model + if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_) + throw FitterError("Expected beam outside of measurement range(" + std::to_string(nr_data_points_) + "), index: " + std::to_string(expected_center_beam)); + if (expected_identifiers[expected_center_beam] != 1) // expected center beam MUST contain the rendered model throw FitterError("Expected beam does not go through entity"); } From 7af2446bcb431e46c8ccb59c8e3c185bf44c5418 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Apr 2022 17:15:29 +0200 Subject: [PATCH 5/5] [TEMP] Print when optimum is invalid --- ed_sensor_integration/src/kinect/fitter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ed_sensor_integration/src/kinect/fitter.cpp b/ed_sensor_integration/src/kinect/fitter.cpp index 3aa6892d..d27ed327 100644 --- a/ed_sensor_integration/src/kinect/fitter.cpp +++ b/ed_sensor_integration/src/kinect/fitter.cpp @@ -377,6 +377,8 @@ std::unique_ptr Fitter::findOptimum(const EstimationInputData& input } if (valid_optimum) return current_optimum; + + ROS_ERROR_NAMED("fitter", "optimum is invalid"); std::unique_ptr invalid_optimum(new OptimalFit); return invalid_optimum; }