Skip to content

Commit 28d16ab

Browse files
relffokrelffok
authored
Draft: Added GoalUpdatedController BT plugin node (ros-navigation#3044)
* first draft for goal updated controller Signed-off-by: relffok <[email protected]> * added goal_updated_controller to all yamls * added GoalUpdatedController API * added GoaUpdatedController to default plugins * removed first_time param * added test for GoalUpdatedController * linter fix
1 parent 14a63c1 commit 28d16ab

File tree

14 files changed

+295
-0
lines changed

14 files changed

+295
-0
lines changed

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -180,6 +180,9 @@ list(APPEND plugin_libs nav2_controller_selector_bt_node)
180180
add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
181181
list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)
182182

183+
add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
184+
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
185+
183186
foreach(bt_plugin ${plugin_libs})
184187
ament_target_dependencies(${bt_plugin} ${dependencies})
185188
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

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

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
17+
18+
#include <chrono>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "behaviortree_cpp_v3/decorator_node.h"
23+
24+
#include "rclcpp/rclcpp.hpp"
25+
#include "geometry_msgs/msg/pose_stamped.hpp"
26+
27+
namespace nav2_behavior_tree
28+
{
29+
30+
/**
31+
* @brief A BT::DecoratorNode that ticks its child if the goal was updated
32+
*/
33+
class GoalUpdatedController : public BT::DecoratorNode
34+
{
35+
public:
36+
/**
37+
* @brief A constructor for nav2_behavior_tree::GoalUpdatedController
38+
* @param name Name for the XML tag for this node
39+
* @param conf BT node configuration
40+
*/
41+
GoalUpdatedController(
42+
const std::string & name,
43+
const BT::NodeConfiguration & conf);
44+
45+
/**
46+
* @brief Creates list of BT ports
47+
* @return BT::PortsList Containing node-specific ports
48+
*/
49+
static BT::PortsList providedPorts()
50+
{
51+
return {};
52+
}
53+
54+
private:
55+
/**
56+
* @brief The main override required by a BT action
57+
* @return BT::NodeStatus Status of tick execution
58+
*/
59+
BT::NodeStatus tick() override;
60+
61+
bool goal_was_updated_;
62+
geometry_msgs::msg::PoseStamped goal_;
63+
std::vector<geometry_msgs::msg::PoseStamped> goals_;
64+
};
65+
66+
} // namespace nav2_behavior_tree
67+
68+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_

‎nav2_behavior_tree/nav2_tree_nodes.xml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -254,5 +254,8 @@
254254
<input_port name="length_factor">Length multiplication factor to check if the path is significantly longer </input_port>
255255
</Decorator>
256256

257+
<Decorator ID="GoalUpdatedController">
258+
</Decorator>
259+
257260
</TreeNodesModel>
258261
</root>

‎nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <chrono>
16+
#include <string>
17+
18+
#include "rclcpp/rclcpp.hpp"
19+
#include "geometry_msgs/msg/pose_stamped.hpp"
20+
#include "behaviortree_cpp_v3/decorator_node.h"
21+
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
22+
23+
24+
namespace nav2_behavior_tree
25+
{
26+
27+
GoalUpdatedController::GoalUpdatedController(
28+
const std::string & name,
29+
const BT::NodeConfiguration & conf)
30+
: BT::DecoratorNode(name, conf)
31+
{
32+
}
33+
34+
BT::NodeStatus GoalUpdatedController::tick()
35+
{
36+
if (status() == BT::NodeStatus::IDLE) {
37+
// Reset since we're starting a new iteration of
38+
// the goal updated controller (moving from IDLE to RUNNING)
39+
40+
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
41+
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
42+
43+
goal_was_updated_ = true;
44+
}
45+
46+
setStatus(BT::NodeStatus::RUNNING);
47+
48+
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
49+
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
50+
geometry_msgs::msg::PoseStamped current_goal;
51+
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
52+
53+
if (goal_ != current_goal || goals_ != current_goals) {
54+
goal_ = current_goal;
55+
goals_ = current_goals;
56+
goal_was_updated_ = true;
57+
}
58+
59+
// The child gets ticked the first time through and any time the goal has
60+
// changed or preempted. In addition, once the child begins to run, it is ticked each time
61+
// 'til completion
62+
if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) {
63+
goal_was_updated_ = false;
64+
const BT::NodeStatus child_state = child_node_->executeTick();
65+
66+
switch (child_state) {
67+
case BT::NodeStatus::RUNNING:
68+
return BT::NodeStatus::RUNNING;
69+
70+
case BT::NodeStatus::SUCCESS:
71+
return BT::NodeStatus::SUCCESS;
72+
73+
case BT::NodeStatus::FAILURE:
74+
default:
75+
return BT::NodeStatus::FAILURE;
76+
}
77+
}
78+
79+
return status();
80+
}
81+
82+
} // namespace nav2_behavior_tree
83+
84+
#include "behaviortree_cpp_v3/bt_factory.h"
85+
BT_REGISTER_NODES(factory)
86+
{
87+
factory.registerNodeType<nav2_behavior_tree::GoalUpdatedController>("GoalUpdatedController");
88+
}

‎nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,10 @@ ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp)
1818
target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node)
1919
ament_target_dependencies(test_single_trigger_node ${dependencies})
2020

