Skip to content

Commit

Permalink
Draft: Added GoalUpdatedController BT plugin node (ros-navigation#3044)
Browse files Browse the repository at this point in the history
* first draft for goal updated controller

Signed-off-by: relffok <57466265+relffok@users.noreply.github.com>

* 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
  • Loading branch information
relffok authored Jun 28, 2022
1 parent 14a63c1 commit 28d16ab
Show file tree
Hide file tree
Showing 14 changed files with 295 additions and 0 deletions.
3 changes: 3 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -180,6 +180,9 @@ list(APPEND plugin_libs nav2_controller_selector_bt_node)
add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)

add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)

foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_

#include <chrono>
#include <string>
#include <vector>

#include "behaviortree_cpp_v3/decorator_node.h"

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace nav2_behavior_tree
{

/**
* @brief A BT::DecoratorNode that ticks its child if the goal was updated
*/
class GoalUpdatedController : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GoalUpdatedController
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
GoalUpdatedController(
const std::string & name,
const BT::NodeConfiguration & conf);

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts()
{
return {};
}

private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick() override;

bool goal_was_updated_;
geometry_msgs::msg::PoseStamped goal_;
std::vector<geometry_msgs::msg::PoseStamped> goals_;
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
3 changes: 3 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -254,5 +254,8 @@
<input_port name="length_factor">Length multiplication factor to check if the path is significantly longer </input_port>
</Decorator>

<Decorator ID="GoalUpdatedController">
</Decorator>

</TreeNodesModel>
</root>
88 changes: 88 additions & 0 deletions nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,88 @@
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"


namespace nav2_behavior_tree
{

GoalUpdatedController::GoalUpdatedController(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::DecoratorNode(name, conf)
{
}

BT::NodeStatus GoalUpdatedController::tick()
{
if (status() == BT::NodeStatus::IDLE) {
// Reset since we're starting a new iteration of
// the goal updated controller (moving from IDLE to RUNNING)

config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);

goal_was_updated_ = true;
}

setStatus(BT::NodeStatus::RUNNING);

std::vector<geometry_msgs::msg::PoseStamped> current_goals;
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
geometry_msgs::msg::PoseStamped current_goal;
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);

if (goal_ != current_goal || goals_ != current_goals) {
goal_ = current_goal;
goals_ = current_goals;
goal_was_updated_ = true;
}

// The child gets ticked the first time through and any time the goal has
// changed or preempted. In addition, once the child begins to run, it is ticked each time
// 'til completion
if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) {
goal_was_updated_ = false;
const BT::NodeStatus child_state = child_node_->executeTick();

switch (child_state) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;

case BT::NodeStatus::SUCCESS:
return BT::NodeStatus::SUCCESS;

case BT::NodeStatus::FAILURE:
default:
return BT::NodeStatus::FAILURE;
}
}

return status();
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::GoalUpdatedController>("GoalUpdatedController");
}
4 changes: 4 additions & 0 deletions nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,10 @@ ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp)
target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node)
ament_target_dependencies(test_single_trigger_node ${dependencies})

ament_add_gtest(test_goal_updated_controller test_goal_updated_controller.cpp)
target_link_libraries(test_goal_updated_controller nav2_goal_updated_controller_bt_node)
ament_target_dependencies(test_goal_updated_controller ${dependencies})

ament_add_gtest(test_decorator_path_longer_on_approach test_path_longer_on_approach.cpp)
target_link_libraries(test_decorator_path_longer_on_approach nav2_path_longer_on_approach_bt_node)
ament_target_dependencies(test_decorator_path_longer_on_approach ${dependencies})
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>

#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"

using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT

class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
void SetUp()
{
// Setting fake goals on blackboard
geometry_msgs::msg::PoseStamped goal1;
goal1.header.stamp = node_->now();
std::vector<geometry_msgs::msg::PoseStamped> poses1;
poses1.push_back(goal1);
config_->blackboard->set("goal", goal1);
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses1);

bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
"goal_updated_controller", *config_);
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
bt_node_->setChild(dummy_node_.get());
}

void TearDown()
{
dummy_node_.reset();
bt_node_.reset();
}

protected:
static std::shared_ptr<nav2_behavior_tree::GoalUpdatedController> bt_node_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};

std::shared_ptr<nav2_behavior_tree::GoalUpdatedController>
GoalUpdatedControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
GoalUpdatedControllerTestFixture::dummy_node_ = nullptr;

TEST_F(GoalUpdatedControllerTestFixture, test_behavior)
{
// Creating updated fake-goals
geometry_msgs::msg::PoseStamped goal2;
goal2.header.stamp = node_->now();
std::vector<geometry_msgs::msg::PoseStamped> poses2;
poses2.push_back(goal2);

// starting in idle
EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE);

// tick for the first time, dummy node should be ticked
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);

// tick again with updated goal, dummy node should be ticked
config_->blackboard->set("goal", goal2);
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);

// tick again without update, dummy node should not be ticked
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);

// tick again with updated goals, dummy node should be ticked
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>("goals", poses2);
dummy_node_->changeStatus(BT::NodeStatus::SUCCESS);
EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);

// initialize ROS
rclcpp::init(argc, argv);

bool all_successful = RUN_ALL_TESTS();

// shutdown ROS
rclcpp::shutdown();

return all_successful;
}
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_multirobot_params_1.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ bt_navigator:
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_multirobot_params_2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ bt_navigator:
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ bt_navigator:
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<!--
This Behavior Tree replans the global path only when the goal is updated.
-->

<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<PipelineSequence name="NavigateWithReplanning">
<GoalUpdatedController> <!-- only tick child if goal was updated-->
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</GoalUpdatedController>
<FollowPath path="{path}" controller_id="FollowPath"/>
</PipelineSequence>
</BehaviorTree>
</root>
1 change: 1 addition & 0 deletions nav2_bt_navigator/src/bt_navigator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
"nav2_path_expiring_timer_condition",
"nav2_distance_traveled_condition_bt_node",
"nav2_single_trigger_bt_node",
"nav2_goal_updated_controller_bt_node",
"nav2_is_battery_low_condition_bt_node",
"nav2_navigate_through_poses_action_bt_node",
"nav2_navigate_to_pose_action_bt_node",
Expand Down
1 change: 1 addition & 0 deletions nav2_system_tests/src/costmap_filters/keepout_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ bt_navigator:
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@ bt_navigator:
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@ bt_navigator:
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
Expand Down

0 comments on commit 28d16ab

Please sign in to comment.