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.

Sign up for GitHub

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

Jump to bottom

Use parameterized frames #1729

Closed
wants to merge 1 commit into from
Closed

Use parameterized frames #1729

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
6 changes: 5 additions & 1 deletion nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ class GoalReachedCondition : public BT::ConditionNode
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
node_->get_parameter_or<std::string>("global_frame", global_frame_, "map");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How are you planning on getting these parameters?

It seems like these would better be input ports for the BT node (see this for example https://github.com/ros-planning/navigation2/blob/71f9a62d0f8dcb0267935d6c6068138b5afad3d5/nav2_behavior_tree/plugins/action/spin_action.cpp#L49) with a default value to map and base_link. Then if you wanted to change, you change the frames in the BT XML file.

I think this could work as well, but you'd need to figure out what namespace this parameter appears in to add to your config file, which actually sounds like the best situation. Figure out where this parameterization is going to be and then add it to our nav2_params.yaml file.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not familiar with nav2_behavior_tree so it's going to take a bit of time.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So I'd recommend just looking at this spin_dist as a literal example. All you need to do is add a string templated BT::InputPort to the provide input ports function. The order of args is variable name, default value, description string. Then in the constructor of the node use getInput to get the input of the 2 variables to store. Then you're done!

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ymd-stella you can also use this as a direct guide: #1741 as @naiveHobo did this for the other controller based on your request.

node_->get_parameter_or<std::string>("robot_base_frame", robot_base_frame_, "base_link");
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");

initialized_ = true;
Expand All @@ -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;
}
Expand Down Expand Up @@ -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
Expand Down
4 changes: 3 additions & 1 deletion nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand All @@ -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_;
Expand Down
6 changes: 4 additions & 2 deletions nav2_costmap_2d/src/collision_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -173,7 +175,7 @@ void CollisionChecker::unorientFootprint(
std::vector<geometry_msgs::msg::Point> & 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.");
}

Expand Down
4 changes: 4 additions & 0 deletions nav2_recoveries/include/nav2_recoveries/recovery.hpp
View file
Open in desktop
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("odom_frame", odom_frame_);
node_->get_parameter("robot_base_frame", robot_base_frame_);

action_server_ = std::make_shared<ActionServer>(
node_, recovery_name_,
Expand Down Expand Up @@ -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()
{
Expand Down
4 changes: 2 additions & 2 deletions nav2_recoveries/plugins/back_up.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ 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")) {
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;
}
Expand All @@ -64,7 +64,7 @@ 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")) {
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;
}
Expand Down
4 changes: 2 additions & 2 deletions nav2_recoveries/plugins/spin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ 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")) {
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;
}
Expand All @@ -88,7 +88,7 @@ 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")) {
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;
}
Expand Down
13 changes: 12 additions & 1 deletion nav2_recoveries/src/recovery_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,13 @@ RecoveryServer::RecoveryServer()
declare_parameter(
"plugin_types",
rclcpp::ParameterValue(plugin_types));

declare_parameter(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please add these to our nav2_params.yaml file (and the 2 multi-robot param example files) and verify changing them works properly.

"odom_frame",
rclcpp::ParameterValue("odom"));
declare_parameter(
"robot_base_frame",
rclcpp::ParameterValue("base_link"));
}


Expand All @@ -71,8 +78,12 @@ 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 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<nav2_costmap_2d::CollisionChecker>(
*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_);
Expand Down