diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e02598195e1..7ca95d7f91d 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -33,8 +33,13 @@ class GoalReachedCondition : public BT::ConditionNode GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) - : BT::ConditionNode(condition_name, conf), initialized_(false) + : BT::ConditionNode(condition_name, conf), + initialized_(false), + global_frame_("map"), + robot_base_frame_("base_link") { + getInput("global_frame", global_frame_); + getInput("robot_base_frame", robot_base_frame_); } GoalReachedCondition() = delete; @@ -88,7 +93,9 @@ class GoalReachedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("goal", "Destination") + BT::InputPort("goal", "Destination"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("robot_base_frame", std::string("base_link"), "Robot base frame") }; } @@ -103,6 +110,8 @@ class GoalReachedCondition : public BT::ConditionNode bool initialized_; double goal_reached_tol_; + std::string global_frame_; + std::string robot_base_frame_; double transform_tolerance_; }; diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index 4105586a040..59e3e331e99 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_bringup/bringup/launch/navigation_launch.py @@ -114,7 +114,7 @@ def generate_launch_description(): node_executable='recoveries_server', node_name='recoveries_server', output='screen', - parameters=[{'use_sim_time': use_sim_time}], + parameters=[configured_params], remappings=remappings), Node( diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index b59a1e64937..d561cd7bea9 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -198,6 +198,22 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + plugin_names: ["spin", "back_up", "wait"] + plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + robot_state_publisher: ros__parameters: use_sim_time: True diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 2ec38a01566..57a142aba34 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -198,6 +198,22 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + plugin_names: ["spin", "back_up", "wait"] + plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + robot_state_publisher: ros__parameters: use_sim_time: True diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 6d50c03ce81..50738827bea 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -135,9 +135,9 @@ local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 + global_frame: odom robot_base_frame: base_link use_sim_time: True - global_frame: odom rolling_window: true width: 3 height: 3 @@ -186,8 +186,8 @@ global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 - robot_base_frame: base_link global_frame: map + robot_base_frame: base_link use_sim_time: True plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] @@ -245,6 +245,22 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + plugin_names: ["spin", "back_up", "wait"] + plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + robot_state_publisher: ros__parameters: use_sim_time: True diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index e4e374ce028..636a907f076 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -47,6 +47,7 @@ class CostmapTopicCollisionChecker tf2_ros::Buffer & tf, std::string name = "collision_checker", std::string global_frame = "map", + std::string robot_base_frame = "base_link", double transform_tolerance = 0.1); ~CostmapTopicCollisionChecker() = default; @@ -62,6 +63,7 @@ class CostmapTopicCollisionChecker // 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/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index e81f6ab764c..225e6ac3fb8 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -38,9 +38,11 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker( tf2_ros::Buffer & tf, std::string name, std::string global_frame, + std::string robot_base_frame, double transform_tolerance) : name_(name), global_frame_(global_frame), + robot_base_frame_(robot_base_frame), tf_(tf), costmap_sub_(costmap_sub), footprint_sub_(footprint_sub), @@ -107,7 +109,7 @@ void CostmapTopicCollisionChecker::unorientFootprint( { geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, tf_, global_frame_, "base_link", + current_pose, tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { throw CollisionCheckerException("Robot pose unavailable."); diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index a486f190a01..5583a19d4dc 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("global_frame", global_frame_); + node_->get_parameter("robot_base_frame", robot_base_frame_); node_->get_parameter("transform_tolerance", transform_tolerance_); action_server_ = std::make_shared( @@ -141,6 +143,8 @@ class Recovery : public nav2_core::Recovery double cycle_frequency_; double enabled_; + std::string global_frame_; + std::string robot_base_frame_; double transform_tolerance_; void execute() diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 2faaf030008..3f258402036 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -54,7 +54,10 @@ 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", "base_link", transform_tolerance_)) { + if (!nav2_util::getCurrentPose( + initial_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); return Status::FAILED; } @@ -65,7 +68,10 @@ 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", "base_link", transform_tolerance_)) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { 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 f96aaaaa742..e24be17d899 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -71,7 +71,10 @@ 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", "base_link", transform_tolerance_)) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } @@ -89,7 +92,10 @@ 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", "base_link", transform_tolerance_)) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { 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 3a40c4c7328..127d00a9941 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -22,7 +22,7 @@ namespace recovery_server { RecoveryServer::RecoveryServer() -: nav2_util::LifecycleNode("nav2_recoveries", "", true), +: LifecycleNode("recoveries_server", "", true), plugin_loader_("nav2_core", "nav2_core::Recovery") { declare_parameter( @@ -46,6 +46,12 @@ RecoveryServer::RecoveryServer() "plugin_types", rclcpp::ParameterValue(plugin_types)); + declare_parameter( + "global_frame", + rclcpp::ParameterValue(std::string("odom"))); + declare_parameter( + "robot_base_frame", + rclcpp::ParameterValue(std::string("base_link"))); declare_parameter( "transform_tolerance", rclcpp::ParameterValue(0.1)); @@ -63,8 +69,8 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - this->get_node_base_interface(), - this->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); transform_listener_ = std::make_shared(*tf_); @@ -76,11 +82,16 @@ 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 global_frame, robot_base_frame; + get_parameter("global_frame", global_frame); + get_parameter("robot_base_frame", robot_base_frame); collision_checker_ = std::make_shared( - *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom", transform_tolerance_); + *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), + global_frame, robot_base_frame, transform_tolerance_); - this->get_parameter("plugin_names", plugin_names_); - this->get_parameter("plugin_types", plugin_types_); + get_parameter("plugin_names", plugin_names_); + get_parameter("plugin_types", plugin_types_); loadRecoveryPlugins();