forked from ros-navigation/navigation2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add condition nodes for time and distance replanning (ros-navigation#…
…1705) * Add condition nodes and behavior tree to enable replan on new goal Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Fix time expired and distance traveled conditions * Remove new_goal_received from blackboard * Fix IDLE check condition in new condition nodes * Fix lint errors * Fix lint errors * Address reviewer's comments Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com> * Add tests Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
- Loading branch information
Showing
15 changed files
with
570 additions
and
8 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
65 changes: 65 additions & 0 deletions
65
nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,65 @@ | ||
// Copyright (c) 2020 Sarthak Mittal | ||
// Copyright (c) 2019 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__DISTANCE_TRAVELED_CONDITION_HPP_ | ||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ | ||
|
||
#include <string> | ||
#include <memory> | ||
|
||
#include "behaviortree_cpp_v3/condition_node.h" | ||
|
||
#include "rclcpp/rclcpp.hpp" | ||
#include "geometry_msgs/msg/pose_stamped.hpp" | ||
#include "tf2_ros/buffer.h" | ||
|
||
namespace nav2_behavior_tree | ||
{ | ||
|
||
class DistanceTraveledCondition : public BT::ConditionNode | ||
{ | ||
public: | ||
DistanceTraveledCondition( | ||
const std::string & condition_name, | ||
const BT::NodeConfiguration & conf); | ||
|
||
DistanceTraveledCondition() = delete; | ||
|
||
BT::NodeStatus tick() override; | ||
|
||
static BT::PortsList providedPorts() | ||
{ | ||
return { | ||
BT::InputPort<double>("distance", 1.0, "Distance"), | ||
BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"), | ||
BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame") | ||
}; | ||
} | ||
|
||
private: | ||
rclcpp::Node::SharedPtr node_; | ||
std::shared_ptr<tf2_ros::Buffer> tf_; | ||
|
||
geometry_msgs::msg::PoseStamped start_pose_; | ||
|
||
double distance_; | ||
double transform_tolerance_; | ||
std::string global_frame_; | ||
std::string robot_base_frame_; | ||
}; | ||
|
||
} // namespace nav2_behavior_tree | ||
|
||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ |
54 changes: 54 additions & 0 deletions
54
nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,54 @@ | ||
// Copyright (c) 2020 Sarthak Mittal | ||
// Copyright (c) 2019 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__TIME_EXPIRED_CONDITION_HPP_ | ||
#define NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ | ||
|
||
#include <string> | ||
|
||
#include "rclcpp/rclcpp.hpp" | ||
#include "behaviortree_cpp_v3/condition_node.h" | ||
|
||
namespace nav2_behavior_tree | ||
{ | ||
|
||
class TimeExpiredCondition : public BT::ConditionNode | ||
{ | ||
public: | ||
TimeExpiredCondition( | ||
const std::string & condition_name, | ||
const BT::NodeConfiguration & conf); | ||
|
||
TimeExpiredCondition() = delete; | ||
|
||
BT::NodeStatus tick() override; | ||
|
||
// Any BT node that accepts parameters must provide a requiredNodeParameters method | ||
static BT::PortsList providedPorts() | ||
{ | ||
return { | ||
BT::InputPort<double>("seconds", 1.0, "Seconds") | ||
}; | ||
} | ||
|
||
private: | ||
rclcpp::Node::SharedPtr node_; | ||
rclcpp::Time start_; | ||
double period_; | ||
}; | ||
|
||
} // namespace nav2_behavior_tree | ||
|
||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ |
91 changes: 91 additions & 0 deletions
91
nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,91 @@ | ||
// Copyright (c) 2019 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. | ||
|
||
#ifndef NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ | ||
#define NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ | ||
|
||
#include <string> | ||
#include <memory> | ||
|
||
#include "nav2_util/robot_utils.hpp" | ||
#include "nav2_util/geometry_utils.hpp" | ||
|
||
#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp" | ||
|
||
namespace nav2_behavior_tree | ||
{ | ||
|
||
DistanceTraveledCondition::DistanceTraveledCondition( | ||
const std::string & condition_name, | ||
const BT::NodeConfiguration & conf) | ||
: BT::ConditionNode(condition_name, conf), | ||
distance_(1.0), | ||
transform_tolerance_(0.1), | ||
global_frame_("map"), | ||
robot_base_frame_("base_link") | ||
{ | ||
getInput("distance", distance_); | ||
getInput("global_frame", global_frame_); | ||
getInput("robot_base_frame", robot_base_frame_); | ||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node"); | ||
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer"); | ||
node_->get_parameter("transform_tolerance", transform_tolerance_); | ||
} | ||
|
||
BT::NodeStatus DistanceTraveledCondition::tick() | ||
{ | ||
if (status() == BT::NodeStatus::IDLE) { | ||
if (!nav2_util::getCurrentPose( | ||
start_pose_, *tf_, global_frame_, robot_base_frame_, | ||
transform_tolerance_)) | ||
{ | ||
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); | ||
} | ||
return BT::NodeStatus::FAILURE; | ||
} | ||
|
||
// Determine distance travelled since we've started this iteration | ||
geometry_msgs::msg::PoseStamped current_pose; | ||
if (!nav2_util::getCurrentPose( | ||
current_pose, *tf_, global_frame_, robot_base_frame_, | ||
transform_tolerance_)) | ||
{ | ||
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); | ||
return BT::NodeStatus::FAILURE; | ||
} | ||
|
||
// Get euclidean distance | ||
auto travelled = nav2_util::geometry_utils::euclidean_distance( | ||
start_pose_.pose, current_pose.pose); | ||
|
||
if (travelled < distance_) { | ||
return BT::NodeStatus::FAILURE; | ||
} | ||
|
||
// Update start pose | ||
start_pose_ = current_pose; | ||
|
||
return BT::NodeStatus::SUCCESS; | ||
} | ||
|
||
} // namespace nav2_behavior_tree | ||
|
||
#include "behaviortree_cpp_v3/bt_factory.h" | ||
BT_REGISTER_NODES(factory) | ||
{ | ||
factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled"); | ||
} | ||
|
||
#endif // NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ |
69 changes: 69 additions & 0 deletions
69
nav2_behavior_tree/plugins/condition/time_expired_condition.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,69 @@ | ||
// Copyright (c) 2019 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. | ||
|
||
#ifndef NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ | ||
#define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ | ||
|
||
#include <string> | ||
#include <memory> | ||
|
||
#include "behaviortree_cpp_v3/condition_node.h" | ||
|
||
#include "nav2_behavior_tree/plugins/time_expired_condition.hpp" | ||
|
||
namespace nav2_behavior_tree | ||
{ | ||
|
||
TimeExpiredCondition::TimeExpiredCondition( | ||
const std::string & condition_name, | ||
const BT::NodeConfiguration & conf) | ||
: BT::ConditionNode(condition_name, conf), | ||
period_(1.0) | ||
{ | ||
getInput("seconds", period_); | ||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node"); | ||
start_ = node_->now(); | ||
} | ||
|
||
BT::NodeStatus TimeExpiredCondition::tick() | ||
{ | ||
if (status() == BT::NodeStatus::IDLE) { | ||
start_ = node_->now(); | ||
return BT::NodeStatus::FAILURE; | ||
} | ||
|
||
// Determine how long its been since we've started this iteration | ||
auto elapsed = node_->now() - start_; | ||
|
||
// Now, get that in seconds | ||
auto seconds = elapsed.seconds(); | ||
|
||
if (seconds < period_) { | ||
return BT::NodeStatus::FAILURE; | ||
} | ||
|
||
start_ = node_->now(); // Reset the timer | ||
return BT::NodeStatus::SUCCESS; | ||
} | ||
|
||
} // namespace nav2_behavior_tree | ||
|
||
#include "behaviortree_cpp_v3/bt_factory.h" | ||
BT_REGISTER_NODES(factory) | ||
{ | ||
factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>("TimeExpired"); | ||
} | ||
|
||
#endif // NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,11 +1,29 @@ | ||
ament_add_gtest(test_decorator_distance_controller | ||
plugins/decorator/test_distance_controller.cpp | ||
) | ||
|
||
target_link_libraries(test_decorator_distance_controller | ||
nav2_distance_controller_bt_node | ||
) | ||
|
||
ament_target_dependencies(test_decorator_distance_controller | ||
${dependencies} | ||
) | ||
|
||
ament_add_gtest(test_condition_distance_traveled | ||
plugins/condition/test_distance_traveled.cpp | ||
) | ||
target_link_libraries(test_condition_distance_traveled | ||
nav2_distance_traveled_condition_bt_node | ||
) | ||
ament_target_dependencies(test_condition_distance_traveled | ||
${dependencies} | ||
) | ||
|
||
ament_add_gtest(test_condition_time_expired | ||
plugins/condition/test_time_expired.cpp | ||
) | ||
target_link_libraries(test_condition_time_expired | ||
nav2_time_expired_condition_bt_node | ||
) | ||
ament_target_dependencies(test_condition_time_expired | ||
${dependencies} | ||
) |
Oops, something went wrong.