tree execution server, crash on pointer corruption solved

Author: jocacace

behaviortree_ros2/src/tree_execution_server.cpp

@@ -176,90 +176,76 @@ void TreeExecutionServer::execute(
     executeRegistration();
   }
 
-  // Loop until something happens with ROS or the node completes
-  try
-  {
-    // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard
-    auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);
-
-    p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);
-    p_->tree_name = goal->target_tree;
-    p_->payload = goal->payload;
-
-    // call user defined function after the tree has been created
-    onTreeCreated(p_->tree);
-    p_->groot_publisher.reset();
-    // p_->groot_publisher =
-    //     std::make_shared<BT::Groot2Publisher>(p_->tree, p_->params.groot2_port);
-    BT::DebuggableTree debugTree{std::shared_ptr<BT::Tree>(&(p_->tree)), true, false};
-    BT::PublisherZMQ publisher(debugTree, p_->params.groot2_port);
-
-    // Loop until the tree is done or a cancel is requested
-    const auto period =
-        std::chrono::milliseconds(static_cast<int>(1000.0 / p_->params.tick_frequency));
-    auto loop_deadline = std::chrono::steady_clock::now() + period;
-
-    // operations to be done if the tree execution is aborted, either by
-    // cancel requested or by onLoopAfterTick()
-    auto stop_action = [this, &action_result](BT::NodeStatus status,
+  // This blackboard will be owned by "MainTree". It parent is p_->global_blackboard
+  auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);
+
+  p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);
+  p_->tree_name = goal->target_tree;
+  p_->payload = goal->payload;
+
+  // call user defined function after the tree has been created
+  onTreeCreated(p_->tree);
+  p_->groot_publisher.reset();
+  BT::DebuggableTree debugTree{std::shared_ptr<BT::Tree>(&p_->tree, [](BT::Tree*) {}), true, false};
+  BT::PublisherZMQ publisher(debugTree, p_->params.groot2_port);
+
+  // Loop until the tree is done or a cancel is requested
+  const auto period = std::chrono::milliseconds(static_cast<int>(1000.0 / p_->params.tick_frequency));
+  auto loop_deadline = std::chrono::steady_clock::now() + period;
+
+  // operations to be done if the tree execution is aborted, either by
+  // cancel requested or by onLoopAfterTick()
+  auto stop_action = [this, &action_result](BT::NodeStatus status,
                                               const std::string& message) {
-      p_->tree.haltTree();
-      action_result->node_status = ConvertNodeStatus(status);
-      // override the message value if the user defined function returns it
-      if(auto msg = onTreeExecutionCompleted(status, true))
-      {
-        action_result->return_message = msg.value();
-      }
-      else
-      {
-        action_result->return_message = message;
-      }
-      RCLCPP_WARN(kLogger, action_result->return_message.c_str());
-    };
-
-    while(rclcpp::ok() && status == BT::NodeStatus::RUNNING)
+    p_->tree.haltTree();
+    action_result->node_status = ConvertNodeStatus(status);
+    // override the message value if the user defined function returns it
+    if(auto msg = onTreeExecutionCompleted(status, true))
     {
-      if(goal_handle->is_canceling())
-      {
-        stop_action(status, "Action Server canceling and halting Behavior Tree");
-        goal_handle->canceled(action_result);
-        return;
-      }
-
-      // tick the tree once and publish the action feedback
-      status = p_->tree.tickExactlyOnce();
-
-      if(const auto res = onLoopAfterTick(status); res.has_value())
-      {
-        stop_action(res.value(), "Action Server aborted by onLoopAfterTick()");
-        goal_handle->abort(action_result);
-        return;
-      }
-
-      if(const auto res = onLoopFeedback(); res.has_value())
-      {
-        auto feedback = std::make_shared<ExecuteTree::Feedback>();
-        feedback->message = res.value();
-        goal_handle->publish_feedback(feedback);
-      }
-
-      const auto now = std::chrono::steady_clock::now();
-      if(now < loop_deadline)
-      {
-        p_->tree.sleep(std::chrono::duration_cast<std::chrono::system_clock::duration>(
-            loop_deadline - now));
-      }
-      loop_deadline += period;
+      action_result->return_message = msg.value();
     }
-  }
-  catch(const std::exception& ex)
+    else
+    {
+      action_result->return_message = message;
+    }
+    RCLCPP_WARN(kLogger, action_result->return_message.c_str());
+  };
+
+  while(rclcpp::ok() && status == BT::NodeStatus::RUNNING)
   {
-    action_result->return_message = std::string("Behavior Tree exception:") + ex.what();
-    RCLCPP_ERROR(kLogger, action_result->return_message.c_str());
-    goal_handle->abort(action_result);
-    return;
-  }
+    if(goal_handle->is_canceling())
+    {
+      stop_action(status, "Action Server canceling and halting Behavior Tree");
+      goal_handle->canceled(action_result);
+      return;
+    }
 
+    // tick the tree once and publish the action feedback
+    status = p_->tree.tickExactlyOnce();
+
+    if(const auto res = onLoopAfterTick(status); res.has_value())
+    {
+      stop_action(res.value(), "Action Server aborted by onLoopAfterTick()");
+      goal_handle->abort(action_result);
+      return;
+    }
+
+    if(const auto res = onLoopFeedback(); res.has_value())
+    {
+      auto feedback = std::make_shared<ExecuteTree::Feedback>();
+      feedback->message = res.value();
+      goal_handle->publish_feedback(feedback);
+    }
+
+    const auto now = std::chrono::steady_clock::now();
+    if(now < loop_deadline)
+    {
+      p_->tree.sleep(std::chrono::duration_cast<std::chrono::system_clock::duration>(
+          loop_deadline - now));
+    }
+    loop_deadline += period;
+  }
+ 
   // Call user defined onTreeExecutionCompleted function.
   // Override the message value if the user defined function returns it
   if(auto msg = onTreeExecutionCompleted(status, false))

bt_examples/behavior_trees/example_1.xml

@@ -2,7 +2,7 @@
 <root BTCPP_format="4" main_tree_to_execute="ExampleTree1">
     <!-- ////////// -->
     <BehaviorTree ID="ExampleTree1">
-        <Repeat num_cycles="100">
+        <Repeat num_cycles="10">
             <Sequence>
                 <Delay delay_msec="1000">
                     <AlwaysSuccess/>