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

Create subscriber on constructor #4859

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
+355 −122
Open

Create subscriber on constructor #4859

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
cd9c4e2
Create subscriber on constructor
Tacha-S Jan 16, 2025
c65bdfe
Get BT input topic name
Tacha-S Jan 17, 2025
99c1359
Add createROSInterfaces()
Tacha-S Jan 31, 2025
12249d0
Add initialize()
Tacha-S Feb 5, 2025
ad45545
Fix bug
Tacha-S Feb 5, 2025
e981817
Call initialize where input port updates occur
Tacha-S Feb 6, 2025
e420aeb
Call initialize() on constructor
Tacha-S Feb 13, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 19 additions & 11 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,12 +50,6 @@ class BtServiceNode : public BT::ActionNodeBase
: BT::ActionNodeBase(service_node_name, conf), service_name_(service_name), service_node_name_(
service_node_name)
{
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

// Get the required items from the blackboard
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
Expand All @@ -67,11 +61,7 @@ class BtServiceNode : public BT::ActionNodeBase
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);

// Now that we have node_ to use, create the service client for this BT service
getInput("service_name", service_name_);
service_client_ = node_->create_client<ServiceT>(
service_name_,
rclcpp::SystemDefaultsQoS(),
callback_group_);
createROSInterfaces();
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

// Make a request for the service without parameter
request_ = std::make_shared<typename ServiceT::Request>();
Expand Down Expand Up @@ -101,6 +91,24 @@ class BtServiceNode : public BT::ActionNodeBase
{
}

/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces()
{
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

getInput("service_name", service_name_);
service_client_ = node_->create_client<ServiceT>(
service_name_,
rclcpp::SystemDefaultsQoS(),
callback_group_);
}

/**
* @brief Any subclass of BtServiceNode that accepts parameters must provide a
* providedPorts method and call providedBasicPorts in it.
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,11 @@ class ControllerSelector : public BT::SyncActionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Function to perform some user-defined operation on tick
*/
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,11 @@ class GoalCheckerSelector : public BT::SyncActionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Function to perform some user-defined operation on tick
*/
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,11 @@ class PlannerSelector : public BT::SyncActionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Function to perform some user-defined operation on tick
*/
Expand Down
5 changes: 5 additions & 0 deletions ...ehavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,11 @@ class ProgressCheckerSelector : public BT::SyncActionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Function to perform some user-defined operation on tick
*/
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,11 @@ class SmootherSelector : public BT::SyncActionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Function to perform some user-defined operation on tick
*/
Expand Down
5 changes: 5 additions & 0 deletions ...avior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,11 @@ class IsBatteryChargingCondition : public BT::ConditionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Callback function for battery topic
* @param msg Shared pointer to sensor_msgs::msg::BatteryState message
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,11 @@ class IsBatteryLowCondition : public BT::ConditionNode
*/
void initialize();

/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
Expand Down
4 changes: 4 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,10 @@ class IsPathValidCondition : public BT::ConditionNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();
rclcpp::Node::SharedPtr node_;
rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
// The timeout value while waiting for a response from the
Expand Down
4 changes: 4 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,10 @@ class IsStuckCondition : public BT::ConditionNode
static BT::PortsList providedPorts() {return {};}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
Expand Down
4 changes: 4 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,10 @@ class GoalUpdater : public BT::DecoratorNode
}

private:
/**
* @brief Function to create ROS interfaces
*/
void createROSInterfaces();
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/action/controller_selector_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,11 @@ ControllerSelector::ControllerSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
createROSInterfaces();
}

void ControllerSelector::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,11 @@ GoalCheckerSelector::GoalCheckerSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
createROSInterfaces();
}

void GoalCheckerSelector::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");

Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/action/planner_selector_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,11 @@ PlannerSelector::PlannerSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
createROSInterfaces();
}

void PlannerSelector::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,11 @@ ProgressCheckerSelector::ProgressCheckerSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
createROSInterfaces();
}

void ProgressCheckerSelector::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");

Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,11 @@ SmootherSelector::SmootherSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
createROSInterfaces();
}

