From 71f9a62d0f8dcb0267935d6c6068138b5afad3d5 Mon Sep 17 00:00:00 2001 From: ymd-stella Date: Thu, 14 May 2020 21:07:12 +0900 Subject: [PATCH] Use parameterized frames (ros-planning#1726) Signed-off-by: ymd-stella --- .../plugins/condition/goal_reached_condition.cpp | 6 +++++- .../include/nav2_costmap_2d/collision_checker.hpp | 4 +++- nav2_costmap_2d/src/collision_checker.cpp | 6 ++++-- .../include/nav2_recoveries/recovery.hpp | 4 ++++ nav2_recoveries/plugins/back_up.cpp | 4 ++-- nav2_recoveries/plugins/spin.cpp | 4 ++-- nav2_recoveries/src/recovery_server.cpp | 13 ++++++++++++- 7 files changed, 32 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c2cd02b8..03f14ed57c 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -60,6 +60,8 @@ class GoalReachedCondition : public BT::ConditionNode { node_ = config().blackboard->get("node"); node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); + node_->get_parameter_or("global_frame", global_frame_, "map"); + node_->get_parameter_or("robot_base_frame", robot_base_frame_, "base_link"); tf_ = config().blackboard->get>("tf_buffer"); initialized_ = true; @@ -70,7 +72,7 @@ class GoalReachedCondition : public BT::ConditionNode { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_)) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, global_frame_, robot_base_frame_)) { RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); return false; } @@ -105,6 +107,8 @@ class GoalReachedCondition : public BT::ConditionNode bool initialized_; double goal_reached_tol_; + std::string global_frame_; + std::string robot_base_frame_; }; } // namespace nav2_behavior_tree diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp index 13139e6cd7..0cfba2705c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp @@ -44,7 +44,8 @@ class CollisionChecker FootprintSubscriber & footprint_sub, tf2_ros::Buffer & tf, std::string name = "collision_checker", - std::string global_frame = "map"); + std::string global_frame = "map", + std::string robot_base_frame = "base_link"); ~CollisionChecker(); @@ -65,6 +66,7 @@ class CollisionChecker // Name used for logging std::string name_; std::string global_frame_; + std::string robot_base_frame_; tf2_ros::Buffer & tf_; CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp index 4e8d92c127..b0e6499621 100644 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ b/nav2_costmap_2d/src/collision_checker.cpp @@ -34,9 +34,11 @@ CollisionChecker::CollisionChecker( FootprintSubscriber & footprint_sub, tf2_ros::Buffer & tf, std::string name, - std::string global_frame) + std::string global_frame, + std::string robot_base_frame) : name_(name), global_frame_(global_frame), + robot_base_frame_(robot_base_frame), tf_(tf), costmap_sub_(costmap_sub), footprint_sub_(footprint_sub) @@ -173,7 +175,7 @@ void CollisionChecker::unorientFootprint( std::vector & reset_footprint) { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, tf_, global_frame_)) { + if (!nav2_util::getCurrentPose(current_pose, tf_, global_frame_, robot_base_frame_)) { throw CollisionCheckerException("Robot pose unavailable."); } diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 80606e90ff..54daea7dba 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -97,6 +97,8 @@ class Recovery : public nav2_core::Recovery tf_ = tf; node_->get_parameter("cycle_frequency", cycle_frequency_); + node_->get_parameter("odom_frame", odom_frame_); + node_->get_parameter("robot_base_frame", robot_base_frame_); action_server_ = std::make_shared( node_, recovery_name_, @@ -140,6 +142,8 @@ class Recovery : public nav2_core::Recovery double cycle_frequency_; double enabled_; + std::string odom_frame_; + std::string robot_base_frame_; void execute() { diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 9706e6e82b..70b5941ed9 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -53,7 +53,7 @@ Status BackUp::onRun(const std::shared_ptr command) command_x_ = command->target.x; command_speed_ = command->speed; - if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom")) { + if (!nav2_util::getCurrentPose(initial_pose_, *tf_, odom_frame_, robot_base_frame_)) { RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); return Status::FAILED; } @@ -64,7 +64,7 @@ Status BackUp::onRun(const std::shared_ptr command) Status BackUp::onCycleUpdate() { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, odom_frame_, robot_base_frame_)) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 9afd6a2ac4..24fa9bfb46 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -70,7 +70,7 @@ void Spin::onConfigure() Status Spin::onRun(const std::shared_ptr command) { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, odom_frame_, robot_base_frame_)) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } @@ -88,7 +88,7 @@ Status Spin::onRun(const std::shared_ptr command) Status Spin::onCycleUpdate() { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { + if (!nav2_util::getCurrentPose(current_pose, *tf_, odom_frame_, robot_base_frame_)) { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index ecd23d4481..9589293425 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -45,6 +45,13 @@ RecoveryServer::RecoveryServer() declare_parameter( "plugin_types", rclcpp::ParameterValue(plugin_types)); + + declare_parameter( + "odom_frame", + rclcpp::ParameterValue("odom")); + declare_parameter( + "robot_base_frame", + rclcpp::ParameterValue("base_link")); } @@ -71,8 +78,12 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) shared_from_this(), costmap_topic); footprint_sub_ = std::make_unique( shared_from_this(), footprint_topic, 1.0); + + std::string odom_frame, robot_base_frame; + this->get_parameter("odom_frame", odom_frame); + this->get_parameter("robot_base_frame", robot_base_frame); collision_checker_ = std::make_shared( - *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom"); + *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), odom_frame, robot_base_frame); this->get_parameter("plugin_names", plugin_names_); this->get_parameter("plugin_types", plugin_types_);