fix breakoff condition

Author: juliangaal

advanced/include/mbf_advanced/mbf_cpp_client.h

@@ -53,7 +53,7 @@ struct MBFClient
     {
         if (at_end())
         {
-            throw std::runtime_error("Reached end! No more next poses");
+            throw std::runtime_error("Reached end! No more next poses. Exiting BT.");
         }
 
         auto result = log_move(*it_);

advanced/src/mbf_behavior_tree.cpp

@@ -28,17 +28,7 @@ class AttemptNext : public BT::SyncActionNode
         if (mbfclient_)
         {
             ROS_INFO_STREAM("BT: " << this->name());
-
-            try
-            {
-                auto status = mbfclient_->next_move() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
-                return status;
-            }
-            catch (const std::runtime_error& e)
-            {
-                ROS_INFO_STREAM("Reached end of circle poses: " << e.what() << ". Returning BT::NodeStatus::Failure to break our of BT.");
-                return BT::NodeStatus::FAILURE;
-            }
+            return mbfclient_->next_move() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
         }
 
         return BT::NodeStatus::FAILURE;

advanced/src/test.cpp

@@ -0,0 +1,59 @@
+#include <behaviortree_cpp_v3/bt_factory.h>
+
+BT::NodeStatus start()
+{
+    std::cout << "start" << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus stop()
+{
+    std::cout << "stop" << std::endl;
+    return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus final_stop()
+{
+    std::cout << "final_stop" << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+//class Repeat: BT::SimpleActionNode
+//{
+//    Repeat(const std::string& name, const BT::NodeConfiguration &config)
+//    : BT::RepeatNode(name, config)
+//    {}
+//
+//    static BT::PortsList providedPorts()
+//    {
+//        return { BT::InputPort<int>("n_tries")};
+//    }
+//
+//    BT::NodeStatus tick() override
+//    {
+//        BT::Optional<int> msg = getInput<int>("n_tries");
+//        if (!msg)
+//        {
+//            throw BT::RuntimeError("missing required input [n_tries]: ",
+//                                   msg.error() );
+//        }
+//
+//        // use the method value() to extract the valid message.
+//        std::cout << "Robot says: " << msg.value() << std::endl;
+//        return BT::NodeStatus::SUCCESS;
+//    }
+//};
+
+int main(int argc, char** argv)
+{
+    BT::BehaviorTreeFactory factory;
+    factory.registerSimpleCondition("start", std::bind(start));
+    factory.registerSimpleCondition("stop", std::bind(stop));
+//    factory.registerNodeType<Repeat>("Repeat");
+    factory.registerSimpleCondition("final_stop", std::bind(final_stop));
+
+    auto tree = factory.createTreeFromFile(BT_XML_PATH);
+    tree.tickRoot();
+
+    return 0;
+}