Skip to content

Commit c8e8b23

Browse files
SteveMacenskiredvinaaDylanDeCoeyer-Quimesisalexanderjyuen
authored
Jazzy Sync 4: Dec 13, 2024 (#4797)
* Pass IDLE to on_tick, use that for initialize condition (#4744) * Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa <[email protected]> * Fix battery sub creation bug Signed-off-by: redvinaa <[email protected]> --------- Signed-off-by: redvinaa <[email protected]> * nav2_costmap_2d: add missing default_value_ copy in Costmap2D operator= (#4753) default_value_ is an attribute of the Costmap2D class and should be copied along with the other attributes. Signed-off-by: Dylan De Coeyer <[email protected]> * nav2_costmap_2d plugin container layer (#4781) * updated CMakeLists.txt to include plugin_container_layer Signed-off-by: alexander <[email protected]> * added plugin container layer to costmap_plugins.xml Signed-off-by: alexander <[email protected]> * initial commit of plugin container layer Signed-off-by: alexander <[email protected]> * fixed plugin namespace Signed-off-by: alexander <[email protected]> * fixed message_filter Signed-off-by: alexander <[email protected]> * linting Signed-off-by: alexander <[email protected]> * modified addPlugin method to also take layer name Signed-off-by: alexander <[email protected]> * reverted default plugins to be empty, free costmap_ during layer destruction, changed addPlugin to take layer name as an argument Signed-off-by: alexander <[email protected]> * CMake changes to test plugin container layer Signed-off-by: alexander <[email protected]> * added helper method to add plugin container layer Signed-off-by: alexander <[email protected]> * added initial implementation of plugin container tests Signed-off-by: alexander <[email protected]> * added enable to dynamic params, removed unecessary comments, removed unecessary members Signed-off-by: alexander <[email protected]> * cleaned up and added additional tests Signed-off-by: alexander <[email protected]> * added Apache copyrights Signed-off-by: alexander <[email protected]> * linting for ament_cpplint Signed-off-by: alexander <[email protected]> * added example file for plugin_container_layer to nav2_bringup Signed-off-by: alexander <[email protected]> * removed unused rolling_window_ member variable Signed-off-by: alexander <[email protected]> * removed default plugins and plugin types Signed-off-by: alexander <[email protected]> * switched to using CombinationMethod enum, added updateWithMaxWithoutUnknownOverwrite case Signed-off-by: alexander <[email protected]> * removed combined_costmap_ Signed-off-by: alexander <[email protected]> * fixed layer naming and accomodating tests Signed-off-by: alexander <[email protected]> * removed nav2_params_plugin_container_layer.yaml Signed-off-by: alexander <[email protected]> * added more comprehensive checks for checking if layers are clearable Signed-off-by: alexander <[email protected]> * added dynamics parameter handling, fixed current_ setting, increased test coverage Signed-off-by: alexander <[email protected]> * removed unnecessary locks, added default value Signed-off-by: alexander <[email protected]> * removed unecessary resetMap Signed-off-by: alexander <[email protected]> * added layer resetting when reset method is called Signed-off-by: alexander <[email protected]> * swapped logic for isClearable Signed-off-by: alexander <[email protected]> * fixed breaking tests, removed unnecessary combined_costmap_ Signed-off-by: alexander <[email protected]> * consolidated initialization for loops Signed-off-by: alexander <[email protected]> * switched default_value_ to NO_INFORMATION Signed-off-by: alexander <[email protected]> * added clearArea function Signed-off-by: alexander <[email protected]> * added clearArea test Signed-off-by: alexander <[email protected]> * removed TODO message Signed-off-by: alexander <[email protected]> * removed constructor and destructors since they do nothing Signed-off-by: alexander <[email protected]> * added check on costmap layer to see if it is clearable first Signed-off-by: alexander <[email protected]> * fixed tests for clearing functionality Signed-off-by: alexander <[email protected]> * added try catch around initialization of plugins Signed-off-by: alexander <[email protected]> * fixed indents Signed-off-by: alexander <[email protected]> --------- Signed-off-by: alexander <[email protected]> * bumping to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski <[email protected]> * Fix backporting error: renamed header files Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: redvinaa <[email protected]> Signed-off-by: Dylan De Coeyer <[email protected]> Signed-off-by: alexander <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Vince Reda <[email protected]> Co-authored-by: DylanDeCoeyer-Quimesis <[email protected]> Co-authored-by: alexanderjyuen <[email protected]>
1 parent 2da6aa8 commit c8e8b23

File tree

77 files changed

+1185
-131
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

77 files changed

+1185
-131
lines changed

‎nav2_amcl/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>nav2_amcl</name>
5-
<version>1.3.3</version>
5+
<version>1.3.4</version>
66
<description>
77
<p>
88
amcl is a probabilistic localization system for a robot moving in

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -191,15 +191,15 @@ class BtActionNode : public BT::ActionNodeBase
191191
{
192192
// first step to be done only at the beginning of the Action
193193
if (!BT::isStatusActive(status())) {
194-
// setting the status to RUNNING to notify the BT Loggers (if any)
195-
setStatus(BT::NodeStatus::RUNNING);
196-
197194
// reset the flag to send the goal or not, allowing the user the option to set it in on_tick
198195
should_send_goal_ = true;
199196

200197
// user defined callback, may modify "should_send_goal_".
201198
on_tick();
202199

200+
// setting the status to RUNNING to notify the BT Loggers (if any)
201+
setStatus(BT::NodeStatus::RUNNING);
202+
203203
if (!should_send_goal_) {
204204
return BT::NodeStatus::FAILURE;
205205
}

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
8686

8787
private:
8888
bool is_recovery_;
89-
bool initialized_;
9089
};
9190

9291
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -84,9 +84,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
8484
"error_code_id", "The back up behavior server error code")
8585
});
8686
}
87-
88-
private:
89-
bool initialized_;
9087
};
9188

