Unify blackboard and fix segfault

src/btree_nodes/actorTree.xml

@@ -14,18 +14,25 @@
                         <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
                         <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
                     </Control>
-                    <Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
+                    <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
                 </Sequence>
             </ReactiveSequence>
             <Sequence>
                 <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
+                <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
                 <Action ID="SetCalledTo" state="false"/>
             </Sequence>
         </Fallback>
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
+        <Action ID="ActorAnimation">
+            <input_port name="animation">Name of animation to play</input_port>
+        </Action>
+        <Action ID="ActorMovement">
+            <input_port name="animation">Name of animation to play</input_port>
+            <input_port name="target">Pose to move to</input_port>
+        </Action>
         <Condition ID="IsCalled"/>
         <Action ID="SetCalledTo">
             <input_port name="state">state to set called to</input_port>

src/btree_nodes/robotTree.xml

@@ -28,6 +28,13 @@
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
+        <Action ID="ActorAnimation">
+            <input_port name="animation">Name of animation to play</input_port>
+        </Action>
+        <Action ID="ActorMovement">
+            <input_port name="animation">Name of animation to play</input_port>
+            <input_port name="target">Pose to move to</input_port>
+        </Action>
         <Condition ID="IsCalled"/>
         <Action ID="SetCalledTo">
             <input_port name="state">state to set called to</input_port>
@@ -40,10 +47,6 @@
             <input_port name="area">Bounds to check in</input_port>
             <input_port name="pose">Position of object</input_port>
         </Condition>
-        <Action ID="MoveActorToTarget">
-            <input_port name="current">Current actor position</input_port>
-            <input_port name="target">Target to move actor to</input_port>
-        </Action>
         <Action ID="OffsetPose">
             <input_port name="input">initial position as Pose</input_port>
             <input_port name="offset">offset as a Point</input_port>

src/btree_nodes/src/Tree.cpp

@@ -1,4 +1,5 @@
 #include "Area.h"
+#include <behaviortree_cpp_v3/blackboard.h>
 #include <rclcpp/rclcpp.hpp>
 #include <random>
 #include <rclcpp/utilities.hpp>
@@ -19,16 +20,14 @@
 #include <chrono>
 #include <thread>
 
-
-
 int main(int argc, char **argv) {
     rclcpp::init(argc, argv);
     rclcpp::NodeOptions node_options;
     node_options.automatically_declare_parameters_from_overrides(true);
 
     auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
-    auto blackboardVector = std::vector<BT::Blackboard::Ptr>();
+    auto blackboard = Blackboard::create();
-
+    auto blackboardMutex = std::mutex();
 
     rclcpp::executors::SingleThreadedExecutor executor;
 
@@ -108,16 +107,19 @@ int main(int argc, char **argv) {
     NodeBuilder builderActorAnimation = [&actorAnimationNode](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<ActorAnimation>(name, config, actorAnimationNode, "/ActorPlugin/animation");
     };
+    factory.registerBuilder<ActorAnimation>("ActorAnimation",builderActorAnimation);
     executor.add_node(actorAnimationNode);
 
     auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options);
-    NodeBuilder builderActorMovement = [&actorMovementNode, &blackboardVector](const std::string &name, const NodeConfiguration &config) {
+    NodeBuilder builderActorMovement = [&actorMovementNode, &blackboard, &blackboardMutex](const std::string &name, const NodeConfiguration &config) {
-        return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboardVector](std::shared_ptr<const Movement::Feedback> feedback){
+        return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboard,&blackboardMutex](std::shared_ptr<const Movement::Feedback> feedback){
-            for (auto blackboard : blackboardVector){
+            blackboardMutex.lock();
-                blackboard->set("actorPos", feedback);
+            blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
-            }
+            blackboardMutex.unlock();
         });
     };
+    factory.registerBuilder<ActorMovement>("ActorMovement",builderActorMovement);
+    executor.add_node(actorMovementNode);    
 
     bool called = false;
     auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
