From 09ffe954c87f41cc438d5c9736db687098df794a Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 8 Mar 2021 11:37:31 +0100 Subject: [PATCH 1/5] Fix #2186 --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 8 ++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 12 ++++++ nav2_planner/src/planner_server.cpp | 12 +++++- nav2_util/include/nav2_util/robot_utils.hpp | 5 +++ nav2_util/src/robot_utils.cpp | 37 +++++++++++++++++++ nav2_util/test/test_robot_utils.cpp | 2 + 6 files changed, 75 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 1bc733460d8..b6b560d0796 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -140,6 +140,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose); + /** + * @brief Transform the pose in the global frame of the costmap + * @param pose will be set to the pose of the robot in the global frame of the costmap + * @return True if the pose was transformed successfully, false otherwise + */ + + bool getPoseInGlobalFrame(geometry_msgs::msg::PoseStamped & pose); + /** @brief Returns costmap name */ std::string getName() const { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 70de2770f9a..b259c506f55 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -534,4 +534,16 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) global_frame_, robot_base_frame_, transform_tolerance_); } +bool +Costmap2DROS::getPoseInGlobalFrame(geometry_msgs::msg::PoseStamped & pose) +{ + if (pose.header.frame_id == global_frame_) { + return true; + } else { + return nav2_util::getPoseInTargetFrame( + pose, *tf_buffer_, + global_frame_, transform_tolerance_); + } +} + } // namespace nav2_costmap_2d diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 16df788cf0d..977d4dda645 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -241,11 +241,21 @@ PlannerServer::computePlan() return; } + // Copy needed as goal is const + geometry_msgs::msg::PoseStamped goal_pose = goal->goal; + if (!costmap_ros_->getPoseInGlobalFrame(start) || + !costmap_ros_->getPoseInGlobalFrame(goal_pose)) { + RCLCPP_WARN( + get_logger(), "Could not transform the start or goal pose in the costmap frame"); + action_server_->terminate_current(); + return; + } + if (action_server_->is_preempt_requested()) { goal = action_server_->accept_pending_goal(); } - result->path = getPlan(start, goal->goal, goal->planner_id); + result->path = getPlan(start, goal_pose, goal->planner_id); if (result->path.poses.size() == 0) { RCLCPP_WARN( diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 0575dad5d9e..c5e68d7cf94 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -36,6 +36,11 @@ bool getCurrentPose( tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map", const std::string robot_frame = "base_link", const double transform_timeout = 0.1); +bool getPoseInTargetFrame( + geometry_msgs::msg::PoseStamped & pose, + tf2_ros::Buffer & tf_buffer, const std::string target_frame, + const double transform_timeout = 0.1); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6f735e6d25a..315f3386eb1 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -66,4 +66,41 @@ bool getCurrentPose( return false; } +bool getPoseInTargetFrame( + geometry_msgs::msg::PoseStamped & pose, + tf2_ros::Buffer & tf_buffer, const std::string target_frame, + const double transform_timeout) +{ + static rclcpp::Logger logger = rclcpp::get_logger("getPoseInTargetFrame"); + + try { + pose = tf_buffer.transform( + pose, target_frame, + tf2::durationFromSec(transform_timeout)); + return true; + } catch (tf2::LookupException & ex) { + RCLCPP_ERROR( + logger, + "No Transform available Error looking up target frame: %s\n", ex.what()); + } catch (tf2::ConnectivityException & ex) { + RCLCPP_ERROR( + logger, + "Connectivity Error looking up target frame: %s\n", ex.what()); + } catch (tf2::ExtrapolationException & ex) { + RCLCPP_ERROR( + logger, + "Extrapolation Error looking up target frame: %s\n", ex.what()); + } catch (tf2::TimeoutException & ex) { + RCLCPP_ERROR( + logger, + "Transform timeout with tolerance: %.4f", transform_timeout); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger, "Failed to transform from %s to %s", + pose.header.frame_id.c_str(), target_frame.c_str()); + } + + return false; +} + } // end namespace nav2_util diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index a3530048a4b..5d5598a24da 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -29,4 +29,6 @@ TEST(RobotUtils, LookupExceptionError) geometry_msgs::msg::PoseStamped global_pose; tf2_ros::Buffer tf(node->get_clock()); ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1)); + global_pose.header.frame_id = "base_link"; + ASSERT_FALSE(nav2_util::getPoseInTargetFrame(global_pose, tf, "map", 0.1)); } From 01b28ba21c470b30b83bed3959c9727e31e7a5cd Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 9 Mar 2021 22:03:37 +0100 Subject: [PATCH 2/5] goal_pose instead of goal->goal --- nav2_planner/src/planner_server.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 977d4dda645..5a25eaaed7b 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -241,7 +241,7 @@ PlannerServer::computePlan() return; } - // Copy needed as goal is const + // Changing the start and goal pose frame to the global_frame_ of costmap_ros_ if needed geometry_msgs::msg::PoseStamped goal_pose = goal->goal; if (!costmap_ros_->getPoseInGlobalFrame(start) || !costmap_ros_->getPoseInGlobalFrame(goal_pose)) { @@ -261,7 +261,7 @@ PlannerServer::computePlan() RCLCPP_WARN( get_logger(), "Planning algorithm %s failed to generate a valid" " path to (%.2f, %.2f)", goal->planner_id.c_str(), - goal->goal.pose.position.x, goal->goal.pose.position.y); + goal_pose.pose.position.x, goal_pose.pose.position.y); action_server_->terminate_current(); return; } @@ -269,8 +269,8 @@ PlannerServer::computePlan() RCLCPP_DEBUG( get_logger(), "Found valid path of size %lu to (%.2f, %.2f)", - result->path.poses.size(), goal->goal.pose.position.x, - goal->goal.pose.position.y); + result->path.poses.size(), goal_pose.pose.position.x, + goal_pose.pose.position.y); // Publish the plan for visualization purposes publishPlan(result->path); From 456b6fd4af5c8697d7b37faf47f8535537149a8c Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 9 Mar 2021 22:17:23 +0100 Subject: [PATCH 3/5] avoid redundant code --- nav2_util/src/robot_utils.cpp | 37 +++-------------------------------- 1 file changed, 3 insertions(+), 34 deletions(-) diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 315f3386eb1..bab61d95f04 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -28,42 +28,11 @@ bool getCurrentPose( tf2_ros::Buffer & tf_buffer, const std::string global_frame, const std::string robot_frame, const double transform_timeout) { - static rclcpp::Logger logger = rclcpp::get_logger("getCurrentPose"); - geometry_msgs::msg::PoseStamped robot_pose; - tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose); - tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); - robot_pose.header.frame_id = robot_frame; - robot_pose.header.stamp = rclcpp::Time(); + global_pose.header.frame_id = robot_frame; + global_pose.header.stamp = rclcpp::Time(); - try { - global_pose = tf_buffer.transform( - robot_pose, global_frame, - tf2::durationFromSec(transform_timeout)); - return true; - } catch (tf2::LookupException & ex) { - RCLCPP_ERROR( - logger, - "No Transform available Error looking up robot pose: %s\n", ex.what()); - } catch (tf2::ConnectivityException & ex) { - RCLCPP_ERROR( - logger, - "Connectivity Error looking up robot pose: %s\n", ex.what()); - } catch (tf2::ExtrapolationException & ex) { - RCLCPP_ERROR( - logger, - "Extrapolation Error looking up robot pose: %s\n", ex.what()); - } catch (tf2::TimeoutException & ex) { - RCLCPP_ERROR( - logger, - "Transform timeout with tolerance: %.4f", transform_timeout); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - logger, "Failed to transform from %s to %s", - global_frame.c_str(), robot_frame.c_str()); - } - - return false; + return getPoseInTargetFrame(global_pose, tf_buffer, global_frame, transform_timeout); } bool getPoseInTargetFrame( From 04b4ac3f5888a86231f2bff31b61cf5c97ea0ca6 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 10 Mar 2021 09:42:02 +0100 Subject: [PATCH 4/5] change name and switch to input + output param pattern --- .../include/nav2_costmap_2d/costmap_2d_ros.hpp | 9 +++++---- nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 ++++++---- nav2_planner/src/planner_server.cpp | 4 ++-- nav2_util/include/nav2_util/robot_utils.hpp | 7 ++++--- nav2_util/src/robot_utils.cpp | 16 +++++++++------- nav2_util/test/test_robot_utils.cpp | 2 +- 6 files changed, 27 insertions(+), 21 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index b6b560d0796..491341eba55 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -141,12 +141,13 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose); /** - * @brief Transform the pose in the global frame of the costmap - * @param pose will be set to the pose of the robot in the global frame of the costmap + * @brief Transform the input_pose in the global frame of the costmap + * @param input_pose pose to be transformed + * @param transformed_pose pose transformed * @return True if the pose was transformed successfully, false otherwise */ - - bool getPoseInGlobalFrame(geometry_msgs::msg::PoseStamped & pose); + bool transformPoseToGlobalFrame(const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose); /** @brief Returns costmap name */ std::string getName() const diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index b259c506f55..18b3b979fab 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -535,13 +535,15 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) } bool -Costmap2DROS::getPoseInGlobalFrame(geometry_msgs::msg::PoseStamped & pose) +Costmap2DROS::transformPoseToGlobalFrame(const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose) { - if (pose.header.frame_id == global_frame_) { + if (input_pose.header.frame_id == global_frame_) { + transformed_pose = input_pose; return true; } else { - return nav2_util::getPoseInTargetFrame( - pose, *tf_buffer_, + return nav2_util::transformPoseInTargetFrame( + input_pose, transformed_pose, *tf_buffer_, global_frame_, transform_tolerance_); } } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 5a25eaaed7b..74ec6da06a4 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -243,8 +243,8 @@ PlannerServer::computePlan() // Changing the start and goal pose frame to the global_frame_ of costmap_ros_ if needed geometry_msgs::msg::PoseStamped goal_pose = goal->goal; - if (!costmap_ros_->getPoseInGlobalFrame(start) || - !costmap_ros_->getPoseInGlobalFrame(goal_pose)) { + if (!costmap_ros_->transformPoseToGlobalFrame(start, start) || + !costmap_ros_->transformPoseToGlobalFrame(goal->goal, goal_pose)) { RCLCPP_WARN( get_logger(), "Could not transform the start or goal pose in the costmap frame"); action_server_->terminate_current(); diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index c5e68d7cf94..7dafe77e718 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -36,10 +36,11 @@ bool getCurrentPose( tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map", const std::string robot_frame = "base_link", const double transform_timeout = 0.1); -bool getPoseInTargetFrame( - geometry_msgs::msg::PoseStamped & pose, +bool transformPoseInTargetFrame( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose, tf2_ros::Buffer & tf_buffer, const std::string target_frame, - const double transform_timeout = 0.1); + const double transform_timeout = 0.1); } // end namespace nav2_util diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index bab61d95f04..27f7d5b0aff 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -32,19 +32,21 @@ bool getCurrentPose( global_pose.header.frame_id = robot_frame; global_pose.header.stamp = rclcpp::Time(); - return getPoseInTargetFrame(global_pose, tf_buffer, global_frame, transform_timeout); + return transformPoseInTargetFrame( + global_pose, global_pose, tf_buffer, global_frame, transform_timeout); } -bool getPoseInTargetFrame( - geometry_msgs::msg::PoseStamped & pose, +bool transformPoseInTargetFrame( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose, tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout) { - static rclcpp::Logger logger = rclcpp::get_logger("getPoseInTargetFrame"); + static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame"); try { - pose = tf_buffer.transform( - pose, target_frame, + transformed_pose = tf_buffer.transform( + input_pose, target_frame, tf2::durationFromSec(transform_timeout)); return true; } catch (tf2::LookupException & ex) { @@ -66,7 +68,7 @@ bool getPoseInTargetFrame( } catch (tf2::TransformException & ex) { RCLCPP_ERROR( logger, "Failed to transform from %s to %s", - pose.header.frame_id.c_str(), target_frame.c_str()); + input_pose.header.frame_id.c_str(), target_frame.c_str()); } return false; diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index 5d5598a24da..bd277515ab0 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -30,5 +30,5 @@ TEST(RobotUtils, LookupExceptionError) tf2_ros::Buffer tf(node->get_clock()); ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1)); global_pose.header.frame_id = "base_link"; - ASSERT_FALSE(nav2_util::getPoseInTargetFrame(global_pose, tf, "map", 0.1)); + ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1)); } From 7be1f16cbb071d58cf5d23469bc2aa58c8b39d12 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 10 Mar 2021 10:58:40 +0100 Subject: [PATCH 5/5] code style --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 3 ++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 ++- nav2_planner/src/planner_server.cpp | 3 ++- nav2_util/src/robot_utils.cpp | 4 ++-- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 491341eba55..3d6011f4f17 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -146,7 +146,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @param transformed_pose pose transformed * @return True if the pose was transformed successfully, false otherwise */ - bool transformPoseToGlobalFrame(const geometry_msgs::msg::PoseStamped & input_pose, + bool transformPoseToGlobalFrame( + const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped & transformed_pose); /** @brief Returns costmap name */ diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 18b3b979fab..4c9c346ba43 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -535,7 +535,8 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) } bool -Costmap2DROS::transformPoseToGlobalFrame(const geometry_msgs::msg::PoseStamped & input_pose, +Costmap2DROS::transformPoseToGlobalFrame( + const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped & transformed_pose) { if (input_pose.header.frame_id == global_frame_) { diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 74ec6da06a4..123ffde0660 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -244,7 +244,8 @@ PlannerServer::computePlan() // Changing the start and goal pose frame to the global_frame_ of costmap_ros_ if needed geometry_msgs::msg::PoseStamped goal_pose = goal->goal; if (!costmap_ros_->transformPoseToGlobalFrame(start, start) || - !costmap_ros_->transformPoseToGlobalFrame(goal->goal, goal_pose)) { + !costmap_ros_->transformPoseToGlobalFrame(goal->goal, goal_pose)) + { RCLCPP_WARN( get_logger(), "Could not transform the start or goal pose in the costmap frame"); action_server_->terminate_current(); diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 27f7d5b0aff..615366de29d 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -33,14 +33,14 @@ bool getCurrentPose( global_pose.header.stamp = rclcpp::Time(); return transformPoseInTargetFrame( - global_pose, global_pose, tf_buffer, global_frame, transform_timeout); + global_pose, global_pose, tf_buffer, global_frame, transform_timeout); } bool transformPoseInTargetFrame( const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped & transformed_pose, tf2_ros::Buffer & tf_buffer, const std::string target_frame, - const double transform_timeout) + const double transform_timeout) { static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");