9289
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase
6161
double transform_tolerance_;
6262
std::shared_ptr<tf2_ros::Buffer> tf_;
6363
std::string robot_base_frame_;
64-
bool initialized_;
6564
};
6665

6766
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,6 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
8686

8787
private:
8888
bool is_recovery_;
89-
bool initialized_;
9089
};
9190

9291
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -61,9 +61,6 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
6161
BT::InputPort<double>("wait_duration", 1.0, "Wait time")
6262
});
6363
}
64-
65-
private:
66-
bool initialized_;
6764
};
6865

6966
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,6 @@ class DistanceTraveledCondition : public BT::ConditionNode
8080
double distance_;
8181
double transform_tolerance_;
8282
std::string global_frame_, robot_base_frame_;
83-
bool initialized_;
8483
};
8584

8685
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode
9090
rclcpp::Node::SharedPtr node_;
9191
std::shared_ptr<tf2_ros::Buffer> tf_;
9292

93-
bool initialized_;
9493
double goal_reached_tol_;
9594
double transform_tolerance_;
9695
std::string robot_base_frame_;

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,6 @@ class IsBatteryLowCondition : public BT::ConditionNode
8686
double min_battery_;
8787
bool is_voltage_;
8888
bool is_battery_low_;
89-
bool initialized_;
9089
};
9190

9291
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,6 @@ class IsPathValidCondition : public BT::ConditionNode
7373
// The timeout value while waiting for a responce from the
7474
// is path valid service
7575
std::chrono::milliseconds server_timeout_;
76-
bool initialized_;
7776
};
7877

7978
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,6 @@ class TimeExpiredCondition : public BT::ConditionNode
6868
rclcpp::Node::SharedPtr node_;
6969
rclcpp::Time start_;
7070
double period_;
71-
bool initialized_;
7271
};
7372

7473
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,6 @@ class TransformAvailableCondition : public BT::ConditionNode
8080

8181
std::string child_frame_;
8282
std::string parent_frame_;
83-
bool initialized_;
8483
};
8584

8685
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,6 @@ class RateController : public BT::DecoratorNode
6464
std::chrono::time_point<std::chrono::high_resolution_clock> start_;
6565
double period_;
6666
bool first_time_;
67-
bool initialized_;
6867
};
6968

7069
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>nav2_behavior_tree</name>
5-
<version>1.3.3</version>
5+
<version>1.3.4</version>
66
<description>Nav2 behavior tree wrappers, nodes, and utilities</description>
77
<maintainer email="[email protected]">Michael Jeronimo</maintainer>
88
<maintainer email="[email protected]">Carlos Orduno</maintainer>

