Added preemption to recovery server #1644
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,76 @@ | ||
| #ifndef NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ | ||
| #define NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ | ||
|
There was a problem hiding this comment. I think this guard is wrong, check with ament_cpplint and ament_uncrustify. I'm sure they're complaining and will give you the exact header guard to use. There was a problem hiding this comment. hmm I think uncrustify wasn't complaining on this one, I run it through and it only complaint about the copyright, but I'll check it again. If you say it because of the There was a problem hiding this comment. Just see what it says when you fix the copyright and work from there. Maybe I'm mistaken. |
||
|
|
||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| #include "behaviortree_cpp_v3/condition_node.h" | ||
| #include "geometry_msgs/msg/pose_stamped.hpp" | ||
| #include "nav2_util/robot_utils.hpp" | ||
| #include "rclcpp/rclcpp.hpp" | ||
| #include "tf2_ros/buffer.h" | ||
|
|
||
| namespace nav2_behavior_tree | ||
| { | ||
|
|
||
| class GoalUpdatedCondition : public BT::ConditionNode | ||
| { | ||
| public: | ||
| GoalUpdatedCondition( | ||
| const std::string & condition_name, | ||
| const BT::NodeConfiguration & conf) | ||
| : BT::ConditionNode(condition_name, conf), initialized_(false) | ||
| { | ||
| } | ||
|
|
||
| GoalUpdatedCondition() = delete; | ||
|
|
||
| BT::NodeStatus tick() override | ||
| { | ||
| if (!initialized_) { | ||
| initialize(); | ||
| } | ||
|
|
||
| if (isGoalUpdated()) { | ||
| return BT::NodeStatus::SUCCESS; | ||
| } | ||
| return BT::NodeStatus::FAILURE; | ||
| } | ||
|
|
||
| void initialize() | ||
| { | ||
| goal_ = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal"); | ||
|
There was a problem hiding this comment. You can add this stuff to the constructor and remove the initialize function and the variable. There was a problem hiding this comment. I can remove the function and add the logic in the tick, but I think this initialization needs to be here (in the tick). I think the blackboard goal might not be set when the constructor is called (i.e., when we build the tree). There was a problem hiding this comment. And now that I see it, I'm also missing to set back to false the initialized_ variable to false if the node in halt. I just realized that the goal needs to be updated in the first tick of the condition, otherwise if the goal changed between restarts of this node, this one will succeed on the first tick! There was a problem hiding this comment. If you set goal to a bogus value, then the first goal that comes in will trigger the updated condition. I think that would be fine, wouldn't it? Then the comparison for is current would be that there's a new goal and therefore replan. I think the goal_ could actually be constructed as default so that it is always out of date on startup causing the updated condition to be true on the first tick. Does that make sense / correct? There was a problem hiding this comment. The thing is, how do you know if you are in the first tick? What we are checking is wether the goal has changed, meaning that return success means "goal has changed" and failure means "goal has not changed". What I thought is: 1st tick: A flag tells that is the first tick, so we check the goal (and a return could be here straight away) The reason behind this is how the parent ReactiveFallback node works. That node will halt all children and return success when one of its children returns success. This means that this new condition node needs to return failure in each tick, except from when the goal changes. This also means that we need to reset the current goal any time we enter the parent control node, so we don't return success in the first tick. I think first tick should always be initialization and return failure. What do you think? Maybe I don't understand what you mean. There was a problem hiding this comment. If this is the first iteration, lets game out the 2 things this does:
Isn't that what we want? For subsequent calls, you're updating the goal_ param with the new blackboard param, correct, so then in the future the goal_ is the same and it won't replan. Once a new goal comes in, it'll again mismatch and cause a replan. Your 1st tick I think right now wouldn't actually cause the planner to plan at all. You set the goal_ to the initial request goal and then you check if they're the same, which they now are. That would make this return false that there is no new goal and then wouldn't navigate or plan ever until a new preempted goal was requested. You could test this by just running this code and sending the first goal and seeing if the robot actually tries to plan on the initial goal sent. There was a problem hiding this comment. We should continue this in the new PR. |
||
| initialized_ = true; | ||
| } | ||
|
|
||
| bool | ||
| isGoalUpdated() | ||
| { | ||
| auto current_goal = config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal"); | ||
| if (goal_ != current_goal) { | ||
|
There was a problem hiding this comment. This was clever. I thought about setting a flag and required a new BT node in the root. This gets around that. Good idea. There was a problem hiding this comment. I couldn't think on other way to check if there if the goal was preempted in more than one place. Thanks! |
||
| goal_ = current_goal; | ||
| return true; | ||
| } else { | ||
| return false; | ||
| } | ||
| } | ||
|
|
||
| static BT::PortsList providedPorts() | ||
| { | ||
| return {}; | ||
| } | ||
|
|
||
| private: | ||
| geometry_msgs::msg::PoseStamped goal_; | ||
| bool initialized_; | ||
| }; | ||
|
|
||
| } // namespace nav2_behavior_tree | ||
|
|
||
| #include "behaviortree_cpp_v3/bt_factory.h" | ||
| BT_REGISTER_NODES(factory) | ||
| { | ||
| factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>("GoalUpdated"); | ||
| } | ||
|
|
||
| #endif // NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ | ||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I am missing the copyright here, because I don't really know what I am supposed to write :)
Any ideas?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Copy paste from another file and just change to your name
(literally)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Perfect, thanks!