21+
ament_add_gtest(test_goal_updated_controller test_goal_updated_controller.cpp)
22+
target_link_libraries(test_goal_updated_controller nav2_goal_updated_controller_bt_node)
23+
ament_target_dependencies(test_goal_updated_controller ${dependencies})
24+
2125
ament_add_gtest(test_decorator_path_longer_on_approach test_path_longer_on_approach.cpp)
2226
target_link_libraries(test_decorator_path_longer_on_approach nav2_path_longer_on_approach_bt_node)
2327
ament_target_dependencies(test_decorator_path_longer_on_approach ${dependencies})

‎nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2020 Sarthak Mittal
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <gtest/gtest.h>
17+
#include <chrono>
18+
#include <memory>
19+
#include <set>
20+
21+
#include "../../test_behavior_tree_fixture.hpp"
22+
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
23+
24+
using namespace std::chrono; // NOLINT
25+
using namespace std::chrono_literals; // NOLINT
26+
27+
class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
28+
{
29+
public:
30+
void SetUp()
31+
{
32+
// Setting fake goals on blackboard
33+
geometry_msgs::msg::PoseStamped goal1;
34+
goal1.header.stamp = node_->now();
35+
std::vector<geometry_msgs::msg::PoseStamped> poses1;
36+
poses1.push_back(goal1);
37+
config_->blackboard->set("goal", goal1);
38+
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses1);
39+
40+
bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
41+
"goal_updated_controller", *config_);
42+
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
43+
bt_node_->setChild(dummy_node_.get());
44+
}
45+
46+
void TearDown()
47+
{
48+
dummy_node_.reset();
49+
bt_node_.reset();
50+
}
51+
52+
protected:
53+
static std::shared_ptr<nav2_behavior_tree::GoalUpdatedController> bt_node_;
54+
static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
55+
};
56+
57+
std::shared_ptr<nav2_behavior_tree::GoalUpdatedController>
58+
GoalUpdatedControllerTestFixture::bt_node_ = nullptr;
59+
std::shared_ptr<nav2_behavior_tree::DummyNode>
60+
GoalUpdatedControllerTestFixture::dummy_node_ = nullptr;
61+
62+
TEST_F(GoalUpdatedControllerTestFixture, test_behavior)
63+
{
64+
// Creating updated fake-goals
65+
geometry_msgs::msg::PoseStamped goal2;
66+
goal2.header.stamp = node_->now();
67+
std::vector<geometry_msgs::msg::PoseStamped> poses2;
68+
poses2.push_back(goal2);
69+
70+
// starting in idle
71+
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);
72+
73+
// tick for the first time, dummy node should be ticked
74+
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
75+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
76+
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
77+
78+
// tick again with updated goal, dummy node should be ticked
79+
config_->blackboard->set("goal", goal2);
80+
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
81+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
82+
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
83+
84+
// tick again without update, dummy node should not be ticked
85+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
86+
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
87+
88+
// tick again with updated goals, dummy node should be ticked
89+
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses2);
90+
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
91+
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
92+
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
93+
}
94+
95+
int main(int argc, char ** argv)
96+
{
97+
::testing::InitGoogleTest(&argc, argv);
98+
99+
// initialize ROS
100+
rclcpp::init(argc, argv);
101+
102+
bool all_successful = RUN_ALL_TESTS();
103+
104+
// shutdown ROS
105+
rclcpp::shutdown();
106+
107+
return all_successful;
108+
}

‎nav2_bringup/params/nav2_multirobot_params_1.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,7 @@ bt_navigator:
8282
- nav2_path_expiring_timer_condition
8383
- nav2_distance_traveled_condition_bt_node
8484
- nav2_single_trigger_bt_node
85+
- nav2_goal_updated_controller_bt_node
8586
- nav2_is_battery_low_condition_bt_node
8687
- nav2_navigate_through_poses_action_bt_node
8788
- nav2_navigate_to_pose_action_bt_node

‎nav2_bringup/params/nav2_multirobot_params_2.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,7 @@ bt_navigator:
8282
- nav2_path_expiring_timer_condition
8383
- nav2_distance_traveled_condition_bt_node
8484
- nav2_single_trigger_bt_node
85+
- nav2_goal_updated_controller_bt_node
8586
- nav2_is_battery_low_condition_bt_node
8687
- nav2_navigate_through_poses_action_bt_node
8788
- nav2_navigate_to_pose_action_bt_node

‎nav2_bringup/params/nav2_params.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,7 @@ bt_navigator:
8282
- nav2_path_expiring_timer_condition
8383
- nav2_distance_traveled_condition_bt_node
8484
- nav2_single_trigger_bt_node
85+
- nav2_goal_updated_controller_bt_node
8586
- nav2_is_battery_low_condition_bt_node
8687
- nav2_navigate_through_poses_action_bt_node
8788
- nav2_navigate_to_pose_action_bt_node

‎nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
<!--
2+
This Behavior Tree replans the global path only when the goal is updated.
3+
-->
4+
5+
<root main_tree_to_execute="MainTree">
6+
<BehaviorTree ID="MainTree">
7+
<PipelineSequence name="NavigateWithReplanning">
8+
<GoalUpdatedController> <!-- only tick child if goal was updated-->
9+
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
10+
</GoalUpdatedController>
11+
<FollowPath path="{path}" controller_id="FollowPath"/>
12+
</PipelineSequence>
13+
</BehaviorTree>
14+
</root>

0 commit comments

Comments
 (0)