@@ -138,24 +140,19 @@ int main(int argc, char **argv) {
 
     std::cout << "Creating tree." << std::endl;
 
-    Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml");
+    Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml",blackboard);
-    Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml");
+    Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml",blackboard);
-
-    blackboardVector.push_back(actorTree.rootBlackboard());
-    blackboardVector.push_back(robotTree.rootBlackboard());
 
     auto init = std::make_shared<geometry_msgs::msg::Pose>();
     init->position.x = 6.0;
     init->position.y = 6.0;
 
-    for (auto blackboard : blackboardVector){
+    blackboard->set("safeArea",safeArea);
-        blackboard->set("safeArea",safeArea);
+    blackboard->set("warningArea",warningArea);
-        blackboard->set("warningArea",warningArea);
+    blackboard->set("unsafeArea",unsafeArea);
-        blackboard->set("unsafeArea",unsafeArea);
+    blackboard->set("negativeYTable",negativeYTable);
-        blackboard->set("negativeYTable",negativeYTable);
+    blackboard->set("positiveYTable",positiveYTable);
-        blackboard->set("positiveYTable",positiveYTable);
+    blackboard->set("actorPos", init);
-        blackboard->set("actorPos", init);
-    }
 
     std::cout << "Starting subscriber." << std::endl;
 
@@ -166,13 +163,13 @@ int main(int argc, char **argv) {
         std::cout << "Executor stopped." << std::endl;
     }).detach();
 
-    executor.add_node(actorMovementNode);
-
     std::cout << "Looping." << std::endl;
 
     while (rclcpp::ok()) {
+        blackboardMutex.lock();
+        actorTree.tickRoot();
         robotTree.tickRoot();
-        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+        blackboardMutex.unlock();
     }
 
     std::cout << "End." << std::endl;

src/btree_nodes/src/nodes/ActorMovement.cpp

@@ -3,6 +3,7 @@
 //
 
 #include "ActorMovement.h"
+#include <behaviortree_cpp_v3/basic_types.h>
 
 
 ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback)
