@@ -32,17 +32,16 @@ struct TreeExecutionServer::Pimpl
3232{
3333 rclcpp_action::Server<ExecuteTree>::SharedPtr action_server;
3434 std::thread action_thread;
35- // thread safe bool to keep track of cancel requests
36- std::atomic<bool> cancel_requested{ false };
3735
3836 std::shared_ptr<bt_server::ParamListener> param_listener;
3937 bt_server::Params params;
4038
4139 BT::BehaviorTreeFactory factory;
4240 std::shared_ptr<BT::Groot2Publisher> groot_publisher;
4341
44- std::string current_tree_name;
45- std::shared_ptr<BT::Tree> tree;
42+ std::string tree_name;
43+ std::string payload;
44+ BT::Tree tree;
4645 BT::Blackboard::Ptr global_blackboard;
4746 bool factory_initialized_ = false;
4847
@@ -122,7 +121,6 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
122121{
123122 RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s",
124123 goal->target_tree.c_str());
125- p_->cancel_requested = false;
126124 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
127125}
128126
@@ -136,7 +134,6 @@ rclcpp_action::CancelResponse TreeExecutionServer::handle_cancel(
136134 "processing one.");
137135 return rclcpp_action::CancelResponse::REJECT;
138136 }
139- p_->cancel_requested = true;
140137 return rclcpp_action::CancelResponse::ACCEPT;
141138}
142139
@@ -171,43 +168,50 @@ void TreeExecutionServer::execute(
171168 // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard
172169 auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);
173170
174- p_->tree = std::make_shared<BT::Tree>( );
175- *( p_->tree) = p_->factory.createTree( goal->target_tree, root_blackboard) ;
176- p_->current_tree_name = goal->target_tree ;
171+ p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard );
172+ p_->tree_name = goal->target_tree;
173+ p_->payload = goal->payload ;
177174
178175 // call user defined function after the tree has been created
179- onTreeCreated(* p_->tree);
176+ onTreeCreated(p_->tree);
180177 p_->groot_publisher.reset();
181178 p_->groot_publisher =
182- std::make_shared<BT::Groot2Publisher>(*( p_->tree) , p_->params.groot2_port);
179+ std::make_shared<BT::Groot2Publisher>(p_->tree, p_->params.groot2_port);
183180
184181 // Loop until the tree is done or a cancel is requested
185182 const auto period =
186183 std::chrono::milliseconds(static_cast<int>(1000.0 / p_->params.tick_frequency));
187184 auto loop_deadline = std::chrono::steady_clock::now() + period;
188185
189186 // operations to be done if the tree execution is aborted, either by
190- // cancel_requested_ or by onLoopAfterTick()
187+ // cancel requested or by onLoopAfterTick()
191188 auto stop_action = [this, &action_result](BT::NodeStatus status,
192189 const std::string& message) {
190+ p_->tree.haltTree();
193191 action_result->node_status = ConvertNodeStatus(status);
194- action_result->error_message = message;
195- RCLCPP_WARN(kLogger, action_result->error_message.c_str());
196- p_->tree->haltTree();
197- onTreeExecutionCompleted(status, true);
192+ // override the message value if the user defined function returns it
193+ if(auto msg = onTreeExecutionCompleted(status, true))
194+ {
195+ action_result->return_message = msg.value();
196+ }
197+ else
198+ {
199+ action_result->return_message = message;
200+ }
201+ RCLCPP_WARN(kLogger, action_result->return_message.c_str());
198202 };
199203
200204 while(rclcpp::ok() && status == BT::NodeStatus::RUNNING)
201205 {
202- if(p_->cancel_requested )
206+ if(goal_handle->is_canceling() )
203207 {
204208 stop_action(status, "Action Server canceling and halting Behavior Tree");
205209 goal_handle->canceled(action_result);
206210 return;
207211 }
208212
209213 // tick the tree once and publish the action feedback
210- status = p_->tree-> tickExactlyOnce();
214+ status = p_->tree. tickExactlyOnce();
211215
212216 if(const auto res = onLoopAfterTick(status); res.has_value())
213217 {
@@ -219,56 +223,67 @@ void TreeExecutionServer::execute(
219223 if(const auto res = onLoopFeedback(); res.has_value())
220224 {
221225 auto feedback = std::make_shared<ExecuteTree::Feedback>();
222- feedback->msg = res.value();
226+ feedback->message = res.value();
223227 goal_handle->publish_feedback(feedback);
224228 }
225229
226230 const auto now = std::chrono::steady_clock::now();
227231 if(now < loop_deadline)
228232 {
229- p_->tree-> sleep(loop_deadline - now);
233+ p_->tree. sleep(loop_deadline - now);
230234 }
231235 loop_deadline += period;
232236 }
233237 }
234238 catch(const std::exception& ex)
235239 {
236- action_result->error_message = std::string("Behavior Tree exception:") + ex.what();
237- RCLCPP_ERROR(kLogger, action_result->error_message .c_str());
240+ action_result->return_message = std::string("Behavior Tree exception:") + ex.what();
241+ RCLCPP_ERROR(kLogger, action_result->return_message .c_str());
238242 goal_handle->abort(action_result);
239243 return;
240244 }
241245
242- // call user defined execution complete function
243- onTreeExecutionCompleted(status, false);
246+ // Call user defined onTreeExecutionCompleted function.
247+ // Override the message value if the user defined function returns it
248+ if(auto msg = onTreeExecutionCompleted(status, false))
249+ {
250+ action_result->return_message = msg.value();
251+ }
252+ else
253+ {
254+ action_result->return_message =
255+ std::string("Tree finished with status: ") + BT::toStr(status);
256+ }
244257
245258 // set the node_status result to the action
246259 action_result->node_status = ConvertNodeStatus(status);
247260
248261 // return success or aborted for the action result
249262 if(status == BT::NodeStatus::SUCCESS)
250263 {
251- RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status) .c_str());
264+ RCLCPP_INFO(kLogger, action_result->return_message .c_str());
252265 goal_handle->succeed(action_result);
253266 }
254267 else
255268 {
256- action_result->error_message = std::string("Behavior Tree failed during execution "
257- "with status: ") +
258- BT::toStr(status);
259- RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
269+ RCLCPP_ERROR(kLogger, action_result->return_message.c_str());
260270 goal_handle->abort(action_result);
261271 }
262272}
263273
264- const std::string& TreeExecutionServer::currentTreeName() const
274+ const std::string& TreeExecutionServer::treeName() const
275+ {
276+ return p_->tree_name;
277+ }
278+
279+ const std::string& TreeExecutionServer::goalPayload() const
265280{
266- return p_->current_tree_name ;
281+ return p_->payload ;
267282}
268283
269- BT::Tree* TreeExecutionServer::currentTree()
284+ const BT::Tree& TreeExecutionServer::tree() const
270285{
271- return p_->tree ? p_->tree.get() : nullptr ;
286+ return p_->tree;
272287}
273288
274289BT::Blackboard::Ptr TreeExecutionServer::globalBlackboard()
0 commit comments