void SmootherSelector::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,11 @@ IsBatteryChargingCondition::IsBatteryChargingCondition(
: BT::ConditionNode(condition_name, conf),
battery_topic_("/battery_status"),
is_battery_charging_(false)
{
createROSInterfaces();
}

void IsBatteryChargingCondition::createROSInterfaces()
{
getInput("battery_topic", battery_topic_);
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
Expand Down
10 changes: 8 additions & 2 deletions nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,19 +29,25 @@ IsBatteryLowCondition::IsBatteryLowCondition(
is_voltage_(false),
is_battery_low_(false)
{
createROSInterfaces();
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Member

Choose a reason for hiding this comment

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

Shouldn't we do initialize() here because initialize gets the input ports

Copy link
Member

Choose a reason for hiding this comment

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

In general, should we change them all from createROSInterfaces() in the constructor to initalize() so that if there are other non-ros-interface making initialization steps that might be important for the interface definitions, we perform them here too?

Copy link
Author

Choose a reason for hiding this comment

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

I think consistently calling initialize() in the constructor is a good idea, and I agree with that approach.
However, limiting createROSInterfaces() to only within initialize() and not calling either createROSInterfaces() or initialize() from the constructor would not resolve the issue mentioned in the issue, where the behavior during the first execution becomes undefined. Therefore, I would like to avoid this.

Copy link
Member

@SteveMacenski SteveMacenski Feb 11, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

I think consistently calling initialize() in the constructor is a good idea, and I agree with that approach.

OK! I see you updated that in this case on battery low, but not in the other cases. Can you update all the BT nodes for that change so they're all consistent? I think with that, this is ready to merge.

However, limiting createROSInterfaces() to only within initialize() and not calling either createROSInterfaces() or initialize() from the constructor would not resolve the issue mentioned in the issue, where the behavior during the first execution becomes undefined. Therefore, I would like to avoid this.

I'm saying that we should call initialize() in the constructor and also in the pattern below we've added to all of the tick()'s. Shouldn't that handle the situation?

  if (!BT::isStatusActive(status())) {
    initialize();
  }

I'm basically just saying I want you to replace createROSInterfaces() in the constructor with initialize(), who itself calls createROSInterfaces(). That way, if there are additional steps to initialization beyond just creating ROS interfaces, we call all of that stuff in the constructor as well as on the first tick() so the behavior is totally consistent between them.

Basically, use initialize() in both locations and createROSInterfaces() should only ever be called by initialize() and no one else as a general design pattern across all nodes.

}

void IsBatteryLowCondition::initialize()
{
getInput("min_battery", min_battery_);
getInput("is_voltage", is_voltage_);

createROSInterfaces();
}

void IsBatteryLowCondition::createROSInterfaces()
{
std::string battery_topic_new;
getInput("battery_topic", battery_topic_new);
getInput("is_voltage", is_voltage_);

// Only create a new subscriber if the topic has changed or subscriber is empty
if (battery_topic_new != battery_topic_ || !battery_sub_) {
battery_topic_ = battery_topic_new;

node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
Expand Down
9 changes: 7 additions & 2 deletions nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,7 @@ IsPathValidCondition::IsPathValidCondition(
const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
createROSInterfaces();

server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
}
Expand All @@ -36,6 +35,12 @@ void IsPathValidCondition::initialize()
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
}

void IsPathValidCondition::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
}

BT::NodeStatus IsPathValidCondition::tick()
{
if (!BT::isStatusActive(status())) {
Expand Down
27 changes: 16 additions & 11 deletions nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,22 @@ IsStuckCondition::IsStuckCondition(
odom_history_size_(10),
current_accel_(0.0),
brake_accel_limit_(-10.0)
{
createROSInterfaces();

RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node");

RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting on odometry");
}

IsStuckCondition::~IsStuckCondition()
{
RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
callback_group_executor_.cancel();
callback_group_executor_thread.join();
}

void IsStuckCondition::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
Expand All @@ -45,17 +61,6 @@ IsStuckCondition::IsStuckCondition(
rclcpp::SystemDefaultsQoS(),
std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1),
sub_option);

RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node");

RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting on odometry");
}

IsStuckCondition::~IsStuckCondition()
{
RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
callback_group_executor_.cancel();
callback_group_executor_thread.join();
}

void IsStuckCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)
Expand Down
5 changes: 5 additions & 0 deletions nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,11 @@ GoalUpdater::GoalUpdater(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::DecoratorNode(name, conf)
{
createROSInterfaces();
}

void GoalUpdater::createROSInterfaces()
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
Expand Down
Loading