@@ -37,30 +38,40 @@ BT::NodeStatus ActorMovement::onStart() {
     goal.animation_speed=1.0;
     goal.target = *target;
 
-
-
     auto send_goal_options = Client<Movement>::SendGoalOptions();
-    send_goal_options.feedback_callback = [=](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, const std::shared_ptr<const Movement::Feedback> feedback) {
+    send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) {
         positionCallback(feedback);
     };
-    send_goal_options.result_callback = [=](const ClientGoalHandle<Movement>::WrappedResult & parameter) {
+    send_goal_options.result_callback = [&](ClientGoalHandle<Movement>::WrappedResult parameter) {
         mutex.lock();
         if(parameter.code == ResultCode::SUCCEEDED){
+            std::cout << "Success?" << std::endl;
             result = BT::NodeStatus::SUCCESS;
         }else{
+            std::cout << "Failure?" << std::endl;
             result = BT::NodeStatus::FAILURE;
         }
         mutex.unlock();
     };
+    send_goal_options.goal_response_callback = [&](ClientGoalHandle<Movement>::SharedPtr parameter){
+        if(!parameter){
+            std::cout << "ActorMovement rejected by server." << std::endl;
+            result = BT::NodeStatus::FAILURE;
+        }
+    };
 
     client->async_send_goal(goal,send_goal_options);
     
+    result = BT::NodeStatus::RUNNING;
     return BT::NodeStatus::RUNNING;
 }
 
 BT::NodeStatus ActorMovement::onRunning() {
     mutex.lock();
     auto status = result;
+    if(result != BT::NodeStatus::RUNNING){
+        result = BT::NodeStatus::IDLE;
+    }
     mutex.unlock();
     return status;
 }
@@ -68,4 +79,5 @@ BT::NodeStatus ActorMovement::onRunning() {
 void ActorMovement::onHalted() {
     std::cout << "halted move" << std::endl;
     client->async_cancel_all_goals();
+    result = BT::NodeStatus::IDLE;
 }

src/btree_nodes/src/nodes/MoveConnection.cpp

@@ -4,6 +4,8 @@
 
 #include "MoveConnection.h"
 
+MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {}
+
 void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
                                     const std::function<void(bool)> &callback) {
     std::cout<<"planAndExecute."<<std::endl;
@@ -56,10 +58,6 @@ void MoveConnection::setSpeedMultiplier(const double speed){
     }
 }
 
-MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {
-
-}
-
 void MoveConnection::stop() {
     std::cout<<"Stopping MoveConnection"<<std::endl;
     cancelled = true;

src/ros_actor_action_server/src/Server.cpp

@@ -3,20 +3,35 @@
 //
 
 #include "Server.hpp"
-
+#include <condition_variable>
 
 using namespace ros_actor_message_queue_msgs;
 
 static const char* feedbackName;
 static const char* actionName;
 
+static std::mutex feedbackMutex;
+static std::mutex rosMutex;
+
+static std::condition_variable stateCondition;
+
+static ActionMessage cancelAction;
+
 void sendAction(mqd_t queue, ActionMessage *message) { 
     std::cout << "Sending action..." << std::endl;
     mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); 
     std::cout << "Sent action" << std::endl;
 }
 
+void waitForState(FeedbackMessage *currentFeedback,ActorPluginState state){
+    std::unique_lock lock(feedbackMutex);
+    stateCondition.wait(lock,[&currentFeedback,state]{return currentFeedback->state==state;});
+    
+    return;
+}
+
 int main(int argc, char **argv) {
+    cancelAction.state = IDLE;
     rclcpp::init(argc, argv);
 
     auto node = rclcpp::Node::make_shared("ActorPlugin", "ActorPlugin");
@@ -64,90 +79,89 @@ int main(int argc, char **argv) {
 
     FeedbackMessage currentFeedback;
     ActionMessage currentAction;
-    std::mutex mutex;
 
-    currentAction.state = IDLE;
+    std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction] {
-
-    std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction, &mutex] {
         while (true) {
             FeedbackMessage newFeedback;
             if(mq_receive(feedbackQueue, (char *)&newFeedback, sizeof(ros_actor_message_queue_msgs::FeedbackMessage), nullptr)==-1){
                 perror(nullptr);
             }
-            mutex.lock();
+            feedbackMutex.lock();
-            if (newFeedback.state != currentFeedback.state) {
+            if(newFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
-                currentAction.state = newFeedback.state;
+                auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Animation_Feedback>();
+                feedback->progress = newFeedback.progress;
+                currentAnimationGoalPtr->publish_feedback(feedback);
             }
-            if(currentAction.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
+            if(newFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
-                if(newFeedback.progress<1){
+                auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
-                    auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Animation_Feedback>();
+                feedback->current = newFeedback.current;
-                    feedback->progress = newFeedback.progress;
+                currentMovementGoalPtr->publish_feedback(feedback);
-                    currentAnimationGoalPtr->publish_feedback(feedback);
+            }
-                }else{
+            if (newFeedback.state==IDLE) {
+                if(currentFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
                     currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
                     currentAnimationGoalPtr = nullptr;
                 }
-            }
+                if(currentFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
-            if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
-                if(newFeedback.progress<1){
-                    auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
-                    feedback->current = newFeedback.current;
-                    currentMovementGoalPtr->publish_feedback(feedback);
-                }else{
                     currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
                     currentMovementGoalPtr = nullptr;
                 }
             }
             currentFeedback = newFeedback;
-            mutex.unlock();
+            feedbackMutex.unlock();
+            stateCondition.notify_all();
         }
     }).detach();
 
     auto animationServer = rclcpp_action::create_server<action::Animation>(
         node, "animation",
-        [&currentAction, &mutex](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
+        [&currentAction, &currentFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
             // igndbg << "goal" << shared_mmap->currentState << std::endl;
-            mutex.lock();
+            rosMutex.lock();
 
-            if (currentAction.state == IDLE) {
+            if (currentFeedback.state == IDLE) {
                 std::cout << "Accepting..." << std::endl;
+                
                 strcpy(currentAction.animationName, animationGoal->animation_name.data());
 
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
             } else {
                 std::cout << "Rejecting..." << std::endl;
-                mutex.unlock();
+                rosMutex.unlock();
                 return rclcpp_action::GoalResponse::REJECT;
             }
         },
-        [actionQueue, &currentAction, &mutex](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
+        [actionQueue, &currentFeedback](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
-            if (currentAction.state == IDLE) {
+            if (currentFeedback.state == IDLE) {
                 return rclcpp_action::CancelResponse::REJECT;
             } else {
-                mutex.lock();
+                rosMutex.lock();
+                sendAction(actionQueue, &cancelAction);
                 
-                currentAction.state = IDLE;
+                waitForState(&currentFeedback, IDLE);
-                sendAction(actionQueue, &currentAction);
+                rosMutex.unlock();
-
-                mutex.unlock();
 
                 return rclcpp_action::CancelResponse::ACCEPT;
             }
         },
-        [actionQueue, &currentAction, &currentAnimationGoalPtr, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
+        [actionQueue, &currentAction, &currentAnimationGoalPtr, &currentFeedback](const AnimationServerGoalPtr &animationGoalPtr) {
+            feedbackMutex.lock();
             currentAnimationGoalPtr = animationGoalPtr;
+            feedbackMutex.unlock();
             currentAction.state = ANIMATION;
             sendAction(actionQueue, &currentAction);
 
-            mutex.unlock();
+            waitForState(&currentFeedback, ANIMATION);
+            rosMutex.unlock();
         });
 
     auto movementServer = rclcpp_action::create_server<action::Movement>(
         node, "movement",
-        [&currentAction, &mutex](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
+        [&currentAction, &currentFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
-            mutex.lock();
+            rosMutex.lock();
-            if (currentAction.state == IDLE) {
+            if (currentFeedback.state == IDLE) {
                 std::cout << "Accepting..." << std::endl;
+                
                 currentAction.target = movementGoal->target;
                 currentAction.animationDistance = movementGoal->animation_distance;
                 strcpy(currentAction.animationName, movementGoal->animation_name.data());
@@ -155,34 +169,35 @@ int main(int argc, char **argv) {
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
             } else {
                 std::cout << "Rejecting..." << std::endl;
-                mutex.unlock();
+                rosMutex.unlock();
                 return rclcpp_action::GoalResponse::REJECT;
             }
         },
-        [actionQueue, &currentAction, &mutex](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
+        [actionQueue, &currentFeedback](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
-            if (currentAction.state == IDLE) {
+            if (currentFeedback.state == IDLE) {
                 return rclcpp_action::CancelResponse::REJECT;
             } else {
-                mutex.lock();
+                rosMutex.lock();
+                sendAction(actionQueue, &cancelAction);
                 
-                currentAction.state = IDLE;
+                waitForState(&currentFeedback, IDLE);
-                sendAction(actionQueue, &currentAction);
+                rosMutex.unlock();
                 
-                mutex.unlock();
                 return rclcpp_action::CancelResponse::ACCEPT;
             }
         },
-        [actionQueue, &currentAction, &currentMovementGoalPtr, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
+        [actionQueue, &currentAction, &currentFeedback, &currentMovementGoalPtr](const MovementServerGoalPtr &movementGoalPtr) {
+            feedbackMutex.lock();
             currentMovementGoalPtr = movementGoalPtr;
+            feedbackMutex.unlock();
             currentAction.state = MOVEMENT;
             sendAction(actionQueue, &currentAction);
             
-            mutex.unlock();
+            waitForState(&currentFeedback, MOVEMENT);
+            rosMutex.unlock();
         });
     
     while(rclcpp::ok()){
         rclcpp::spin(node);
     }
-    
-    
 }