‎nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -24,8 +24,7 @@ AssistedTeleopAction::AssistedTeleopAction(
2424
const std::string & xml_tag_name,
2525
const std::string & action_name,
2626
const BT::NodeConfiguration & conf)
27-
: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf),
28-
initialized_(false)
27+
: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
2928
{}
3029

3130
void AssistedTeleopAction::initialize()
@@ -36,12 +35,11 @@ void AssistedTeleopAction::initialize()
3635

3736
// Populate the input message
3837
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
39-
initialized_ = true;
4038
}
4139

4240
void AssistedTeleopAction::on_tick()
4341
{
44-
if (!initialized_) {
42+
if (!BT::isStatusActive(status())) {
4543
initialize();
4644
}
4745

‎nav2_behavior_tree/plugins/action/back_up_action.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -24,8 +24,7 @@ BackUpAction::BackUpAction(
2424
const std::string & xml_tag_name,
2525
const std::string & action_name,
2626
const BT::NodeConfiguration & conf)
27-
: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf),
28-
initialized_(false)
27+
: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
2928
{
3029
}
3130

@@ -44,12 +43,11 @@ void nav2_behavior_tree::BackUpAction::initialize()
4443
goal_.target.z = 0.0;
4544
goal_.speed = speed;
4645
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
47-
initialized_ = true;
4846
}
4947

5048
void BackUpAction::on_tick()
5149
{
52-
if (!initialized_) {
50+
if (!BT::isStatusActive(status())) {
5351
initialize();
5452
}
5553

‎nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

+2-5
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,7 @@ RemovePassedGoals::RemovePassedGoals(
2828
const std::string & name,
2929
const BT::NodeConfiguration & conf)
3030
: BT::ActionNodeBase(name, conf),
31-
viapoint_achieved_radius_(0.5),
32-
initialized_(false)
31+
viapoint_achieved_radius_(0.5)
3332
{}
3433

3534
void RemovePassedGoals::initialize()
@@ -46,9 +45,7 @@ void RemovePassedGoals::initialize()
4645

4746
inline BT::NodeStatus RemovePassedGoals::tick()
4847
{
49-
setStatus(BT::NodeStatus::RUNNING);
50-
51-
if (!initialized_) {
48+
if (!BT::isStatusActive(status())) {
5249
initialize();
5350
}
5451

‎nav2_behavior_tree/plugins/action/spin_action.cpp

+2-7
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,7 @@ SpinAction::SpinAction(
2323
const std::string & xml_tag_name,
2424
const std::string & action_name,
2525
const BT::NodeConfiguration & conf)
26-
: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf),
27-
initialized_(false)
28-
{
29-
}
26+
: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf) {}
3027

3128
void SpinAction::initialize()
3229
{
@@ -37,13 +34,11 @@ void SpinAction::initialize()
3734
goal_.target_yaw = dist;
3835
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
3936
getInput("is_recovery", is_recovery_);
40-
41-
initialized_ = true;
4237
}
4338

4439
void SpinAction::on_tick()
4540
{
46-
if (!initialized_) {
41+
if (!BT::isStatusActive(status())) {
4742
initialize();
4843
}
4944

‎nav2_behavior_tree/plugins/action/wait_action.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,7 @@ WaitAction::WaitAction(
2525
const std::string & xml_tag_name,
2626
const std::string & action_name,
2727
const BT::NodeConfiguration & conf)
28-
: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf),
29-
initialized_(false)
28+
: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
3029
{
3130
}
3231

@@ -42,12 +41,11 @@ void WaitAction::initialize()
4241
}
4342

4443
goal_.time = rclcpp::Duration::from_seconds(duration);
45-
initialized_ = true;
4644
}
4745

4846
void WaitAction::on_tick()
4947
{
50-
if (!initialized_) {
48+
if (!BT::isStatusActive(status())) {
5149
initialize();
5250
}
5351

‎nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,7 @@ DistanceTraveledCondition::DistanceTraveledCondition(
2929
const BT::NodeConfiguration & conf)
3030
: BT::ConditionNode(condition_name, conf),
3131
distance_(1.0),
32-
transform_tolerance_(0.1),
33-
initialized_(false)
32+
transform_tolerance_(0.1)
3433
{
3534
}
3635

@@ -46,12 +45,11 @@ void DistanceTraveledCondition::initialize()
4645
node_, "global_frame", this);
4746
robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
4847
node_, "robot_base_frame", this);
49-
initialized_ = true;
5048
}
5149

