diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 49574999c4..eef3ad2c86 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -238,7 +238,7 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena try { tree_ = bt_->createTreeFromFile(filename, blackboard_); for (auto & blackboard : tree_.blackboard_stack) { - blackboard->set("node", client_node_); + blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index 4bb9905759..cbb5606d8b 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -59,7 +59,7 @@ class AssistedTeleopActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -71,8 +71,8 @@ class AssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index 40f6f65c88..a9261fe4b8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -56,7 +56,7 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 85e3d3764d..9df8b3da15 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -59,7 +59,7 @@ class BackUpActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -71,8 +71,8 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index 99e6eb1b45..c7f379a64c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -55,7 +55,7 @@ class CancelBackUpActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index a283dee33e..ec3aa03a07 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -140,14 +140,14 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("sequence", result_.result->sequence); return BT::NodeStatus::SUCCESS; } BT::NodeStatus on_cancelled() override { - config().blackboard->set>("sequence", result_.result->sequence); - config().blackboard->set("on_cancelled_triggered", true); + config().blackboard->set("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); return BT::NodeStatus::SUCCESS; } @@ -170,12 +170,12 @@ class BTActionNodeTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("wait_for_service_timeout", 1000ms); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("on_cancelled_triggered", false); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("on_cancelled_triggered", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -303,7 +303,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; - config_->blackboard->set("on_cancelled_triggered", false); + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -382,7 +382,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; - config_->blackboard->set("on_cancelled_triggered", false); + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -456,7 +456,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // reset state variable ticks = 0; - config_->blackboard->set("on_cancelled_triggered", false); + config_->blackboard->set("on_cancelled_triggered", false); result = BT::NodeStatus::RUNNING; // main BT execution loop diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index 88173e8420..e21f9802d7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -44,7 +44,7 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -56,8 +56,8 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); factory_->registerNodeType("ClearEntireCostmap"); } @@ -133,7 +133,7 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -145,8 +145,8 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); factory_->registerNodeType( "ClearCostmapExceptRegion"); @@ -228,7 +228,7 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -240,8 +240,8 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); factory_->registerNodeType( "ClearCostmapAroundRobot"); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index ec619b0120..001c3de67f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -67,7 +67,7 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -79,7 +79,7 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index d726186cd3..f1da97cb25 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -65,7 +65,7 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -77,7 +77,7 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 290c9133a5..7be55c08ae 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -55,7 +55,7 @@ class CancelControllerActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 73a1510a1e..8c98db78b3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -38,7 +38,7 @@ class ControllerSelectorTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 06478df588..c4439f1e1c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -60,7 +60,7 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -72,7 +72,7 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index 5378123a46..762351bdd1 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -56,7 +56,7 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index c4fcc57519..fea7a2b2b9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -58,7 +58,7 @@ class FollowPathActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -70,7 +70,7 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -130,7 +130,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) nav_msgs::msg::Path path; path.poses.resize(1); path.poses[0].pose.position.x = 1.0; - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -150,7 +150,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) // set new goal path.poses[0].pose.position.x = -2.5; - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index e797dbd4cf..1fd0e286cb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -38,7 +38,7 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 5fa2c9aa03..90cdfbbb0c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -61,7 +61,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -73,9 +73,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); std::vector poses; - config_->blackboard->set>( + config_->blackboard->set( "goals", poses); BT::NodeBuilder builder = @@ -136,7 +136,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) poses.resize(1); poses[0].pose.position.x = -2.5; poses[0].pose.orientation.x = 1.0; - config_->blackboard->set>("goals", poses); + config_->blackboard->set("goals", poses); // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 264b775a68..5b3764befb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -59,7 +59,7 @@ class NavigateToPoseActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -71,7 +71,7 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -145,7 +145,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) // set new goal pose.pose.position.x = -2.5; pose.pose.orientation.x = 1.0; - config_->blackboard->set("goal", pose); + config_->blackboard->set("goal", pose); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index 8c76e1440d..b8851d81c3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -38,7 +38,7 @@ class PlannerSelectorTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 2f7a36e7fa..73e72fa764 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -44,7 +44,7 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -56,7 +56,7 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( "ReinitializeGlobalLocalization"); diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 9e1aa32f8e..7bfda58e54 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -43,10 +43,10 @@ class RemovePassedGoalsTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); - config_->blackboard->set>( + config_->blackboard->set( "tf_buffer", transform_handler_->getBuffer()); diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index c2e4389021..6460a5d9b9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -58,7 +58,7 @@ class SmoothPathActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -70,7 +70,7 @@ class SmoothPathActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -146,7 +146,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) pose.pose.position.x = -2.5; pose.pose.orientation.x = 1.0; path.poses.push_back(pose); - config_->blackboard->set("unsmoothed_path", path); + config_->blackboard->set("unsmoothed_path", path); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index d3516aba19..de73cc4fe5 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -38,7 +38,7 @@ class SmootherSelectorTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique(name, config); diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index a6d9f36d57..e1285d28a9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -59,7 +59,7 @@ class SpinActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -71,8 +71,8 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index ea3b01b983..8cc3083eb3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -55,7 +55,7 @@ class CancelSpinActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 5540b7b645..9ca640d47d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -41,7 +41,7 @@ class TruncatePathTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index f54250a2b6..2e9fb8b168 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -50,7 +50,7 @@ class WaitActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( @@ -62,8 +62,8 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); - config_->blackboard->set("initial_pose_received", false); - config_->blackboard->set("number_recoveries", 0); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index 0773c72c6d..ebce2f6341 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -55,7 +55,7 @@ class CancelWaitActionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); config_->blackboard->set( diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index 46af6d5c6e..79fa1e08c2 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -36,7 +36,7 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index da27d33e47..a39a414089 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -37,7 +37,7 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp index f8142b9ad4..9c022359a1 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp @@ -56,7 +56,7 @@ class IsPathValidTestFixture : public ::testing::Test factory_ = std::make_shared(); config_ = new BT::NodeConfiguration(); config_->blackboard = BT::Blackboard::create(); - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp index 8885821bcc..2f47ab826a 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -34,7 +34,7 @@ class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::Behavio node_ = std::make_shared("test_path_expiring_condition"); config_ = new BT::NodeConfiguration(); config_->blackboard = BT::Blackboard::create(); - config_->blackboard->set("node", node_); + config_->blackboard->set("node", node_); bt_node_ = std::make_shared( "time_expired", *config_); } @@ -78,7 +78,7 @@ TEST_F(PathExpiringTimerConditionTestFixture, test_behavior) pose.pose.position.x = 1.0; path.poses.push_back(pose); - config_->blackboard->set("path", path); + config_->blackboard->set("path", path); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); rclcpp::sleep_for(1500ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 9dc6d9b2a4..2b21fd416c 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -36,10 +36,10 @@ class TransformAvailableConditionTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); - config_->blackboard->set>( + config_->blackboard->set( "tf_buffer", transform_handler_->getBuffer()); config_->blackboard->set( @@ -48,7 +48,7 @@ class TransformAvailableConditionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); } static void TearDownTestCase() diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index 7fc4a21e48..b33e9e3a8b 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -35,7 +35,7 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree std::vector poses1; poses1.push_back(goal1); config_->blackboard->set("goal", goal1); - config_->blackboard->set>("goals", poses1); + config_->blackboard->set("goals", poses1); bt_node_ = std::make_shared( "goal_updated_controller", *config_); @@ -86,7 +86,7 @@ TEST_F(GoalUpdatedControllerTestFixture, test_behavior) EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); // tick again with updated goals, dummy node should be ticked - config_->blackboard->set>("goals", poses2); + config_->blackboard->set("goals", poses2); dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index cc6db706f2..221371a414 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -40,7 +40,7 @@ class GoalUpdaterTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp index 8dfec28537..0f08bafe69 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -42,7 +42,7 @@ class PathLongerOnApproachTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); @@ -106,7 +106,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // Assuming distance between waypoints to be 1.5m new_path.poses[i].pose.position.x = 1.5 * i; } - config_->blackboard->set("path", new_path); + config_->blackboard->set("path", new_path); tree_->rootNode()->executeTick(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -132,7 +132,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // Assuming distance between waypoints to be 3.0m old_path.poses[i - 1].pose.position.x = 3.0 * i; } - config_->blackboard->set("path", old_path); + config_->blackboard->set("path", old_path); tree_->rootNode()->executeTick(); // set new path on blackboard @@ -141,7 +141,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // Assuming distance between waypoints to be 1.5m new_path.poses[i].pose.position.x = 1.5 * i; } - config_->blackboard->set("path", new_path); + config_->blackboard->set("path", new_path); tree_->rootNode()->executeTick(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 44193b62af..26113b2699 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -35,7 +35,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi void SetUp() { odom_smoother_ = std::make_shared(node_); - config_->blackboard->set>( + config_->blackboard->set( "odom_smoother", odom_smoother_); // NOLINT geometry_msgs::msg::PoseStamped goal; @@ -43,7 +43,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi config_->blackboard->set("goal", goal); std::vector fake_poses; - config_->blackboard->set>("goals", fake_poses); // NOLINT + config_->blackboard->set("goals", fake_poses); // NOLINT bt_node_ = std::make_shared("speed_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp index 437837f19f..6215909ae9 100644 --- a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp @@ -43,10 +43,10 @@ class BehaviorTreeTestFixture : public ::testing::Test // Create the blackboard that will be shared by all of the nodes in the tree config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard - config_->blackboard->set( + config_->blackboard->set( "node", node_); - config_->blackboard->set>( + config_->blackboard->set( "tf_buffer", transform_handler_->getBuffer()); config_->blackboard->set( @@ -55,7 +55,7 @@ class BehaviorTreeTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); - config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("initial_pose_received", false); transform_handler_->activate(); transform_handler_->waitForTransform(); diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 348ca0860f..53769abfcd 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -232,7 +232,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr // Reset state for new action feedback start_time_ = clock_->now(); auto blackboard = bt_action_server_->getBlackboard(); - blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard blackboard->set(goals_blackboard_id_, std::move(goal_poses)); diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index a8298fc36f..326a0bd9fb 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -241,10 +241,10 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) // Reset state for new action feedback start_time_ = clock_->now(); auto blackboard = bt_action_server_->getBlackboard(); - blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goal_blackboard_id_, goal_pose); + blackboard->set(goal_blackboard_id_, goal_pose); return true; } diff --git a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp index 1a35aa6686..f6b8ecd62a 100644 --- a/nav2_core/include/nav2_core/behavior_tree_navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -220,10 +220,10 @@ class BehaviorTreeNavigator : public NavigatorBase } BT::Blackboard::Ptr blackboard = bt_action_server_->getBlackboard(); - blackboard->set>("tf_buffer", feedback_utils.tf); // NOLINT - blackboard->set("initial_pose_received", false); // NOLINT - blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set>("odom_smoother", odom_smoother); // NOLINT + blackboard->set("tf_buffer", feedback_utils.tf); // NOLINT + blackboard->set("initial_pose_received", false); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set("odom_smoother", odom_smoother); // NOLINT return configure(parent_node, odom_smoother) && ok; } diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48b97848f8..51558d9ba1 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -133,17 +133,17 @@ class BehaviorTreeHandler blackboard = BT::Blackboard::create(); // Put items on the blackboard - blackboard->set("node", node_); // NOLINT + blackboard->set("node", node_); // NOLINT blackboard->set( "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT - blackboard->set>("tf_buffer", tf_); // NOLINT - blackboard->set("initial_pose_received", false); // NOLINT - blackboard->set("number_recoveries", 0); // NOLINT - blackboard->set>("odom_smoother", odom_smoother_); // NOLINT + blackboard->set("tf_buffer", tf_); // NOLINT + blackboard->set("initial_pose_received", false); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT + blackboard->set("odom_smoother", odom_smoother_); // NOLINT // set dummy goal on blackboard geometry_msgs::msg::PoseStamped goal; @@ -157,7 +157,7 @@ class BehaviorTreeHandler goal.pose.orientation.z = 0.0; goal.pose.orientation.w = 1.0; - blackboard->set("goal", goal); // NOLINT + blackboard->set("goal", goal); // NOLINT // Create the Behavior Tree from the XML input try { @@ -599,7 +599,7 @@ TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) goal.pose.orientation.y = 0.0; goal.pose.orientation.z = 0.0; goal.pose.orientation.w = 1.0; - bt_handler->blackboard->set("goal", goal); // NOLINT + bt_handler->blackboard->set("goal", goal); // NOLINT } std::this_thread::sleep_for(10ms);