From 829578ab760047859b4f22f857d81b72632b98c9 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 14:05:50 +0000 Subject: [PATCH 01/14] remove unused header Signed-off-by: Ignacio Vizzo --- nav2_util/include/nav2_util/robot_utils.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 3cc8f87267..422ed38564 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -26,7 +26,6 @@ #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "rclcpp/rclcpp.hpp" namespace nav2_util { From 1cbdb40de2df59f3052c603a24d684dab92d7b9c Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 14:13:40 +0000 Subject: [PATCH 02/14] First step. Return an optional value instead of user provided output. Signed-off-by: Ignacio Vizzo --- nav2_util/include/nav2_util/robot_utils.hpp | 18 ++++++------- nav2_util/src/robot_utils.cpp | 28 +++++++++++---------- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 422ed38564..13fbfcb9a2 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -17,8 +17,9 @@ #ifndef NAV2_UTIL__ROBOT_UTILS_HPP_ #define NAV2_UTIL__ROBOT_UTILS_HPP_ -#include #include +#include +#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -65,15 +66,13 @@ bool transformPoseInTargetFrame( * @param target_frame_id Target frame ID to convert to * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation - * @param tf_transform Output source->target transform - * @return True if got correct transform, otherwise false + * @return tf_transform Output source->target transform */ -bool getTransform( +std::optional getTransform( const std::string & source_frame_id, const std::string & target_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer, - tf2::Transform & tf2_transform); + const std::shared_ptr tf_buffer); /** * @brief Obtains a transform from source_frame_id at source_time -> @@ -86,17 +85,16 @@ bool getTransform( * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation * @param tf_transform Output source->target transform - * @return True if got correct transform, otherwise false + * @return tf_transform Output source->target transform */ -bool getTransform( +std::optional getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & target_frame_id, const rclcpp::Time & target_time, const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer, - tf2::Transform & tf2_transform); + const std::shared_ptr tf_buffer); /** * @brief Validates a twist message contains no nans or infs diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6933b71694..50b0526e35 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -14,6 +14,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -77,21 +78,21 @@ bool transformPoseInTargetFrame( return false; } -bool getTransform( +std::optional getTransform( const std::string & source_frame_id, const std::string & target_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer, - tf2::Transform & tf2_transform) + const std::shared_ptr tf_buffer) { - geometry_msgs::msg::TransformStamped transform; + tf2::Transform tf2_transform; tf2_transform.setIdentity(); // initialize by identical transform if (source_frame_id == target_frame_id) { // We are already in required frame - return true; + return tf2_transform; } + geometry_msgs::msg::TransformStamped transform; try { // Obtaining the transform to get data from source to target frame transform = tf_buffer->lookupTransform( @@ -102,27 +103,28 @@ bool getTransform( rclcpp::get_logger("getTransform"), "Failed to get \"%s\"->\"%s\" frame transform: %s", source_frame_id.c_str(), target_frame_id.c_str(), e.what()); - return false; + return std::nullopt; } // Convert TransformStamped to TF2 transform tf2::fromMsg(transform.transform, tf2_transform); - return true; + return tf2_transform; } -bool getTransform( +std::optional getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & target_frame_id, const rclcpp::Time & target_time, const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer, - tf2::Transform & tf2_transform) + const std::shared_ptr tf_buffer) { - geometry_msgs::msg::TransformStamped transform; + tf2::Transform tf2_transform; tf2_transform.setIdentity(); // initialize by identical transform + geometry_msgs::msg::TransformStamped transform; + try { // Obtaining the transform to get data from source to target frame. // This also considers the time shift between source and target. @@ -135,12 +137,12 @@ bool getTransform( rclcpp::get_logger("getTransform"), "Failed to get \"%s\"->\"%s\" frame transform: %s", source_frame_id.c_str(), target_frame_id.c_str(), ex.what()); - return false; + return std::nullopt; } // Convert TransformStamped to TF2 transform tf2::fromMsg(transform.transform, tf2_transform); - return true; + return tf2_transform; } bool validateTwist(const geometry_msgs::msg::Twist & msg) From a65880b7bc27725cb577ab79681190601b13887e Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 14:36:14 +0000 Subject: [PATCH 03/14] Second step, update the consumers of this utility function Signed-off-by: Ignacio Vizzo --- nav2_collision_monitor/src/pointcloud.cpp | 44 +++++++++----------- nav2_collision_monitor/src/polygon.cpp | 33 +++++++-------- nav2_collision_monitor/src/range.cpp | 49 +++++++++++------------ nav2_collision_monitor/src/scan.cpp | 46 ++++++++++----------- 4 files changed, 78 insertions(+), 94 deletions(-) diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index fb3fca201d..9ef9ad2aae 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -15,6 +15,7 @@ #include "nav2_collision_monitor/pointcloud.hpp" #include +#include #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -78,30 +79,23 @@ bool PointCloud::getData( return false; } - tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } + const auto tf_transform = [&]() -> std::optional { + // Obtaining the transform to get data from source frame and time where it was received to the + // base frame and current time + if (base_shift_correction_) { + return nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_); + } + + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + return nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_); + }(); + + if (!tf_transform.has_value()) {return false;} sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*data_, "y"); @@ -111,7 +105,7 @@ bool PointCloud::getData( for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; // Refill data array if (p_v3_b.z() >= min_height_ && p_v3_b.z() <= max_height_) { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 77fc1f304c..1c73bc3063 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -15,6 +15,7 @@ #include "nav2_collision_monitor/polygon.hpp" #include +#include #include #include "geometry_msgs/msg/point.hpp" @@ -204,21 +205,19 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::size_t new_size = polygon_.polygon.points.size(); // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; - if ( - !nav2_util::getTransform( - polygon_.header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } + std::optional tf_transform = + nav2_util::getTransform( + polygon_.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_); + + if (!tf_transform.has_value()) {return;} // Correct main poly_ vertices poly_.resize(new_size); for (std::size_t i = 0; i < new_size; i++) { // Transform point coordinates from PolygonStamped frame -> to base frame tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; // Fill poly_ array poly_[i] = {p_v3_b.x(), p_v3_b.y()}; @@ -456,21 +455,19 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m } // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; - if ( - !nav2_util::getTransform( - msg->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; - } + std::optional tf_transform = + nav2_util::getTransform( + msg->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_); + + if (!tf_transform.has_value()) {return;} // Set main poly_ vertices first time poly_.resize(new_size); for (std::size_t i = 0; i < new_size; i++) { // Transform point coordinates from PolygonStamped frame -> to base frame tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; // Fill poly_ array poly_[i] = {p_v3_b.x(), p_v3_b.y()}; diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 45540a9c4d..c665af2b8d 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -87,30 +88,26 @@ bool Range::getData( return false; } - tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } + const auto tf_transform = [&]() -> std::optional { + // Obtaining the transform to get data from source frame and time where it was received to the + // base frame and current time + if (base_shift_correction_) { + return nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, base_frame_id_, + curr_time, global_frame_id_, transform_tolerance_, + tf_buffer_); + } + + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + return nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, transform_tolerance_, + tf_buffer_); + }(); + + if (!tf_transform.has_value()) {return false;} + // Calculate poses and refill data array float angle; @@ -124,7 +121,7 @@ bool Range::getData( data_->range * std::cos(angle), data_->range * std::sin(angle), 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); @@ -138,7 +135,7 @@ bool Range::getData( data_->range * std::cos(angle), data_->range * std::sin(angle), 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 45f1e62d70..02e726e704 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "nav2_util/robot_utils.hpp" @@ -77,30 +78,25 @@ bool Scan::getData( return false; } - tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } + const auto tf_transform = [&]() -> std::optional { + // Obtaining the transform to get data from source frame and time where it was received to the + // base frame and current time + if (base_shift_correction_) { + return nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, base_frame_id_, + curr_time, global_frame_id_, transform_tolerance_, + tf_buffer_); + } + + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + return nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, transform_tolerance_, + tf_buffer_); + }(); + + if (!tf_transform.has_value()) {return false;} // Calculate poses and refill data array float angle = data_->angle_min; @@ -111,7 +107,7 @@ bool Scan::getData( data_->ranges[i] * std::cos(angle), data_->ranges[i] * std::sin(angle), 0.0); - tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); From 250311dc2f731c2f24cc153723eef0a8fcb734f0 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 14:42:56 +0000 Subject: [PATCH 04/14] Third step: swap source/target to make it consistent with tf lookups Otherwise is very confusing for any user who is user to the tf2::Buffer::lookupTransform method Signed-off-by: Ignacio Vizzo --- nav2_collision_monitor/src/pointcloud.cpp | 8 +++++--- nav2_collision_monitor/src/polygon.cpp | 18 ++++++++++-------- nav2_collision_monitor/src/range.cpp | 6 +++--- nav2_collision_monitor/src/scan.cpp | 6 +++--- nav2_util/include/nav2_util/robot_utils.hpp | 8 ++++---- nav2_util/src/robot_utils.cpp | 6 +++--- 6 files changed, 28 insertions(+), 24 deletions(-) diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 9ef9ad2aae..e608561509 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -84,15 +84,17 @@ bool PointCloud::getData( // base frame and current time if (base_shift_correction_) { return nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_); + base_frame_id_, curr_time, data_->header.frame_id, + data_->header.stamp, global_frame_id_, transform_tolerance_, + tf_buffer_); } // Obtaining the transform to get data from source frame to base frame without time shift // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_); + base_frame_id_, data_->header.frame_id, transform_tolerance_, + tf_buffer_); }(); if (!tf_transform.has_value()) {return false;} diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 1c73bc3063..924cf271d9 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -205,10 +205,11 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::size_t new_size = polygon_.polygon.points.size(); // Get the transform from PolygonStamped frame to base_frame_id_ - std::optional tf_transform = - nav2_util::getTransform( - polygon_.header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_); + std::optional tf_transform = nav2_util::getTransform( + base_frame_id_, + polygon_.header.frame_id, + transform_tolerance_, + tf_buffer_); if (!tf_transform.has_value()) {return;} @@ -455,10 +456,11 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m } // Get the transform from PolygonStamped frame to base_frame_id_ - std::optional tf_transform = - nav2_util::getTransform( - msg->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_); + std::optional tf_transform = nav2_util::getTransform( + base_frame_id_, + msg->header.frame_id, + transform_tolerance_, + tf_buffer_); if (!tf_transform.has_value()) {return;} diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index c665af2b8d..3fee69f6f5 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -93,8 +93,8 @@ bool Range::getData( // base frame and current time if (base_shift_correction_) { return nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, base_frame_id_, - curr_time, global_frame_id_, transform_tolerance_, + base_frame_id_, curr_time, data_->header.frame_id, + data_->header.stamp, global_frame_id_, transform_tolerance_, tf_buffer_); } @@ -102,7 +102,7 @@ bool Range::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, transform_tolerance_, + base_frame_id_, data_->header.frame_id, transform_tolerance_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 02e726e704..dda950d40d 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -83,8 +83,8 @@ bool Scan::getData( // base frame and current time if (base_shift_correction_) { return nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, base_frame_id_, - curr_time, global_frame_id_, transform_tolerance_, + base_frame_id_, curr_time, data_->header.frame_id, + data_->header.stamp, global_frame_id_, transform_tolerance_, tf_buffer_); } @@ -92,7 +92,7 @@ bool Scan::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, transform_tolerance_, + base_frame_id_, data_->header.frame_id, transform_tolerance_, tf_buffer_); }(); diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 13fbfcb9a2..61bda4c8b6 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -62,15 +62,15 @@ bool transformPoseInTargetFrame( /** * @brief Obtains a transform from source_frame_id -> to target_frame_id - * @param source_frame_id Source frame ID to convert from * @param target_frame_id Target frame ID to convert to + * @param source_frame_id Source frame ID to convert from * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation * @return tf_transform Output source->target transform */ std::optional getTransform( - const std::string & source_frame_id, const std::string & target_frame_id, + const std::string & source_frame_id, const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer); @@ -88,10 +88,10 @@ std::optional getTransform( * @return tf_transform Output source->target transform */ std::optional getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, const std::string & target_frame_id, const rclcpp::Time & target_time, + const std::string & source_frame_id, + const rclcpp::Time & source_time, const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer); diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 50b0526e35..cc4f242768 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -79,8 +79,8 @@ bool transformPoseInTargetFrame( } std::optional getTransform( - const std::string & source_frame_id, const std::string & target_frame_id, + const std::string & source_frame_id, const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) { @@ -112,10 +112,10 @@ std::optional getTransform( } std::optional getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, const std::string & target_frame_id, const rclcpp::Time & target_time, + const std::string & source_frame_id, + const rclcpp::Time & source_time, const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) From b1bc26f75ef028a90a0bdf38308fb4f330dd9a8b Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 14:52:26 +0000 Subject: [PATCH 05/14] transform tolerance -> transform timeout I find this "transform tolerance" to specify something else. Once again, I believe that sticking to tf2 name conventions is better and improve readability for users already used to the tf2 API Signed-off-by: Ignacio Vizzo --- .../include/nav2_collision_monitor/circle.hpp | 4 +-- .../collision_detector_node.hpp | 8 ++--- .../collision_monitor_node.hpp | 8 ++--- .../nav2_collision_monitor/pointcloud.hpp | 4 +-- .../nav2_collision_monitor/polygon.hpp | 8 ++--- .../include/nav2_collision_monitor/range.hpp | 4 +-- .../include/nav2_collision_monitor/scan.hpp | 4 +-- .../include/nav2_collision_monitor/source.hpp | 8 ++--- .../velocity_polygon.hpp | 2 +- .../params/collision_detector_params.yaml | 2 +- .../params/collision_monitor_params.yaml | 2 +- nav2_collision_monitor/src/circle.cpp | 4 +-- .../src/collision_detector_node.cpp | 28 +++++++-------- .../src/collision_monitor_node.cpp | 28 +++++++-------- nav2_collision_monitor/src/pointcloud.cpp | 8 ++--- nav2_collision_monitor/src/polygon.cpp | 10 +++--- nav2_collision_monitor/src/range.cpp | 8 ++--- nav2_collision_monitor/src/scan.cpp | 8 ++--- nav2_collision_monitor/src/source.cpp | 4 +-- .../src/velocity_polygon.cpp | 4 +-- .../test/collision_detector_node_test.cpp | 6 ++-- .../test/collision_monitor_node_test.cpp | 6 ++-- nav2_collision_monitor/test/polygons_test.cpp | 34 +++++++++---------- nav2_collision_monitor/test/sources_test.cpp | 20 +++++------ .../test/velocity_polygons_test.cpp | 8 ++--- nav2_util/include/nav2_util/robot_utils.hpp | 8 ++--- nav2_util/src/robot_utils.cpp | 8 ++--- 27 files changed, 123 insertions(+), 123 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index c440032701..baac1dbfea 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -38,14 +38,14 @@ class Circle : public Polygon * @param polygon_name Name of circle * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing */ Circle( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance); + const tf2::Duration & transform_timeout); /** * @brief Circle class destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index fae479d650..0eaee357f6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -100,18 +100,18 @@ class CollisionDetector : public nav2_util::LifecycleNode /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @return True if all polygons were configured successfully or false in failure case */ bool configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_tolerance); + const tf2::Duration & transform_timeout); /** * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get * source->base time interpolated transform. - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -120,7 +120,7 @@ class CollisionDetector : public nav2_util::LifecycleNode bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 998dacb519..bf744fad01 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -124,18 +124,18 @@ class CollisionMonitor : public nav2_util::LifecycleNode /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @return True if all polygons were configured successfully or false in failure case */ bool configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_tolerance); + const tf2::Duration & transform_timeout); /** * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get * source->base time interpolated transform. - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -144,7 +144,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index bfe8d09ed3..d248ff2f34 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -39,7 +39,7 @@ class PointCloud : public Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -50,7 +50,7 @@ class PointCloud : public Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 5e342cdb7f..45d1fc799d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -47,14 +47,14 @@ class Polygon * @param polygon_name Name of polygon * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID - * @param transform_tolerance Transform tolerance + * @param transform_timeout how long to block before failing */ Polygon( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance); + const tf2::Duration & transform_timeout); /** * @brief Polygon destructor */ @@ -256,8 +256,8 @@ class Polygon std::shared_ptr tf_buffer_; /// @brief Base frame ID std::string base_frame_id_; - /// @brief Transform tolerance - tf2::Duration transform_tolerance_; + /// @brief Transform timeout on how long to block before failing + tf2::Duration transform_timeout_; // Visualization /// @brief Whether to publish the polygon diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 1dc15195f6..6de75411f9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -39,7 +39,7 @@ class Range : public Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -50,7 +50,7 @@ class Range : public Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index efaf82e0e3..4aeea01f0c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -39,7 +39,7 @@ class Scan : public Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -50,7 +50,7 @@ class Scan : public Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 865a3c023b..499e86d3c5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -44,7 +44,7 @@ class Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -55,7 +55,7 @@ class Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** @@ -143,8 +143,8 @@ class Source std::string base_frame_id_; /// @brief Global frame ID for correct transform calculation std::string global_frame_id_; - /// @brief Transform tolerance - tf2::Duration transform_tolerance_; + /// @brief Transform timeout + tf2::Duration transform_timeout_; /// @brief Maximum time interval in which data is considered valid rclcpp::Duration source_timeout_; /// @brief Whether to correct source data towards to base frame movement, diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index e4c65bebad..6aded24c2c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -45,7 +45,7 @@ class VelocityPolygon : public Polygon VelocityPolygon( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance); + const tf2::Duration & transform_timeout); /** * @brief VelocityPolygon destructor */ diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index eb3ee0a873..5cbfec16a9 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -3,7 +3,7 @@ collision_detector: frequency: 10.0 base_frame_id: "base_footprint" odom_frame_id: "odom" - transform_tolerance: 0.5 + transform_timeout: 0.5 source_timeout: 5.0 base_shift_correction: True polygons: ["PolygonFront"] diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index d6b3cd25fd..2ccc9b1951 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -5,7 +5,7 @@ collision_monitor: cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "cmd_vel" state_topic: "collision_monitor_state" - transform_tolerance: 0.5 + transform_timeout: 0.5 source_timeout: 5.0 base_shift_correction: True stop_pub_timeout: 2.0 diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index fa0fc33a4f..169b578b0c 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -28,8 +28,8 @@ Circle::Circle( const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) -: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + const tf2::Duration & transform_timeout) +: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_timeout) { RCLCPP_INFO(logger_, "[%s]: Creating Circle", polygon_name_.c_str()); } diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4f485520c7..ab054ac208 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -141,7 +141,7 @@ CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) bool CollisionDetector::getParameters() { std::string base_frame_id, odom_frame_id; - tf2::Duration transform_tolerance; + tf2::Duration transform_timeout; rclcpp::Duration source_timeout(2.0, 0.0); auto node = shared_from_this(); @@ -156,9 +156,9 @@ bool CollisionDetector::getParameters() node, "odom_frame_id", rclcpp::ParameterValue("odom")); odom_frame_id = get_parameter("odom_frame_id").as_string(); nav2_util::declare_parameter_if_not_declared( - node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - transform_tolerance = - tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + node, "transform_timeout", rclcpp::ParameterValue(0.1)); + transform_timeout = + tf2::durationFromSec(get_parameter("transform_timeout").as_double()); nav2_util::declare_parameter_if_not_declared( node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = @@ -168,12 +168,12 @@ bool CollisionDetector::getParameters() const bool base_shift_correction = get_parameter("base_shift_correction").as_bool(); - if (!configurePolygons(base_frame_id, transform_tolerance)) { + if (!configurePolygons(base_frame_id, transform_timeout)) { return false; } if (!configureSources( - base_frame_id, odom_frame_id, transform_tolerance, source_timeout, + base_frame_id, odom_frame_id, transform_timeout, source_timeout, base_shift_correction)) { return false; @@ -184,7 +184,7 @@ bool CollisionDetector::getParameters() bool CollisionDetector::configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) + const tf2::Duration & transform_timeout) { try { auto node = shared_from_this(); @@ -202,15 +202,15 @@ bool CollisionDetector::configurePolygons( if (polygon_type == "polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); } else if (polygon_type == "circle") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); } else if (polygon_type == "velocity_polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -246,7 +246,7 @@ bool CollisionDetector::configurePolygons( bool CollisionDetector::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction) { @@ -266,7 +266,7 @@ bool CollisionDetector::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout, base_shift_correction); + transform_timeout, source_timeout, base_shift_correction); s->configure(); @@ -274,7 +274,7 @@ bool CollisionDetector::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout, base_shift_correction); + transform_timeout, source_timeout, base_shift_correction); p->configure(); @@ -282,7 +282,7 @@ bool CollisionDetector::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout, base_shift_correction); + transform_timeout, source_timeout, base_shift_correction); r->configure(); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 94c116445a..69382e1ee3 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -231,7 +231,7 @@ bool CollisionMonitor::getParameters( std::string & state_topic) { std::string base_frame_id, odom_frame_id; - tf2::Duration transform_tolerance; + tf2::Duration transform_timeout; rclcpp::Duration source_timeout(2.0, 0.0); auto node = shared_from_this(); @@ -253,9 +253,9 @@ bool CollisionMonitor::getParameters( node, "odom_frame_id", rclcpp::ParameterValue("odom")); odom_frame_id = get_parameter("odom_frame_id").as_string(); nav2_util::declare_parameter_if_not_declared( - node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - transform_tolerance = - tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + node, "transform_timeout", rclcpp::ParameterValue(0.1)); + transform_timeout = + tf2::durationFromSec(get_parameter("transform_timeout").as_double()); nav2_util::declare_parameter_if_not_declared( node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = @@ -270,13 +270,13 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_tolerance)) { + if (!configurePolygons(base_frame_id, transform_timeout)) { return false; } if ( !configureSources( - base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) + base_frame_id, odom_frame_id, transform_timeout, source_timeout, base_shift_correction)) { return false; } @@ -286,7 +286,7 @@ bool CollisionMonitor::getParameters( bool CollisionMonitor::configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) + const tf2::Duration & transform_timeout) { try { auto node = shared_from_this(); @@ -304,15 +304,15 @@ bool CollisionMonitor::configurePolygons( if (polygon_type == "polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); } else if (polygon_type == "circle") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); } else if (polygon_type == "velocity_polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -337,7 +337,7 @@ bool CollisionMonitor::configurePolygons( bool CollisionMonitor::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction) { @@ -357,7 +357,7 @@ bool CollisionMonitor::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout, base_shift_correction); + transform_timeout, source_timeout, base_shift_correction); s->configure(); @@ -365,7 +365,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout, base_shift_correction); + transform_timeout, source_timeout, base_shift_correction); p->configure(); @@ -373,7 +373,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout, base_shift_correction); + transform_timeout, source_timeout, base_shift_correction); r->configure(); diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index e608561509..2a285e1aba 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -31,12 +31,12 @@ PointCloud::PointCloud( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout, base_shift_correction), + transform_timeout, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str()); @@ -85,7 +85,7 @@ bool PointCloud::getData( if (base_shift_correction_) { return nav2_util::getTransform( base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_tolerance_, + data_->header.stamp, global_frame_id_, transform_timeout_, tf_buffer_); } @@ -93,7 +93,7 @@ bool PointCloud::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_tolerance_, + base_frame_id_, data_->header.frame_id, transform_timeout_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 924cf271d9..067606048b 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -35,11 +35,11 @@ Polygon::Polygon( const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) + const tf2::Duration & transform_timeout) : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), - base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) + base_frame_id_(base_frame_id), transform_timeout_(transform_timeout) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); } @@ -84,7 +84,7 @@ bool Polygon::configure() polygon_name_.c_str(), footprint_topic.c_str()); footprint_sub_ = std::make_unique( node, footprint_topic, *tf_buffer_, - base_frame_id_, tf2::durationToSec(transform_tolerance_)); + base_frame_id_, tf2::durationToSec(transform_timeout_)); } if (visualize_) { @@ -208,7 +208,7 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::optional tf_transform = nav2_util::getTransform( base_frame_id_, polygon_.header.frame_id, - transform_tolerance_, + transform_timeout_, tf_buffer_); if (!tf_transform.has_value()) {return;} @@ -459,7 +459,7 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m std::optional tf_transform = nav2_util::getTransform( base_frame_id_, msg->header.frame_id, - transform_tolerance_, + transform_timeout_, tf_buffer_); if (!tf_transform.has_value()) {return;} diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 3fee69f6f5..9a58812e24 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -31,12 +31,12 @@ Range::Range( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout, base_shift_correction), + transform_timeout, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); @@ -94,7 +94,7 @@ bool Range::getData( if (base_shift_correction_) { return nav2_util::getTransform( base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_tolerance_, + data_->header.stamp, global_frame_id_, transform_timeout_, tf_buffer_); } @@ -102,7 +102,7 @@ bool Range::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_tolerance_, + base_frame_id_, data_->header.frame_id, transform_timeout_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index dda950d40d..edd8c941a5 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -29,12 +29,12 @@ Scan::Scan( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout, base_shift_correction), + transform_timeout, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str()); @@ -84,7 +84,7 @@ bool Scan::getData( if (base_shift_correction_) { return nav2_util::getTransform( base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_tolerance_, + data_->header.stamp, global_frame_id_, transform_timeout_, tf_buffer_); } @@ -92,7 +92,7 @@ bool Scan::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_tolerance_, + base_frame_id_, data_->header.frame_id, transform_timeout_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index a27876f05b..f14fc17bb9 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -29,12 +29,12 @@ Source::Source( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : node_(node), source_name_(source_name), tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), global_frame_id_(global_frame_id), - transform_tolerance_(transform_tolerance), source_timeout_(source_timeout), + transform_timeout_(transform_timeout), source_timeout_(source_timeout), base_shift_correction_(base_shift_correction) { } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 84779a1a2d..da8dded7e3 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -22,8 +22,8 @@ namespace nav2_collision_monitor VelocityPolygon::VelocityPolygon( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) -: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + const tf2::Duration & transform_timeout) +: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_timeout) { RCLCPP_INFO(logger_, "[%s]: Creating VelocityPolygon", polygon_name_.c_str()); } diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index e36ef6e583..4025244c1a 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -53,7 +53,7 @@ static const char STATE_TOPIC[]{"collision_detector_state"}; static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; static const double SIMULATION_TIME_STEP{0.01}; -static const double TRANSFORM_TOLERANCE{0.5}; +static const double TRANSFORM_TIMEOUT{0.5}; static const double SOURCE_TIMEOUT{5.0}; static const double FREQUENCY{10.0}; @@ -246,9 +246,9 @@ void Tester::setCommonParameters() rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); cd_->declare_parameter( - "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); + "transform_timeout", rclcpp::ParameterValue(TRANSFORM_TIMEOUT)); cd_->set_parameter( - rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); + rclcpp::Parameter("transform_timeout", TRANSFORM_TIMEOUT)); cd_->declare_parameter( "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); cd_->set_parameter( diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 34690a6fd1..1e9adabcad 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -60,7 +60,7 @@ static const double LINEAR_LIMIT{0.4}; static const double ANGULAR_LIMIT{0.09}; static const double TIME_BEFORE_COLLISION{1.0}; static const double SIMULATION_TIME_STEP{0.01}; -static const double TRANSFORM_TOLERANCE{0.5}; +static const double TRANSFORM_TIMEOUT{0.5}; static const double SOURCE_TIMEOUT{5.0}; static const double STOP_PUB_TIMEOUT{0.1}; @@ -286,9 +286,9 @@ void Tester::setCommonParameters() rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); cm_->declare_parameter( - "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); + "transform_timeout", rclcpp::ParameterValue(TRANSFORM_TIMEOUT)); cm_->set_parameter( - rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); + rclcpp::Parameter("transform_timeout", TRANSFORM_TIMEOUT)); cm_->declare_parameter( "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); cm_->set_parameter( diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 849b627868..e0221881e6 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -70,7 +70,7 @@ static const double LINEAR_LIMIT{0.4}; static const double ANGULAR_LIMIT{0.09}; static const double TIME_BEFORE_COLLISION{1.0}; static const double SIMULATION_TIME_STEP{0.01}; -static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; +static const tf2::Duration TRANSFORM_TIMEOUT{tf2::durationFromSec(0.1)}; class TestNode : public nav2_util::LifecycleNode { @@ -173,9 +173,9 @@ class PolygonWrapper : public nav2_collision_monitor::Polygon const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) + const tf2::Duration & transform_timeout) : nav2_collision_monitor::Polygon( - node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + node, polygon_name, tf_buffer, base_frame_id, transform_timeout) { } @@ -198,9 +198,9 @@ class CircleWrapper : public nav2_collision_monitor::Circle const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) + const tf2::Duration & transform_timeout) : nav2_collision_monitor::Circle( - node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + node, polygon_name, tf_buffer, base_frame_id, transform_timeout) { } @@ -377,7 +377,7 @@ void Tester::createPolygon(const std::string & action_type, const bool is_static polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_TRUE(polygon_->configure()); polygon_->activate(); } @@ -389,7 +389,7 @@ void Tester::createCircle(const std::string & action_type) circle_ = std::make_shared( test_node_, CIRCLE_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_TRUE(circle_->configure()); circle_->activate(); } @@ -535,7 +535,7 @@ TEST_F(Tester, testPolygonUndeclaredActionType) // "action_type" parameter is not initialized polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(polygon_->configure()); // Check that "action_type" parameter is not set after configuring ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "action_type")); @@ -550,7 +550,7 @@ TEST_F(Tester, testPolygonUndeclaredPoints) rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(polygon_->configure()); // Check that "points" and "polygon_sub_topic" parameters are not set after configuring ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); @@ -564,7 +564,7 @@ TEST_F(Tester, testPolygonIncorrectActionType) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(polygon_->configure()); } @@ -580,7 +580,7 @@ TEST_F(Tester, testPolygonIncorrectPoints1) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(polygon_->configure()); } @@ -596,7 +596,7 @@ TEST_F(Tester, testPolygonIncorrectPoints2) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(polygon_->configure()); } @@ -614,7 +614,7 @@ TEST_F(Tester, testPolygonMaxPoints) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_TRUE(polygon_->configure()); EXPECT_EQ(polygon_->getMinPoints(), max_points + 1); } @@ -625,7 +625,7 @@ TEST_F(Tester, testCircleUndeclaredRadius) circle_ = std::make_shared( test_node_, CIRCLE_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(circle_->configure()); // Check that "radius" parameter is not set after configuring @@ -768,7 +768,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_TRUE(polygon_->configure()); std::vector points; @@ -902,7 +902,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) // Create new polygon polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_TRUE(polygon_->configure()); polygon_->activate(); @@ -925,7 +925,7 @@ TEST_F(Tester, testPolygonInvalidPointsString) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_FALSE(polygon_->configure()); } diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 0bfcd1a062..6db76b2755 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -52,7 +52,7 @@ static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; static const char RANGE_NAME[]{"Range"}; static const char RANGE_TOPIC[]{"range"}; -static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; +static const tf2::Duration TRANSFORM_TIMEOUT{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; class TestNode : public nav2_util::LifecycleNode @@ -171,12 +171,12 @@ class ScanWrapper : public nav2_collision_monitor::Scan const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & data_timeout, const bool base_shift_correction) : nav2_collision_monitor::Scan( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout, base_shift_correction) + transform_timeout, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -194,12 +194,12 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & data_timeout, const bool base_shift_correction) : nav2_collision_monitor::PointCloud( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout, base_shift_correction) + transform_timeout, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -217,12 +217,12 @@ class RangeWrapper : public nav2_collision_monitor::Range const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const rclcpp::Duration & data_timeout, const bool base_shift_correction) : nav2_collision_monitor::Range( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout, base_shift_correction) + transform_timeout, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -294,7 +294,7 @@ void Tester::createSources(const bool base_shift_correction) scan_ = std::make_shared( test_node_, SCAN_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + TRANSFORM_TIMEOUT, DATA_TIMEOUT, base_shift_correction); scan_->configure(); // Create PointCloud object @@ -314,7 +314,7 @@ void Tester::createSources(const bool base_shift_correction) pointcloud_ = std::make_shared( test_node_, POINTCLOUD_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + TRANSFORM_TIMEOUT, DATA_TIMEOUT, base_shift_correction); pointcloud_->configure(); // Create Range object @@ -329,7 +329,7 @@ void Tester::createSources(const bool base_shift_correction) range_ = std::make_shared( test_node_, RANGE_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + TRANSFORM_TIMEOUT, DATA_TIMEOUT, base_shift_correction); range_->configure(); } diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 4fd4f8bc0b..c1a872e43a 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -66,7 +66,7 @@ static const char RIGHT_POLYGON_STR[]{ static const bool IS_HOLONOMIC{true}; static const bool IS_NOT_HOLONOMIC{false}; static const int MIN_POINTS{2}; -static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; +static const tf2::Duration TRANSFORM_TIMEOUT{tf2::durationFromSec(0.1)}; class TestNode : public nav2_util::LifecycleNode { @@ -113,9 +113,9 @@ class VelocityPolygonWrapper : public nav2_collision_monitor::VelocityPolygon const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_tolerance) + const tf2::Duration & transform_timeout) : nav2_collision_monitor::VelocityPolygon( - node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) + node, polygon_name, tf_buffer, base_frame_id, transform_timeout) { } @@ -353,7 +353,7 @@ void Tester::createVelocityPolygon(const std::string & action_type, const bool i velocity_polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); ASSERT_TRUE(velocity_polygon_->configure()); velocity_polygon_->activate(); } diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 61bda4c8b6..e97d05cfc8 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -64,14 +64,14 @@ bool transformPoseInTargetFrame( * @brief Obtains a transform from source_frame_id -> to target_frame_id * @param target_frame_id Target frame ID to convert to * @param source_frame_id Source frame ID to convert from - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param tf_buffer TF buffer to use for the transformation * @return tf_transform Output source->target transform */ std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer); /** @@ -82,7 +82,7 @@ std::optional getTransform( * @param target_frame_id Target frame ID to convert to * @param target_time Current node time to interpolate to * @param fixed_frame_id The frame in which to assume the transform is constant in time - * @param transform_tolerance Transform tolerance + * @param transform_timeout How long to block before failing * @param tf_buffer TF buffer to use for the transformation * @param tf_transform Output source->target transform * @return tf_transform Output source->target transform @@ -93,7 +93,7 @@ std::optional getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & fixed_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer); /** diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index cc4f242768..84fc9818c5 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -81,7 +81,7 @@ bool transformPoseInTargetFrame( std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer) { tf2::Transform tf2_transform; @@ -97,7 +97,7 @@ std::optional getTransform( // Obtaining the transform to get data from source to target frame transform = tf_buffer->lookupTransform( target_frame_id, source_frame_id, - tf2::TimePointZero, transform_tolerance); + tf2::TimePointZero, transform_timeout); } catch (tf2::TransformException & e) { RCLCPP_ERROR( rclcpp::get_logger("getTransform"), @@ -117,7 +117,7 @@ std::optional getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & fixed_frame_id, - const tf2::Duration & transform_tolerance, + const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer) { tf2::Transform tf2_transform; @@ -131,7 +131,7 @@ std::optional getTransform( transform = tf_buffer->lookupTransform( target_frame_id, target_time, source_frame_id, source_time, - fixed_frame_id, transform_tolerance); + fixed_frame_id, transform_timeout); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( rclcpp::get_logger("getTransform"), From 41f89204de4e494a42d247cbc4b27e870eca5e97 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 15:45:35 +0000 Subject: [PATCH 06/14] Last step, convert functions to template functions This allow us to also query for the TransformStamped message from the lookupTransform method. Signed-off-by: Ignacio Vizzo --- nav2_util/include/nav2_util/robot_utils.hpp | 7 +- nav2_util/src/robot_utils.cpp | 81 +++++++++++++++------ 2 files changed, 62 insertions(+), 26 deletions(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index e97d05cfc8..e2c3961c83 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -68,12 +68,14 @@ bool transformPoseInTargetFrame( * @param tf_buffer TF buffer to use for the transformation * @return tf_transform Output source->target transform */ -std::optional getTransform( +template +std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer); + /** * @brief Obtains a transform from source_frame_id at source_time -> * to target_frame_id at target_time time @@ -87,7 +89,8 @@ std::optional getTransform( * @param tf_transform Output source->target transform * @return tf_transform Output source->target transform */ -std::optional getTransform( +template +std::optional getTransform( const std::string & target_frame_id, const rclcpp::Time & target_time, const std::string & source_frame_id, diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 84fc9818c5..3645b40fcf 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -78,24 +78,16 @@ bool transformPoseInTargetFrame( return false; } -std::optional getTransform( +template<> +std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer) { - tf2::Transform tf2_transform; - tf2_transform.setIdentity(); // initialize by identical transform - - if (source_frame_id == target_frame_id) { - // We are already in required frame - return tf2_transform; - } - - geometry_msgs::msg::TransformStamped transform; try { // Obtaining the transform to get data from source to target frame - transform = tf_buffer->lookupTransform( + return tf_buffer->lookupTransform( target_frame_id, source_frame_id, tf2::TimePointZero, transform_timeout); } catch (tf2::TransformException & e) { @@ -105,13 +97,10 @@ std::optional getTransform( source_frame_id.c_str(), target_frame_id.c_str(), e.what()); return std::nullopt; } - - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); - return tf2_transform; } -std::optional getTransform( +template<> +std::optional getTransform( const std::string & target_frame_id, const rclcpp::Time & target_time, const std::string & source_frame_id, @@ -120,15 +109,10 @@ std::optional getTransform( const tf2::Duration & transform_timeout, const std::shared_ptr tf_buffer) { - tf2::Transform tf2_transform; - tf2_transform.setIdentity(); // initialize by identical transform - - geometry_msgs::msg::TransformStamped transform; - try { // Obtaining the transform to get data from source to target frame. // This also considers the time shift between source and target. - transform = tf_buffer->lookupTransform( + return tf_buffer->lookupTransform( target_frame_id, target_time, source_frame_id, source_time, fixed_frame_id, transform_timeout); @@ -139,9 +123,58 @@ std::optional getTransform( source_frame_id.c_str(), target_frame_id.c_str(), ex.what()); return std::nullopt; } +} + +template<> +std::optional getTransform( + const std::string & target_frame_id, + const std::string & source_frame_id, + const tf2::Duration & transform_timeout, + const std::shared_ptr tf_buffer) +{ + tf2::Transform tf2_transform; + tf2_transform.setIdentity(); // initialize by identical transform + + if (source_frame_id == target_frame_id) { + // We are already in required frame + return tf2_transform; + } + + const auto transform_msg = getTransform( + target_frame_id, + source_frame_id, + transform_timeout, + tf_buffer); + + if (!transform_msg.has_value()) {return std::nullopt;} + tf2::fromMsg(transform_msg.value().transform, tf2_transform); + return tf2_transform; +} + +template<> +std::optional getTransform( + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_timeout, + const std::shared_ptr tf_buffer) +{ - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); + const auto transform_msg = getTransform( + target_frame_id, + target_time, + source_frame_id, + source_time, + fixed_frame_id, + transform_timeout, + tf_buffer); + + if (!transform_msg.has_value()) {return std::nullopt;} + + tf2::Transform tf2_transform; + tf2::fromMsg(transform_msg.value().transform, tf2_transform); return tf2_transform; } From 0b33a52bfa037a99b3e6264402b3c7a58b6ff133 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 15:57:08 +0000 Subject: [PATCH 07/14] Add nodiscard to avoid confusiong API calls Signed-off-by: Ignacio Vizzo --- nav2_util/include/nav2_util/robot_utils.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index e2c3961c83..b19af02766 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -69,7 +69,7 @@ bool transformPoseInTargetFrame( * @return tf_transform Output source->target transform */ template -std::optional getTransform( +[[nodiscard]] std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, const tf2::Duration & transform_timeout, @@ -90,7 +90,7 @@ std::optional getTransform( * @return tf_transform Output source->target transform */ template -std::optional getTransform( +[[nodiscard]] std::optional getTransform( const std::string & target_frame_id, const rclcpp::Time & target_time, const std::string & source_frame_id, From 0f0a75ee5bfe1a566d41e309d48da763c6cddcfa Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 15:58:04 +0000 Subject: [PATCH 08/14] Update docs Signed-off-by: Ignacio Vizzo --- nav2_util/include/nav2_util/robot_utils.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index b19af02766..74fe4cf182 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -66,7 +66,7 @@ bool transformPoseInTargetFrame( * @param source_frame_id Source frame ID to convert from * @param transform_timeout How long to block before failing * @param tf_buffer TF buffer to use for the transformation - * @return tf_transform Output source->target transform + * @return transform Output source->target transform */ template [[nodiscard]] std::optional getTransform( @@ -86,8 +86,7 @@ template * @param fixed_frame_id The frame in which to assume the transform is constant in time * @param transform_timeout How long to block before failing * @param tf_buffer TF buffer to use for the transformation - * @param tf_transform Output source->target transform - * @return tf_transform Output source->target transform + * @return transform Output source->target transform */ template [[nodiscard]] std::optional getTransform( From d839f3b22afd66a56e24c7f6fd1c436e07c329e7 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 18:22:57 +0000 Subject: [PATCH 09/14] Revert "transform tolerance -> transform timeout" This reverts commit ca7d06b9568d0b9560b9c372609ad8e8ec7eb118. Signed-off-by: Ignacio Vizzo --- .../include/nav2_collision_monitor/circle.hpp | 4 +-- .../collision_detector_node.hpp | 8 ++--- .../collision_monitor_node.hpp | 8 ++--- .../nav2_collision_monitor/pointcloud.hpp | 4 +-- .../nav2_collision_monitor/polygon.hpp | 8 ++--- .../include/nav2_collision_monitor/range.hpp | 4 +-- .../include/nav2_collision_monitor/scan.hpp | 4 +-- .../include/nav2_collision_monitor/source.hpp | 8 ++--- .../velocity_polygon.hpp | 2 +- .../params/collision_detector_params.yaml | 2 +- .../params/collision_monitor_params.yaml | 2 +- nav2_collision_monitor/src/circle.cpp | 4 +-- .../src/collision_detector_node.cpp | 28 +++++++-------- .../src/collision_monitor_node.cpp | 28 +++++++-------- nav2_collision_monitor/src/pointcloud.cpp | 8 ++--- nav2_collision_monitor/src/polygon.cpp | 10 +++--- nav2_collision_monitor/src/range.cpp | 8 ++--- nav2_collision_monitor/src/scan.cpp | 8 ++--- nav2_collision_monitor/src/source.cpp | 4 +-- .../src/velocity_polygon.cpp | 4 +-- .../test/collision_detector_node_test.cpp | 6 ++-- .../test/collision_monitor_node_test.cpp | 6 ++-- nav2_collision_monitor/test/polygons_test.cpp | 34 +++++++++---------- nav2_collision_monitor/test/sources_test.cpp | 20 +++++------ .../test/velocity_polygons_test.cpp | 8 ++--- nav2_util/include/nav2_util/robot_utils.hpp | 13 +++---- nav2_util/src/robot_utils.cpp | 16 ++++----- 27 files changed, 130 insertions(+), 129 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index baac1dbfea..c440032701 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -38,14 +38,14 @@ class Circle : public Polygon * @param polygon_name Name of circle * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance */ Circle( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout); + const tf2::Duration & transform_tolerance); /** * @brief Circle class destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 0eaee357f6..fae479d650 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -100,18 +100,18 @@ class CollisionDetector : public nav2_util::LifecycleNode /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @return True if all polygons were configured successfully or false in failure case */ bool configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_timeout); + const tf2::Duration & transform_tolerance); /** * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get * source->base time interpolated transform. - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -120,7 +120,7 @@ class CollisionDetector : public nav2_util::LifecycleNode bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index bf744fad01..998dacb519 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -124,18 +124,18 @@ class CollisionMonitor : public nav2_util::LifecycleNode /** * @brief Supporting routine creating and configuring all polygons * @param base_frame_id Robot base frame ID - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @return True if all polygons were configured successfully or false in failure case */ bool configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_timeout); + const tf2::Duration & transform_tolerance); /** * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get * source->base time interpolated transform. - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -144,7 +144,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index d248ff2f34..bfe8d09ed3 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -39,7 +39,7 @@ class PointCloud : public Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -50,7 +50,7 @@ class PointCloud : public Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 45d1fc799d..5e342cdb7f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -47,14 +47,14 @@ class Polygon * @param polygon_name Name of polygon * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID - * @param transform_timeout how long to block before failing + * @param transform_tolerance Transform tolerance */ Polygon( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout); + const tf2::Duration & transform_tolerance); /** * @brief Polygon destructor */ @@ -256,8 +256,8 @@ class Polygon std::shared_ptr tf_buffer_; /// @brief Base frame ID std::string base_frame_id_; - /// @brief Transform timeout on how long to block before failing - tf2::Duration transform_timeout_; + /// @brief Transform tolerance + tf2::Duration transform_tolerance_; // Visualization /// @brief Whether to publish the polygon diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 6de75411f9..1dc15195f6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -39,7 +39,7 @@ class Range : public Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -50,7 +50,7 @@ class Range : public Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index 4aeea01f0c..efaf82e0e3 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -39,7 +39,7 @@ class Scan : public Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -50,7 +50,7 @@ class Scan : public Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 499e86d3c5..865a3c023b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -44,7 +44,7 @@ class Source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, * considering the difference between current time and latest source time @@ -55,7 +55,7 @@ class Source const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction); /** @@ -143,8 +143,8 @@ class Source std::string base_frame_id_; /// @brief Global frame ID for correct transform calculation std::string global_frame_id_; - /// @brief Transform timeout - tf2::Duration transform_timeout_; + /// @brief Transform tolerance + tf2::Duration transform_tolerance_; /// @brief Maximum time interval in which data is considered valid rclcpp::Duration source_timeout_; /// @brief Whether to correct source data towards to base frame movement, diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index 6aded24c2c..e4c65bebad 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -45,7 +45,7 @@ class VelocityPolygon : public Polygon VelocityPolygon( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout); + const tf2::Duration & transform_tolerance); /** * @brief VelocityPolygon destructor */ diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 5cbfec16a9..eb3ee0a873 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -3,7 +3,7 @@ collision_detector: frequency: 10.0 base_frame_id: "base_footprint" odom_frame_id: "odom" - transform_timeout: 0.5 + transform_tolerance: 0.5 source_timeout: 5.0 base_shift_correction: True polygons: ["PolygonFront"] diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 2ccc9b1951..d6b3cd25fd 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -5,7 +5,7 @@ collision_monitor: cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "cmd_vel" state_topic: "collision_monitor_state" - transform_timeout: 0.5 + transform_tolerance: 0.5 source_timeout: 5.0 base_shift_correction: True stop_pub_timeout: 2.0 diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 169b578b0c..fa0fc33a4f 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -28,8 +28,8 @@ Circle::Circle( const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout) -: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_timeout) + const tf2::Duration & transform_tolerance) +: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) { RCLCPP_INFO(logger_, "[%s]: Creating Circle", polygon_name_.c_str()); } diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index ab054ac208..4f485520c7 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -141,7 +141,7 @@ CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) bool CollisionDetector::getParameters() { std::string base_frame_id, odom_frame_id; - tf2::Duration transform_timeout; + tf2::Duration transform_tolerance; rclcpp::Duration source_timeout(2.0, 0.0); auto node = shared_from_this(); @@ -156,9 +156,9 @@ bool CollisionDetector::getParameters() node, "odom_frame_id", rclcpp::ParameterValue("odom")); odom_frame_id = get_parameter("odom_frame_id").as_string(); nav2_util::declare_parameter_if_not_declared( - node, "transform_timeout", rclcpp::ParameterValue(0.1)); - transform_timeout = - tf2::durationFromSec(get_parameter("transform_timeout").as_double()); + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + transform_tolerance = + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = @@ -168,12 +168,12 @@ bool CollisionDetector::getParameters() const bool base_shift_correction = get_parameter("base_shift_correction").as_bool(); - if (!configurePolygons(base_frame_id, transform_timeout)) { + if (!configurePolygons(base_frame_id, transform_tolerance)) { return false; } if (!configureSources( - base_frame_id, odom_frame_id, transform_timeout, source_timeout, + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) { return false; @@ -184,7 +184,7 @@ bool CollisionDetector::getParameters() bool CollisionDetector::configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_timeout) + const tf2::Duration & transform_tolerance) { try { auto node = shared_from_this(); @@ -202,15 +202,15 @@ bool CollisionDetector::configurePolygons( if (polygon_type == "polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else if (polygon_type == "circle") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else if (polygon_type == "velocity_polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -246,7 +246,7 @@ bool CollisionDetector::configurePolygons( bool CollisionDetector::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction) { @@ -266,7 +266,7 @@ bool CollisionDetector::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_timeout, source_timeout, base_shift_correction); + transform_tolerance, source_timeout, base_shift_correction); s->configure(); @@ -274,7 +274,7 @@ bool CollisionDetector::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_timeout, source_timeout, base_shift_correction); + transform_tolerance, source_timeout, base_shift_correction); p->configure(); @@ -282,7 +282,7 @@ bool CollisionDetector::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_timeout, source_timeout, base_shift_correction); + transform_tolerance, source_timeout, base_shift_correction); r->configure(); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 69382e1ee3..94c116445a 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -231,7 +231,7 @@ bool CollisionMonitor::getParameters( std::string & state_topic) { std::string base_frame_id, odom_frame_id; - tf2::Duration transform_timeout; + tf2::Duration transform_tolerance; rclcpp::Duration source_timeout(2.0, 0.0); auto node = shared_from_this(); @@ -253,9 +253,9 @@ bool CollisionMonitor::getParameters( node, "odom_frame_id", rclcpp::ParameterValue("odom")); odom_frame_id = get_parameter("odom_frame_id").as_string(); nav2_util::declare_parameter_if_not_declared( - node, "transform_timeout", rclcpp::ParameterValue(0.1)); - transform_timeout = - tf2::durationFromSec(get_parameter("transform_timeout").as_double()); + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + transform_tolerance = + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = @@ -270,13 +270,13 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_timeout)) { + if (!configurePolygons(base_frame_id, transform_tolerance)) { return false; } if ( !configureSources( - base_frame_id, odom_frame_id, transform_timeout, source_timeout, base_shift_correction)) + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) { return false; } @@ -286,7 +286,7 @@ bool CollisionMonitor::getParameters( bool CollisionMonitor::configurePolygons( const std::string & base_frame_id, - const tf2::Duration & transform_timeout) + const tf2::Duration & transform_tolerance) { try { auto node = shared_from_this(); @@ -304,15 +304,15 @@ bool CollisionMonitor::configurePolygons( if (polygon_type == "polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else if (polygon_type == "circle") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else if (polygon_type == "velocity_polygon") { polygons_.push_back( std::make_shared( - node, polygon_name, tf_buffer_, base_frame_id, transform_timeout)); + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -337,7 +337,7 @@ bool CollisionMonitor::configurePolygons( bool CollisionMonitor::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction) { @@ -357,7 +357,7 @@ bool CollisionMonitor::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_timeout, source_timeout, base_shift_correction); + transform_tolerance, source_timeout, base_shift_correction); s->configure(); @@ -365,7 +365,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_timeout, source_timeout, base_shift_correction); + transform_tolerance, source_timeout, base_shift_correction); p->configure(); @@ -373,7 +373,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_timeout, source_timeout, base_shift_correction); + transform_tolerance, source_timeout, base_shift_correction); r->configure(); diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 2a285e1aba..e608561509 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -31,12 +31,12 @@ PointCloud::PointCloud( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_timeout, source_timeout, base_shift_correction), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str()); @@ -85,7 +85,7 @@ bool PointCloud::getData( if (base_shift_correction_) { return nav2_util::getTransform( base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_timeout_, + data_->header.stamp, global_frame_id_, transform_tolerance_, tf_buffer_); } @@ -93,7 +93,7 @@ bool PointCloud::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_timeout_, + base_frame_id_, data_->header.frame_id, transform_tolerance_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 067606048b..924cf271d9 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -35,11 +35,11 @@ Polygon::Polygon( const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout) + const tf2::Duration & transform_tolerance) : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), - base_frame_id_(base_frame_id), transform_timeout_(transform_timeout) + base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); } @@ -84,7 +84,7 @@ bool Polygon::configure() polygon_name_.c_str(), footprint_topic.c_str()); footprint_sub_ = std::make_unique( node, footprint_topic, *tf_buffer_, - base_frame_id_, tf2::durationToSec(transform_timeout_)); + base_frame_id_, tf2::durationToSec(transform_tolerance_)); } if (visualize_) { @@ -208,7 +208,7 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::optional tf_transform = nav2_util::getTransform( base_frame_id_, polygon_.header.frame_id, - transform_timeout_, + transform_tolerance_, tf_buffer_); if (!tf_transform.has_value()) {return;} @@ -459,7 +459,7 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m std::optional tf_transform = nav2_util::getTransform( base_frame_id_, msg->header.frame_id, - transform_timeout_, + transform_tolerance_, tf_buffer_); if (!tf_transform.has_value()) {return;} diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 9a58812e24..3fee69f6f5 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -31,12 +31,12 @@ Range::Range( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_timeout, source_timeout, base_shift_correction), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); @@ -94,7 +94,7 @@ bool Range::getData( if (base_shift_correction_) { return nav2_util::getTransform( base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_timeout_, + data_->header.stamp, global_frame_id_, transform_tolerance_, tf_buffer_); } @@ -102,7 +102,7 @@ bool Range::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_timeout_, + base_frame_id_, data_->header.frame_id, transform_tolerance_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index edd8c941a5..dda950d40d 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -29,12 +29,12 @@ Scan::Scan( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_timeout, source_timeout, base_shift_correction), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str()); @@ -84,7 +84,7 @@ bool Scan::getData( if (base_shift_correction_) { return nav2_util::getTransform( base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_timeout_, + data_->header.stamp, global_frame_id_, transform_tolerance_, tf_buffer_); } @@ -92,7 +92,7 @@ bool Scan::getData( // considered. Less accurate but much more faster option not dependent on state estimation // frames. return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_timeout_, + base_frame_id_, data_->header.frame_id, transform_tolerance_, tf_buffer_); }(); diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index f14fc17bb9..a27876f05b 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -29,12 +29,12 @@ Source::Source( const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & source_timeout, const bool base_shift_correction) : node_(node), source_name_(source_name), tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), global_frame_id_(global_frame_id), - transform_timeout_(transform_timeout), source_timeout_(source_timeout), + transform_tolerance_(transform_tolerance), source_timeout_(source_timeout), base_shift_correction_(base_shift_correction) { } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index da8dded7e3..84779a1a2d 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -22,8 +22,8 @@ namespace nav2_collision_monitor VelocityPolygon::VelocityPolygon( const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout) -: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_timeout) + const tf2::Duration & transform_tolerance) +: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) { RCLCPP_INFO(logger_, "[%s]: Creating VelocityPolygon", polygon_name_.c_str()); } diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 4025244c1a..e36ef6e583 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -53,7 +53,7 @@ static const char STATE_TOPIC[]{"collision_detector_state"}; static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; static const double SIMULATION_TIME_STEP{0.01}; -static const double TRANSFORM_TIMEOUT{0.5}; +static const double TRANSFORM_TOLERANCE{0.5}; static const double SOURCE_TIMEOUT{5.0}; static const double FREQUENCY{10.0}; @@ -246,9 +246,9 @@ void Tester::setCommonParameters() rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); cd_->declare_parameter( - "transform_timeout", rclcpp::ParameterValue(TRANSFORM_TIMEOUT)); + "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); cd_->set_parameter( - rclcpp::Parameter("transform_timeout", TRANSFORM_TIMEOUT)); + rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); cd_->declare_parameter( "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); cd_->set_parameter( diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 1e9adabcad..34690a6fd1 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -60,7 +60,7 @@ static const double LINEAR_LIMIT{0.4}; static const double ANGULAR_LIMIT{0.09}; static const double TIME_BEFORE_COLLISION{1.0}; static const double SIMULATION_TIME_STEP{0.01}; -static const double TRANSFORM_TIMEOUT{0.5}; +static const double TRANSFORM_TOLERANCE{0.5}; static const double SOURCE_TIMEOUT{5.0}; static const double STOP_PUB_TIMEOUT{0.1}; @@ -286,9 +286,9 @@ void Tester::setCommonParameters() rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); cm_->declare_parameter( - "transform_timeout", rclcpp::ParameterValue(TRANSFORM_TIMEOUT)); + "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); cm_->set_parameter( - rclcpp::Parameter("transform_timeout", TRANSFORM_TIMEOUT)); + rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); cm_->declare_parameter( "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); cm_->set_parameter( diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index e0221881e6..849b627868 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -70,7 +70,7 @@ static const double LINEAR_LIMIT{0.4}; static const double ANGULAR_LIMIT{0.09}; static const double TIME_BEFORE_COLLISION{1.0}; static const double SIMULATION_TIME_STEP{0.01}; -static const tf2::Duration TRANSFORM_TIMEOUT{tf2::durationFromSec(0.1)}; +static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; class TestNode : public nav2_util::LifecycleNode { @@ -173,9 +173,9 @@ class PolygonWrapper : public nav2_collision_monitor::Polygon const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout) + const tf2::Duration & transform_tolerance) : nav2_collision_monitor::Polygon( - node, polygon_name, tf_buffer, base_frame_id, transform_timeout) + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) { } @@ -198,9 +198,9 @@ class CircleWrapper : public nav2_collision_monitor::Circle const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout) + const tf2::Duration & transform_tolerance) : nav2_collision_monitor::Circle( - node, polygon_name, tf_buffer, base_frame_id, transform_timeout) + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) { } @@ -377,7 +377,7 @@ void Tester::createPolygon(const std::string & action_type, const bool is_static polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_TRUE(polygon_->configure()); polygon_->activate(); } @@ -389,7 +389,7 @@ void Tester::createCircle(const std::string & action_type) circle_ = std::make_shared( test_node_, CIRCLE_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_TRUE(circle_->configure()); circle_->activate(); } @@ -535,7 +535,7 @@ TEST_F(Tester, testPolygonUndeclaredActionType) // "action_type" parameter is not initialized polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); // Check that "action_type" parameter is not set after configuring ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "action_type")); @@ -550,7 +550,7 @@ TEST_F(Tester, testPolygonUndeclaredPoints) rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); // Check that "points" and "polygon_sub_topic" parameters are not set after configuring ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); @@ -564,7 +564,7 @@ TEST_F(Tester, testPolygonIncorrectActionType) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); } @@ -580,7 +580,7 @@ TEST_F(Tester, testPolygonIncorrectPoints1) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); } @@ -596,7 +596,7 @@ TEST_F(Tester, testPolygonIncorrectPoints2) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); } @@ -614,7 +614,7 @@ TEST_F(Tester, testPolygonMaxPoints) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_TRUE(polygon_->configure()); EXPECT_EQ(polygon_->getMinPoints(), max_points + 1); } @@ -625,7 +625,7 @@ TEST_F(Tester, testCircleUndeclaredRadius) circle_ = std::make_shared( test_node_, CIRCLE_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(circle_->configure()); // Check that "radius" parameter is not set after configuring @@ -768,7 +768,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_TRUE(polygon_->configure()); std::vector points; @@ -902,7 +902,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) // Create new polygon polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_TRUE(polygon_->configure()); polygon_->activate(); @@ -925,7 +925,7 @@ TEST_F(Tester, testPolygonInvalidPointsString) polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); } diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 6db76b2755..0bfcd1a062 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -52,7 +52,7 @@ static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; static const char RANGE_NAME[]{"Range"}; static const char RANGE_TOPIC[]{"range"}; -static const tf2::Duration TRANSFORM_TIMEOUT{tf2::durationFromSec(0.1)}; +static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; class TestNode : public nav2_util::LifecycleNode @@ -171,12 +171,12 @@ class ScanWrapper : public nav2_collision_monitor::Scan const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & data_timeout, const bool base_shift_correction) : nav2_collision_monitor::Scan( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_timeout, data_timeout, base_shift_correction) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -194,12 +194,12 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & data_timeout, const bool base_shift_correction) : nav2_collision_monitor::PointCloud( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_timeout, data_timeout, base_shift_correction) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -217,12 +217,12 @@ class RangeWrapper : public nav2_collision_monitor::Range const std::shared_ptr tf_buffer, const std::string & base_frame_id, const std::string & global_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const rclcpp::Duration & data_timeout, const bool base_shift_correction) : nav2_collision_monitor::Range( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_timeout, data_timeout, base_shift_correction) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -294,7 +294,7 @@ void Tester::createSources(const bool base_shift_correction) scan_ = std::make_shared( test_node_, SCAN_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TIMEOUT, DATA_TIMEOUT, base_shift_correction); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); scan_->configure(); // Create PointCloud object @@ -314,7 +314,7 @@ void Tester::createSources(const bool base_shift_correction) pointcloud_ = std::make_shared( test_node_, POINTCLOUD_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TIMEOUT, DATA_TIMEOUT, base_shift_correction); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); pointcloud_->configure(); // Create Range object @@ -329,7 +329,7 @@ void Tester::createSources(const bool base_shift_correction) range_ = std::make_shared( test_node_, RANGE_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TIMEOUT, DATA_TIMEOUT, base_shift_correction); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); range_->configure(); } diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index c1a872e43a..4fd4f8bc0b 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -66,7 +66,7 @@ static const char RIGHT_POLYGON_STR[]{ static const bool IS_HOLONOMIC{true}; static const bool IS_NOT_HOLONOMIC{false}; static const int MIN_POINTS{2}; -static const tf2::Duration TRANSFORM_TIMEOUT{tf2::durationFromSec(0.1)}; +static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; class TestNode : public nav2_util::LifecycleNode { @@ -113,9 +113,9 @@ class VelocityPolygonWrapper : public nav2_collision_monitor::VelocityPolygon const std::string & polygon_name, const std::shared_ptr tf_buffer, const std::string & base_frame_id, - const tf2::Duration & transform_timeout) + const tf2::Duration & transform_tolerance) : nav2_collision_monitor::VelocityPolygon( - node, polygon_name, tf_buffer, base_frame_id, transform_timeout) + node, polygon_name, tf_buffer, base_frame_id, transform_tolerance) { } @@ -353,7 +353,7 @@ void Tester::createVelocityPolygon(const std::string & action_type, const bool i velocity_polygon_ = std::make_shared( test_node_, POLYGON_NAME, - tf_buffer_, BASE_FRAME_ID, TRANSFORM_TIMEOUT); + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_TRUE(velocity_polygon_->configure()); velocity_polygon_->activate(); } diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 74fe4cf182..aff6c628f4 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -64,15 +64,15 @@ bool transformPoseInTargetFrame( * @brief Obtains a transform from source_frame_id -> to target_frame_id * @param target_frame_id Target frame ID to convert to * @param source_frame_id Source frame ID to convert from - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation - * @return transform Output source->target transform + * @return tf_transform Output source->target transform */ template [[nodiscard]] std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer); @@ -84,9 +84,10 @@ template * @param target_frame_id Target frame ID to convert to * @param target_time Current node time to interpolate to * @param fixed_frame_id The frame in which to assume the transform is constant in time - * @param transform_timeout How long to block before failing + * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation - * @return transform Output source->target transform + * @param tf_transform Output source->target transform + * @return tf_transform Output source->target transform */ template [[nodiscard]] std::optional getTransform( @@ -95,7 +96,7 @@ template const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & fixed_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer); /** diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 3645b40fcf..6767f7b7fa 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -82,14 +82,14 @@ template<> std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) { try { // Obtaining the transform to get data from source to target frame return tf_buffer->lookupTransform( target_frame_id, source_frame_id, - tf2::TimePointZero, transform_timeout); + tf2::TimePointZero, transform_tolerance); } catch (tf2::TransformException & e) { RCLCPP_ERROR( rclcpp::get_logger("getTransform"), @@ -106,7 +106,7 @@ std::optional getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & fixed_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) { try { @@ -115,7 +115,7 @@ std::optional getTransform( return tf_buffer->lookupTransform( target_frame_id, target_time, source_frame_id, source_time, - fixed_frame_id, transform_timeout); + fixed_frame_id, transform_tolerance); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( rclcpp::get_logger("getTransform"), @@ -129,7 +129,7 @@ template<> std::optional getTransform( const std::string & target_frame_id, const std::string & source_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) { tf2::Transform tf2_transform; @@ -143,7 +143,7 @@ std::optional getTransform( const auto transform_msg = getTransform( target_frame_id, source_frame_id, - transform_timeout, + transform_tolerance, tf_buffer); if (!transform_msg.has_value()) {return std::nullopt;} @@ -158,7 +158,7 @@ std::optional getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, const std::string & fixed_frame_id, - const tf2::Duration & transform_timeout, + const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) { @@ -168,7 +168,7 @@ std::optional getTransform( source_frame_id, source_time, fixed_frame_id, - transform_timeout, + transform_tolerance, tf_buffer); if (!transform_msg.has_value()) {return std::nullopt;} From d21db92a58849d520b3a6db95e20a7a86b1ad4d3 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 14 Mar 2024 18:31:29 +0000 Subject: [PATCH 10/14] =?UTF-8?q?Fix=20linter=20=F0=9F=A4=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ignacio Vizzo --- nav2_util/src/robot_utils.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6767f7b7fa..c4d4eeb448 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -161,7 +161,6 @@ std::optional getTransform( const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer) { - const auto transform_msg = getTransform( target_frame_id, target_time, From 2a45a68d4020319390babe7feefad6d4d9dfdc57 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 28 Mar 2024 08:43:42 +0100 Subject: [PATCH 11/14] reset to main Signed-off-by: Ignacio Vizzo --- nav2_collision_monitor/src/pointcloud.cpp | 46 +++++----- nav2_collision_monitor/src/polygon.cpp | 35 ++++---- nav2_collision_monitor/src/range.cpp | 49 ++++++----- nav2_collision_monitor/src/scan.cpp | 46 +++++----- nav2_util/include/nav2_util/robot_utils.hpp | 30 +++---- nav2_util/src/robot_utils.cpp | 96 +++++++-------------- 6 files changed, 140 insertions(+), 162 deletions(-) diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index e608561509..fb3fca201d 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -15,7 +15,6 @@ #include "nav2_collision_monitor/pointcloud.hpp" #include -#include #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -79,25 +78,30 @@ bool PointCloud::getData( return false; } - const auto tf_transform = [&]() -> std::optional { - // Obtaining the transform to get data from source frame and time where it was received to the - // base frame and current time - if (base_shift_correction_) { - return nav2_util::getTransform( - base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_tolerance_, - tf_buffer_); - } - - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_tolerance_, - tf_buffer_); - }(); - - if (!tf_transform.has_value()) {return false;} + tf2::Transform tf_transform; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*data_, "y"); @@ -107,7 +111,7 @@ bool PointCloud::getData( for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z); - tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array if (p_v3_b.z() >= min_height_ && p_v3_b.z() <= max_height_) { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 924cf271d9..77fc1f304c 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -15,7 +15,6 @@ #include "nav2_collision_monitor/polygon.hpp" #include -#include #include #include "geometry_msgs/msg/point.hpp" @@ -205,20 +204,21 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::size_t new_size = polygon_.polygon.points.size(); // Get the transform from PolygonStamped frame to base_frame_id_ - std::optional tf_transform = nav2_util::getTransform( - base_frame_id_, - polygon_.header.frame_id, - transform_tolerance_, - tf_buffer_); - - if (!tf_transform.has_value()) {return;} + tf2::Transform tf_transform; + if ( + !nav2_util::getTransform( + polygon_.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } // Correct main poly_ vertices poly_.resize(new_size); for (std::size_t i = 0; i < new_size; i++) { // Transform point coordinates from PolygonStamped frame -> to base frame tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0); - tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Fill poly_ array poly_[i] = {p_v3_b.x(), p_v3_b.y()}; @@ -456,20 +456,21 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m } // Get the transform from PolygonStamped frame to base_frame_id_ - std::optional tf_transform = nav2_util::getTransform( - base_frame_id_, - msg->header.frame_id, - transform_tolerance_, - tf_buffer_); - - if (!tf_transform.has_value()) {return;} + tf2::Transform tf_transform; + if ( + !nav2_util::getTransform( + msg->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } // Set main poly_ vertices first time poly_.resize(new_size); for (std::size_t i = 0; i < new_size; i++) { // Transform point coordinates from PolygonStamped frame -> to base frame tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0); - tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Fill poly_ array poly_[i] = {p_v3_b.x(), p_v3_b.y()}; diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 3fee69f6f5..45540a9c4d 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -88,26 +87,30 @@ bool Range::getData( return false; } - const auto tf_transform = [&]() -> std::optional { - // Obtaining the transform to get data from source frame and time where it was received to the - // base frame and current time - if (base_shift_correction_) { - return nav2_util::getTransform( - base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_tolerance_, - tf_buffer_); - } - - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_tolerance_, - tf_buffer_); - }(); - - if (!tf_transform.has_value()) {return false;} - + tf2::Transform tf_transform; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } // Calculate poses and refill data array float angle; @@ -121,7 +124,7 @@ bool Range::getData( data_->range * std::cos(angle), data_->range * std::sin(angle), 0.0); - tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); @@ -135,7 +138,7 @@ bool Range::getData( data_->range * std::cos(angle), data_->range * std::sin(angle), 0.0); - tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index dda950d40d..45f1e62d70 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -16,7 +16,6 @@ #include #include -#include #include "nav2_util/robot_utils.hpp" @@ -78,25 +77,30 @@ bool Scan::getData( return false; } - const auto tf_transform = [&]() -> std::optional { - // Obtaining the transform to get data from source frame and time where it was received to the - // base frame and current time - if (base_shift_correction_) { - return nav2_util::getTransform( - base_frame_id_, curr_time, data_->header.frame_id, - data_->header.stamp, global_frame_id_, transform_tolerance_, - tf_buffer_); - } - - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - return nav2_util::getTransform( - base_frame_id_, data_->header.frame_id, transform_tolerance_, - tf_buffer_); - }(); - - if (!tf_transform.has_value()) {return false;} + tf2::Transform tf_transform; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } // Calculate poses and refill data array float angle = data_->angle_min; @@ -107,7 +111,7 @@ bool Scan::getData( data_->ranges[i] * std::cos(angle), data_->ranges[i] * std::sin(angle), 0.0); - tf2::Vector3 p_v3_b = tf_transform.value() * p_v3_s; + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index aff6c628f4..3cc8f87267 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -17,9 +17,8 @@ #ifndef NAV2_UTIL__ROBOT_UTILS_HPP_ #define NAV2_UTIL__ROBOT_UTILS_HPP_ -#include -#include #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -27,6 +26,7 @@ #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "rclcpp/rclcpp.hpp" namespace nav2_util { @@ -62,19 +62,19 @@ bool transformPoseInTargetFrame( /** * @brief Obtains a transform from source_frame_id -> to target_frame_id - * @param target_frame_id Target frame ID to convert to * @param source_frame_id Source frame ID to convert from + * @param target_frame_id Target frame ID to convert to * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation - * @return tf_transform Output source->target transform + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false */ -template -[[nodiscard]] std::optional getTransform( - const std::string & target_frame_id, +bool getTransform( const std::string & source_frame_id, + const std::string & target_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer); - + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); /** * @brief Obtains a transform from source_frame_id at source_time -> @@ -87,17 +87,17 @@ template * @param transform_tolerance Transform tolerance * @param tf_buffer TF buffer to use for the transformation * @param tf_transform Output source->target transform - * @return tf_transform Output source->target transform + * @return True if got correct transform, otherwise false */ -template -[[nodiscard]] std::optional getTransform( - const std::string & target_frame_id, - const rclcpp::Time & target_time, +bool getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer); + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); /** * @brief Validates a twist message contains no nans or infs diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index c4d4eeb448..6933b71694 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -14,7 +14,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -78,16 +77,24 @@ bool transformPoseInTargetFrame( return false; } -template<> -std::optional getTransform( - const std::string & target_frame_id, +bool getTransform( const std::string & source_frame_id, + const std::string & target_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer) + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) { + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + if (source_frame_id == target_frame_id) { + // We are already in required frame + return true; + } + try { // Obtaining the transform to get data from source to target frame - return tf_buffer->lookupTransform( + transform = tf_buffer->lookupTransform( target_frame_id, source_frame_id, tf2::TimePointZero, transform_tolerance); } catch (tf2::TransformException & e) { @@ -95,24 +102,31 @@ std::optional getTransform( rclcpp::get_logger("getTransform"), "Failed to get \"%s\"->\"%s\" frame transform: %s", source_frame_id.c_str(), target_frame_id.c_str(), e.what()); - return std::nullopt; + return false; } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; } -template<> -std::optional getTransform( - const std::string & target_frame_id, - const rclcpp::Time & target_time, +bool getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer) + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) { + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + try { // Obtaining the transform to get data from source to target frame. // This also considers the time shift between source and target. - return tf_buffer->lookupTransform( + transform = tf_buffer->lookupTransform( target_frame_id, target_time, source_frame_id, source_time, fixed_frame_id, transform_tolerance); @@ -121,60 +135,12 @@ std::optional getTransform( rclcpp::get_logger("getTransform"), "Failed to get \"%s\"->\"%s\" frame transform: %s", source_frame_id.c_str(), target_frame_id.c_str(), ex.what()); - return std::nullopt; - } -} - -template<> -std::optional getTransform( - const std::string & target_frame_id, - const std::string & source_frame_id, - const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer) -{ - tf2::Transform tf2_transform; - tf2_transform.setIdentity(); // initialize by identical transform - - if (source_frame_id == target_frame_id) { - // We are already in required frame - return tf2_transform; + return false; } - const auto transform_msg = getTransform( - target_frame_id, - source_frame_id, - transform_tolerance, - tf_buffer); - - if (!transform_msg.has_value()) {return std::nullopt;} - tf2::fromMsg(transform_msg.value().transform, tf2_transform); - return tf2_transform; -} - -template<> -std::optional getTransform( - const std::string & target_frame_id, - const rclcpp::Time & target_time, - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const std::string & fixed_frame_id, - const tf2::Duration & transform_tolerance, - const std::shared_ptr tf_buffer) -{ - const auto transform_msg = getTransform( - target_frame_id, - target_time, - source_frame_id, - source_time, - fixed_frame_id, - transform_tolerance, - tf_buffer); - - if (!transform_msg.has_value()) {return std::nullopt;} - - tf2::Transform tf2_transform; - tf2::fromMsg(transform_msg.value().transform, tf2_transform); - return tf2_transform; + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; } bool validateTwist(const geometry_msgs::msg::Twist & msg) From c4c651d65fcce1a37fdb9d8e36e5dc6e1ace7d40 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 28 Mar 2024 08:50:04 +0100 Subject: [PATCH 12/14] Add 2 new utils functions to query transforms using messages Signed-off-by: Ignacio Vizzo --- nav2_util/include/nav2_util/robot_utils.hpp | 40 ++++++++++++++ nav2_util/src/robot_utils.cpp | 59 +++++++++++++++------ 2 files changed, 84 insertions(+), 15 deletions(-) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 3cc8f87267..08c52af040 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -23,6 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -99,6 +100,45 @@ bool getTransform( const std::shared_ptr tf_buffer, tf2::Transform & tf2_transform); +/** + * @brief Obtains a transform from source_frame_id -> to target_frame_id + * @param source_frame_id Source frame ID to convert from + * @param target_frame_id Target frame ID to convert to + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param transform_msg Output source->target transform msg + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + geometry_msgs::msg::TransformStamped & transform_msg); + +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Current node time to interpolate to + * @param fixed_frame_id The frame in which to assume the transform is constant in time + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param transform_msg Output source->target transform msg + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + geometry_msgs::msg::TransformStamped & transform_msg); + /** * @brief Validates a twist message contains no nans or infs * @param msg Twist message to validate diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6933b71694..75aa37ada3 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -82,11 +82,8 @@ bool getTransform( const std::string & target_frame_id, const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer, - tf2::Transform & tf2_transform) + geometry_msgs::msg::TransformStamped & transform_msg) { - geometry_msgs::msg::TransformStamped transform; - tf2_transform.setIdentity(); // initialize by identical transform - if (source_frame_id == target_frame_id) { // We are already in required frame return true; @@ -94,7 +91,7 @@ bool getTransform( try { // Obtaining the transform to get data from source to target frame - transform = tf_buffer->lookupTransform( + transform_msg = tf_buffer->lookupTransform( target_frame_id, source_frame_id, tf2::TimePointZero, transform_tolerance); } catch (tf2::TransformException & e) { @@ -104,29 +101,40 @@ bool getTransform( source_frame_id.c_str(), target_frame_id.c_str(), e.what()); return false; } - - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); return true; } bool getTransform( const std::string & source_frame_id, - const rclcpp::Time & source_time, const std::string & target_frame_id, - const rclcpp::Time & target_time, - const std::string & fixed_frame_id, const tf2::Duration & transform_tolerance, const std::shared_ptr tf_buffer, tf2::Transform & tf2_transform) { - geometry_msgs::msg::TransformStamped transform; tf2_transform.setIdentity(); // initialize by identical transform + geometry_msgs::msg::TransformStamped transform; + if (getTransform(source_frame_id, target_frame_id, transform_tolerance, tf_buffer, transform)) { + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; + } + return false; +} +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + geometry_msgs::msg::TransformStamped & transform_msg) +{ try { // Obtaining the transform to get data from source to target frame. // This also considers the time shift between source and target. - transform = tf_buffer->lookupTransform( + transform_msg = tf_buffer->lookupTransform( target_frame_id, target_time, source_frame_id, source_time, fixed_frame_id, transform_tolerance); @@ -138,11 +146,32 @@ bool getTransform( return false; } - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); return true; } +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + if (getTransform( + source_frame_id, source_time, target_frame_id, target_time, fixed_frame_id, + transform_tolerance, tf_buffer, transform)) + { + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; + } + return false; +} + bool validateTwist(const geometry_msgs::msg::Twist & msg) { if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) { From d450502d122962e985193f8699dfb00a2a18f089 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 28 Mar 2024 09:13:02 +0100 Subject: [PATCH 13/14] Move utility function to base class to reuse implementation Signed-off-by: Ignacio Vizzo --- .../include/nav2_collision_monitor/source.hpp | 19 +++++++++++-- nav2_collision_monitor/src/pointcloud.cpp | 24 ++--------------- nav2_collision_monitor/src/range.cpp | 24 ++--------------- nav2_collision_monitor/src/scan.cpp | 24 ++--------------- nav2_collision_monitor/src/source.cpp | 27 +++++++++++++++++++ 5 files changed, 50 insertions(+), 68 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 865a3c023b..a92c7d4c04 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -24,9 +24,9 @@ #include "tf2/time.h" #include "tf2_ros/buffer.h" -#include "nav2_util/lifecycle_node.hpp" - #include "nav2_collision_monitor/types.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "std_msgs/msg//header.hpp" namespace nav2_collision_monitor { @@ -123,6 +123,21 @@ class Source rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); + /** + * @brief Obtain the transform to get data from source frame and time where it was received to the + * base frame and current time (if base_shift_correction_ is true) or the transform without time + * shift considered which is less accurate but much more faster option not dependent on state + * estimation frames. + * @param curr_time Current node time + * @param data_header Current header which contains the frame_id and the stamp + * @param tf_transform Output source->base_frame_id_ transform + * @return True if got correct transform, otherwise false + */ + bool getTransform( + const rclcpp::Time & curr_time, + const std_msgs::msg::Header & data_header, + tf2::Transform & tf_transform) const; + // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index fb3fca201d..817fb48257 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -79,28 +79,8 @@ bool PointCloud::getData( } tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } + if (!getTransform(curr_time, data_->header, tf_transform)) { + return false; } sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 45540a9c4d..8799827f04 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -88,28 +88,8 @@ bool Range::getData( } tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } + if (!getTransform(curr_time, data_->header, tf_transform)) { + return false; } // Calculate poses and refill data array diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 45f1e62d70..660d66e916 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -78,28 +78,8 @@ bool Scan::getData( } tf2::Transform tf_transform; - if (base_shift_correction_) { - // Obtaining the transform to get data from source frame and time where it was received - // to the base frame and current time - if ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } - } else { - // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option not dependent on state estimation - // frames. - if ( - !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return false; - } + if (!getTransform(curr_time, data_->header, tf_transform)) { + return false; } // Calculate poses and refill data array diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index a27876f05b..c7aafd06ac 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -19,6 +19,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -131,4 +132,30 @@ Source::dynamicParametersCallback( return result; } +bool Source::getTransform( + const rclcpp::Time & curr_time, + const std_msgs::msg::Header & data_header, + tf2::Transform & tf_transform) const +{ + if (base_shift_correction_) { + if ( + !nav2_util::getTransform( + data_header.frame_id, data_header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + if ( + !nav2_util::getTransform( + data_header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } + return true; +} + } // namespace nav2_collision_monitor From 47ef2280a17ad32aca04474712f8ce84281e99a6 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Tue, 2 Apr 2024 09:40:37 +0200 Subject: [PATCH 14/14] Fix Typo Signed-off-by: Ignacio Vizzo --- .../include/nav2_collision_monitor/source.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index a92c7d4c04..62e0d60afa 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -26,7 +26,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "std_msgs/msg//header.hpp" +#include "std_msgs/msg/header.hpp" namespace nav2_collision_monitor {