5250
BT::NodeStatus DistanceTraveledCondition::tick()
5351
{
54-
if (!initialized_) {
52+
if (!BT::isStatusActive(status())) {
5553
initialize();
5654
}
5755

‎nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

+2-5
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,7 @@ namespace nav2_behavior_tree
2727
GoalReachedCondition::GoalReachedCondition(
2828
const std::string & condition_name,
2929
const BT::NodeConfiguration & conf)
30-
: BT::ConditionNode(condition_name, conf),
31-
initialized_(false)
30+
: BT::ConditionNode(condition_name, conf)
3231
{
3332
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
3433

@@ -52,13 +51,11 @@ void GoalReachedCondition::initialize()
5251
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
5352

5453
node_->get_parameter("transform_tolerance", transform_tolerance_);
55-
56-
initialized_ = true;
5754
}
5855

5956
BT::NodeStatus GoalReachedCondition::tick()
6057
{
61-
if (!initialized_) {
58+
if (!BT::isStatusActive(status())) {
6259
initialize();
6360
}
6461

‎nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

+22-17
Original file line numberDiff line numberDiff line change
@@ -27,35 +27,40 @@ IsBatteryLowCondition::IsBatteryLowCondition(
2727
battery_topic_("/battery_status"),
2828
min_battery_(0.0),
2929
is_voltage_(false),
30-
is_battery_low_(false),
31-
initialized_(false)
30+
is_battery_low_(false)
3231
{
3332
}
3433

3534
void IsBatteryLowCondition::initialize()
3635
{
3736
getInput("min_battery", min_battery_);
38-
getInput("battery_topic", battery_topic_);
37+
std::string battery_topic_new;
38+
getInput("battery_topic", battery_topic_new);
3939
getInput("is_voltage", is_voltage_);
40-
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
41-
callback_group_ = node_->create_callback_group(
42-
rclcpp::CallbackGroupType::MutuallyExclusive,
43-
false);
44-
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
4540

46-
rclcpp::SubscriptionOptions sub_option;
47-
sub_option.callback_group = callback_group_;
48-
battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
49-
battery_topic_,
50-
rclcpp::SystemDefaultsQoS(),
51-
std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
52-
sub_option);
53-
initialized_ = true;
41+
// Only create a new subscriber if the topic has changed or subscriber is empty
42+
if (battery_topic_new != battery_topic_ || !battery_sub_) {
43+
battery_topic_ = battery_topic_new;
44+
45+
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
46+
callback_group_ = node_->create_callback_group(
47+
rclcpp::CallbackGroupType::MutuallyExclusive,
48+
false);
49+
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
50+
51+
rclcpp::SubscriptionOptions sub_option;
52+
sub_option.callback_group = callback_group_;
53+
battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
54+
battery_topic_,
55+
rclcpp::SystemDefaultsQoS(),
56+
std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
57+
sub_option);
58+
}
5459
}
5560

5661
BT::NodeStatus IsBatteryLowCondition::tick()
5762
{
58-
if (!initialized_) {
63+
if (!BT::isStatusActive(status())) {
5964
initialize();
6065
}
6166

‎nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,7 @@ namespace nav2_behavior_tree
2323
IsPathValidCondition::IsPathValidCondition(
2424
const std::string & condition_name,
2525
const BT::NodeConfiguration & conf)
26-
: BT::ConditionNode(condition_name, conf),
27-
initialized_(false)
26+
: BT::ConditionNode(condition_name, conf)
2827
{
2928
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
3029
client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
@@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition(
3534
void IsPathValidCondition::initialize()
3635
{
3736
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
38-
initialized_ = true;
3937
}
4038

4139
BT::NodeStatus IsPathValidCondition::tick()
4240
{
43-
if (!initialized_) {
41+
if (!BT::isStatusActive(status())) {
4442
initialize();
4543
}
4644

0 commit comments

Comments
 (0)