Skip to content

Add condition nodes for time and distance replanning #1705

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 9 commits into from
May 27, 2020
Merged

Add condition nodes for time and distance replanning #1705

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
cf39a54
Add condition nodes and behavior tree to enable replan on new goal
naiveHobo May 8, 2020
da73f76
Fix time expired and distance traveled conditions
naiveHobo May 16, 2020
363f974
Remove new_goal_received from blackboard
naiveHobo May 16, 2020
da6f5b2
Fix IDLE check condition in new condition nodes
naiveHobo May 17, 2020
24d39ba
Fix lint errors
naiveHobo May 17, 2020
19c8c4f
Fix lint errors
naiveHobo May 17, 2020
e0d2305
Address reviewer's comments
naiveHobo May 19, 2020
ce4b7dc
Add tests
naiveHobo May 20, 2020
de0f797
Merge branch 'master' of github.com:ros-planning/navigation2 into fea…
naiveHobo May 27, 2020
File filter

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Fix IDLE check condition in new condition nodes
  • Loading branch information
@naiveHobo
naiveHobo committed May 19, 2020
commit da6f5b2fc9700ca7353672cd97ab3904ba160b05
5 changes: 2 additions & 3 deletions nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,7 @@ DistanceTraveledCondition::DistanceTraveledCondition(
const std::string & condition_name,
const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf),
distance_(1.0),
first_tick_(true)
distance_(1.0)
{
getInput("distance", distance_);
getInput("global_frame", global_frame_);
Expand All @@ -40,7 +39,7 @@ DistanceTraveledCondition::DistanceTraveledCondition(

BT::NodeStatus DistanceTraveledCondition::tick()
{
if (first_tick_) {
if (status() == BT::NodeStatus::IDLE) {
if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
}
Expand Down
1 change: 0 additions & 1 deletion nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,6 @@ class DistanceTraveledCondition : public BT::ConditionNode
double distance_;
std::string global_frame_;
std::string robot_base_frame_;
bool first_tick_;
};

} // namespace nav2_behavior_tree
Expand Down
6 changes: 2 additions & 4 deletions nav2_behavior_tree/plugins/condition/time_expired_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,17 +30,15 @@ TimeExpiredCondition::TimeExpiredCondition(
const std::string & condition_name,
const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf),
period_(1.0),
first_tick_(true)
period_(1.0)
{
getInput("seconds", period_);
}

BT::NodeStatus TimeExpiredCondition::tick()
{
if (first_tick_) {
if (status() == BT::NodeStatus::IDLE) {
start_ = std::chrono::high_resolution_clock::now();
first_tick_ = false;
return BT::NodeStatus::FAILURE;
}

Expand Down
1 change: 0 additions & 1 deletion nav2_behavior_tree/plugins/condition/time_expired_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,6 @@ class TimeExpiredCondition : public BT::ConditionNode
private:
std::chrono::time_point<std::chrono::high_resolution_clock> start_;
double period_;
bool first_tick_;
};

} // namespace nav2_behavior_tree
Expand Down