Skip to content

Commit 6171087

Browse files
naiveHobonaiveHobo
authored
Add nav2_behavior_tree::BtActionServer (#2010)
* Add nav2_behavior_tree::BtActionServer Signed-off-by: Sarthak Mittal <[email protected]> * Fix cpplint errors Signed-off-by: Sarthak Mittal <[email protected]> * Remove unnecessary statements in BtActionServer Signed-off-by: Sarthak Mittal <[email protected]> * Make nav2_behavior_tree::BtActionServer a composable object * Add comments * Add on preempt callback, fix naming issues, and move tf to bt navigator * Add separate implementation header for BtActionServer * Fix cpplint error * Pass plugin library names as argument to BtActionServer * Remove action server getter and update onPreempt to not load BT * Fix unnecessary include * Fix function names Signed-off-by: Sarthak Mittal <[email protected]> * Fix typo
1 parent 8d6cd28 commit 6171087

8 files changed

Lines changed: 635 additions & 333 deletions

File tree

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp‎

Lines changed: 200 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,200 @@
1+
// Copyright (c) 2020 Sarthak Mittal
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__BT_ACTION_SERVER_HPP_
16+
#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "geometry_msgs/msg/pose_stamped.hpp"
23+
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
24+
#include "nav2_behavior_tree/ros_topic_logger.hpp"
25+
#include "nav2_util/lifecycle_node.hpp"
26+
#include "nav2_util/simple_action_server.hpp"
27+
28+
namespace nav2_behavior_tree
29+
{
30+
/**
31+
* @class nav2_behavior_tree::BtActionServer
32+
* @brief An action server that uses behavior tree to execute an action
33+
*/
34+
template<class ActionT>
35+
class BtActionServer
36+
{
37+
public:
38+
using ActionServer = nav2_util::SimpleActionServer<ActionT>;
39+
40+
typedef std::function<bool (typename ActionT::Goal::ConstSharedPtr)> OnGoalReceivedCallback;
41+
typedef std::function<void ()> OnLoopCallback;
42+
typedef std::function<void ()> OnPreemptCallback;
43+
44+
/**
45+
* @brief A constructor for nav2_behavior_tree::BtActionServer class
46+
*/
47+
explicit BtActionServer(
48+
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
49+
const std::string & action_name,
50+
const std::vector<std::string> & plugin_lib_names,
51+
OnGoalReceivedCallback on_goal_received_callback,
52+
OnLoopCallback on_loop_callback,
53+
OnPreemptCallback on_preempt_callback = nullptr);
54+
55+
/**
56+
* @brief A destructor for nav2_behavior_tree::BtActionServer class
57+
*/
58+
~BtActionServer();
59+
60+
/**
61+
* @brief Configures member variables
62+
*
63+
* Initializes action server for, builds behavior tree from xml file,
64+
* and calls user-defined onConfigure.
65+
* @return true on SUCCESS and false on FAILURE
66+
*/
67+
bool on_configure();
68+
69+
/**
70+
* @brief Activates action server
71+
* @return true on SUCCESS and false on FAILURE
72+
*/
73+
bool on_activate();
74+
75+
/**
76+
* @brief Deactivates action server
77+
* @return true on SUCCESS and false on FAILURE
78+
*/
79+
bool on_deactivate();
80+
81+
/**
82+
* @brief Resets member variables
83+
* @return true on SUCCESS and false on FAILURE
84+
*/
85+
bool on_cleanup();
86+
87+
/**
88+
* @brief Called when in shutdown state
89+
* @return true on SUCCESS and false on FAILURE
90+
*/
91+
bool on_shutdown();
92+
93+
/**
94+
* @brief Replace current BT with another one
95+
* @param bt_xml_filename The file containing the new BT, uses default filename if empty
96+
* @return true if the resulting BT correspond to the one in bt_xml_filename. false
97+
* if something went wrong, and previous BT is maintained
98+
*/
99+
bool loadBehaviorTree(const std::string & bt_xml_filename = "");
100+
101+
/**
102+
* @brief Getter function for BT Blackboard
103+
* @return shared pointer to current BT blackboard
104+
*/
105+
BT::Blackboard::Ptr getBlackboard() const
106+
{
107+
return blackboard_;
108+
}
109+
110+
/**
111+
* @brief Getter function for current BT XML filename
112+
* @return string
113+
*/
114+
std::string getCurrentBTFilename() const
115+
{
116+
return current_bt_xml_filename_;
117+
}
118+
119+
/**
120+
* @brief Wrapper function to accept pending goal if a preempt has been requested
121+
*/
122+
const std::shared_ptr<const typename ActionT::Goal> acceptPendingGoal()
123+
{
124+
return action_server_->accept_pending_goal();
125+
}
126+
127+
/**
128+
* @brief Wrapper function to get current goal
129+
*/
130+
const std::shared_ptr<const typename ActionT::Goal> getCurrentGoal() const
131+
{
132+
return action_server_->get_current_goal();
133+
}
134+
135+
/**
136+
* @brief Wrapper function to publish action feedback
137+
*/
138+
void publishFeedback(typename std::shared_ptr<typename ActionT::Feedback> feedback)
139+
{
140+
action_server_->publish_feedback(feedback);
141+
}
142+
143+
protected:
144+
/**
145+
* @brief Action server callback
146+
*/
147+
void executeCallback();
148+
149+
// Action name
150+
std::string action_name_;
151+
152+
// Our action server implements the template action
153+
std::shared_ptr<ActionServer> action_server_;
154+
155+
// Behavior Tree to be executed when goal is received
156+
BT::Tree tree_;
157+
158+
// The blackboard shared by all of the nodes in the tree
159+
BT::Blackboard::Ptr blackboard_;
160+
161+
// The XML file that cointains the Behavior Tree to create
162+
std::string current_bt_xml_filename_;
163+
std::string default_bt_xml_filename_;
164+
165+
// The wrapper class for the BT functionality
166+
std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
167+
168+
// Libraries to pull plugins (BT Nodes) from
169+
std::vector<std::string> plugin_lib_names_;
170+
171+
// A regular, non-spinning ROS node that we can use for calls to the action client
172+
rclcpp::Node::SharedPtr client_node_;
173+
174+
// Parent node
175+
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
176+
177+
// Clock
178+
rclcpp::Clock::SharedPtr clock_;
179+
180+
// Logger
181+
rclcpp::Logger logger_{rclcpp::get_logger("BtActionServer")};
182+
183+
// To publish BT logs
184+
std::unique_ptr<RosTopicLogger> topic_logger_;
185+
186+
// Parameters for Groot monitoring
187+
bool enable_groot_monitoring_;
188+
int groot_zmq_publisher_port_;
189+
int groot_zmq_server_port_;
190+
191+
// User-provided callbacks
192+
OnGoalReceivedCallback on_goal_received_callback_;
193+
OnLoopCallback on_loop_callback_;
194+
OnPreemptCallback on_preempt_callback_;
195+
};
196+
197+
} // namespace nav2_behavior_tree
198+
199+
#include <nav2_behavior_tree/bt_action_server_impl.hpp> // NOLINT(build/include_order)
200+
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_

0 commit comments

Comments
 (0)