Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Parameterize frame IDs #1742

Merged
13 changes: 11 additions & 2 deletions nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -88,7 +93,9 @@ class GoalReachedCondition : public BT::ConditionNode
static BT::PortsList providedPorts()
{
return {
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination")
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
};
}

Expand All @@ -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_;
};

Expand Down
2 changes: 1 addition & 1 deletion nav2_bringup/bringup/launch/navigation_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
16 changes: 16 additions & 0 deletions nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
16 changes: 16 additions & 0 deletions nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
20 changes: 18 additions & 2 deletions nav2_bringup/bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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"]
Expand Down Expand Up @@ -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
naiveHobo marked this conversation as resolved.
Show resolved Hide resolved
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
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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_;
Expand Down
4 changes: 3 additions & 1 deletion nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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.");
Expand Down
4 changes: 4 additions & 0 deletions nav2_recoveries/include/nav2_recoveries/recovery.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<ActionServer>(
Expand Down Expand Up @@ -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()
Expand Down
10 changes: 8 additions & 2 deletions nav2_recoveries/plugins/back_up.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,10 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> 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;
}
Expand All @@ -65,7 +68,10 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> 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;
}
Expand Down
10 changes: 8 additions & 2 deletions nav2_recoveries/plugins/spin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,10 @@ void Spin::onConfigure()
Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> 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;
}
Expand All @@ -89,7 +92,10 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> 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;
}
Expand Down
23 changes: 17 additions & 6 deletions nav2_recoveries/src/recovery_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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));
Expand All @@ -63,8 +69,8 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)

tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
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<tf2_ros::TransformListener>(*tf_);

Expand All @@ -76,11 +82,16 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
shared_from_this(), costmap_topic);
footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
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<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*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();

Expand Down