Feat/bdd exec comm

CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(bdd_ros2_interfaces REQUIRED)
 find_package(coord2b REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 include_directories(
   include
@@ -35,6 +36,7 @@ target_link_libraries(
   ${rclcpp_TARGETS}
   ${rclcpp_action_TARGETS}
   ${rclcpp_components_TARGETS}
+  yaml-cpp
 )
 
 install(TARGETS collab_pickplace_node
@@ -43,4 +45,7 @@ install(TARGETS collab_pickplace_node
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME})
 
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME})
+
 ament_package()

config/mockup_bhv_topics.yaml

@@ -0,0 +1,3 @@
+located_at_pick: "/obs_policy/located_at_pick_ws"
+is_held: "/obs_policy/is_held"
+located_at_place: "/obs_policy/located_at_place_ws"

include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp

@@ -24,7 +24,9 @@
 #include <string_view>
 #include <thread>
 #include "bdd_ros2_interfaces/action/behaviour.hpp"
-#include "bdd_collab_bhv_cpp/collab_pickplace.hpp"
+#include "bdd_ros2_interfaces/msg/event.hpp"
+#include "bdd_ros2_interfaces/msg/trinary_stamped.hpp"
+#include "bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp"
 
 namespace bdd_collab_bhv {
 
@@ -49,6 +51,8 @@ class CollabPickplaceNode : public rclcpp::Node
   public:
     using Behaviour           = bdd_ros2_interfaces::action::Behaviour;
     using GoalHandleBehaviour = rclcpp_action::ServerGoalHandle<Behaviour>;
+    using Event               = bdd_ros2_interfaces::msg::Event;
+    using TrinaryStamped      = bdd_ros2_interfaces::msg::TrinaryStamped;
 
     explicit CollabPickplaceNode(const rclcpp::NodeOptions &pOptions = rclcpp::NodeOptions());
 
@@ -59,16 +63,26 @@ class CollabPickplaceNode : public rclcpp::Node
   private:
     rclcpp_action::Server<Behaviour>::SharedPtr mBhvServerPtr;
 
+    rclcpp::Publisher<Event>::SharedPtr mEventPublisher;
+
+    rclcpp::Publisher<TrinaryStamped>::SharedPtr mLocatedPickPublisher;
+    rclcpp::Publisher<TrinaryStamped>::SharedPtr mIsHeldPublisher;
+    rclcpp::Publisher<TrinaryStamped>::SharedPtr mLocatedPlacePublisher;
+
     std::thread   mFsmThread;
     std::mutex    mFsmMutex;
     std::mutex    mGoalMutex;
     ExecutionType mExecCtx;
 
+    std::string mLocatedPickTopic;
+    std::string mIsHeldTopic;
+    std::string mLocatedPlaceTopic;
+
     // Shared data between server handlers & FSM loop
     struct GoalData
     {
-        std::weak_ptr<GoalHandleBehaviour> mGoalHandlerPtr;
-        Behaviour::Goal                    mGoalCopy;
+        std::shared_ptr<GoalHandleBehaviour> mGoalHandlerPtr;
+        Behaviour::Goal                      mGoalCopy;
     };
 
     std::optional<GoalData> mPendingGoal;
@@ -83,6 +97,15 @@ class CollabPickplaceNode : public rclcpp::Node
     // Functions
     void fsm_loop();
 
+    // Exported events
+    static constexpr const unsigned int EXPORTED_EVENTS[] = {
+        collab_pickplace::E_TABLE_TOUCHED,
+        collab_pickplace::E_GRASP_DONE,
+        collab_pickplace::E_PLACE_REACHED,
+        collab_pickplace::E_RELEASE_DONE,
+    };
+    static constexpr unsigned int NUM_EXPORTED_EVENTS = 4;
+
 }; // CollabPickplaceNode
 } // namespace bdd_collab_bhv
 #endif // BDD_COLLAB_BHV__HUMAN_PICKPLACE_MOCKUP_NODE_HPP_

include/bdd_collab_bhv_cpp/fsm_behaviours.hpp

@@ -18,27 +18,60 @@
 #include <memory>
 #include <rclcpp/time.hpp>
 #include <rclcpp/node.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include "rclcpp/publisher.hpp"
+#include "unique_identifier_msgs/msg/uuid.hpp"
 #include "coord2b/types/fsm.h"
+#include "bdd_ros2_interfaces/msg/trinary_stamped.hpp"
+#include "bdd_ros2_interfaces/action/behaviour.hpp"
 
 namespace bdd_collab_bhv {
 
 class BehaviourInterface
 {
   public:
-    virtual void step(std::shared_ptr<rclcpp::Node> pNodePtr, const struct fsm_nbx *pFsmPtr) = 0;
+    using UUID                = unique_identifier_msgs::msg::UUID;
+    using Behaviour           = bdd_ros2_interfaces::action::Behaviour;
+    using GoalHandleBehaviour = rclcpp_action::ServerGoalHandle<Behaviour>;
+
+    virtual void step(
+      std::shared_ptr<rclcpp::Node>        pNodePtr,
+      const struct fsm_nbx                *pFsmPtr,
+      const UUID                          &pScenarioContextId = UUID(),
+      std::shared_ptr<GoalHandleBehaviour> pGoalHandlePtr     = nullptr
+    )                             = 0;
     virtual ~BehaviourInterface() = default;
 };
 
 class MockupCollabBehaviour : public BehaviourInterface
 {
   public:
-    MockupCollabBehaviour(rclcpp::Time pNow, uint pHeartbeatDurMiliSec);
+    using TrinaryStamped = bdd_ros2_interfaces::msg::TrinaryStamped;
+
+    MockupCollabBehaviour(
+      rclcpp::Time                                 pNow,
+      uint                                         pHeartbeatDurMiliSec,
+      rclcpp::Publisher<TrinaryStamped>::SharedPtr pLocatedPickPublisher,
+      rclcpp::Publisher<TrinaryStamped>::SharedPtr pIsHeldPublisher,
+      rclcpp::Publisher<TrinaryStamped>::SharedPtr pLocatedPlacePublisher
+    );
 
-    void step(std::shared_ptr<rclcpp::Node> pNodePtr, const struct fsm_nbx *pFsmPtr) override;
+    void step(
+      std::shared_ptr<rclcpp::Node>        pNodePtr,
+      const struct fsm_nbx                *pFsmPtr,
+      const UUID                          &pScenarioContextId = UUID(),
+      std::shared_ptr<GoalHandleBehaviour> pGoalHandlePtr     = nullptr
+    ) override;
 
   private:
     rclcpp::Duration mHeartbeatPeriod;
     rclcpp::Time     mNextHeartbeat;
+
+    std::shared_ptr<Behaviour::Feedback> mFeedbackPtr;
+
+    rclcpp::Publisher<TrinaryStamped>::SharedPtr mLocatedPickPublisher;
+    rclcpp::Publisher<TrinaryStamped>::SharedPtr mIsHeldPublisher;
+    rclcpp::Publisher<TrinaryStamped>::SharedPtr mLocatedPlacePublisher;
 };
 
 } // namespace bdd_collab_bhv

include/bdd_collab_bhv_cpp/utils.hpp

@@ -0,0 +1,41 @@
+#ifndef BDD_COLLAB_BHV__UTILS_HPP
+#define BDD_COLLAB_BHV__UTILS_HPP
+
+#include <string>
+#include <map>
+#include <stdexcept>
+#include <yaml-cpp/yaml.h>
+
+namespace bdd_collab_bhv {
+
+using TopicConfig = std::map<std::string, std::string>;
+
+inline TopicConfig load_topics(const std::string &file_path)
+{
+    try {
+        YAML::Node root = YAML::LoadFile(file_path);
+
+        TopicConfig topics;
+        topics["located_at_pick"]  = root["located_at_pick"].as<std::string>();
+        topics["is_held"]          = root["is_held"].as<std::string>();
+        topics["located_at_place"] = root["located_at_place"].as<std::string>();
+
+        if (
+          topics["located_at_pick"].empty() || topics["is_held"].empty()
+          || topics["located_at_place"].empty()
+        ) {
+            throw std::runtime_error(
+              "YAML must contain non-empty values for 'located_at_pick', 'is_held', and "
+              "'located_at_place'"
+            );
+        }
+
+        return topics;
+    } catch (const YAML::Exception &e) {
+        throw std::runtime_error(std::string("Failed to parse YAML file: ") + e.what());
+    }
+}
+
+} // namespace bdd_collab_bhv
+
+#endif // BDD_COLLAB_BHV__UTILS_HPP

launch/launch_mockup_behaviour.yaml

@@ -7,6 +7,12 @@ launch:
   - arg:
       name: "bhv_server_name"
       default: "/bhv_server"
+  - arg:
+      name: "event_topic"
+      default: "/bdd/events"
+  - arg:
+      name: "topic_config"
+      default: "$(find-pkg-share bdd_collab_bhv_cpp)/config/mockup_bhv_topics.yaml"
   - node:
       pkg: "bdd_collab_bhv_cpp"
       exec: "collab_pickplace_node"
@@ -15,3 +21,7 @@ launch:
       param:
         - name: "bhv_server_name"
           value: "$(var bhv_server_name)"
+        - name: "event_topic"
+          value: "$(var event_topic)"
+        - name: "topic_config"
+          value: "$(var topic_config)"

src/collab_pickplace_node.cpp

@@ -26,13 +26,14 @@
 #include <rclcpp_action/server.hpp>
 #include <rclcpp/utilities.hpp>
 #include <utility>
-#include "bdd_collab_bhv_cpp/collab_pickplace.hpp"
+#include "bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp"
 #include "coord2b/functions/event_loop.h"
 #include "coord2b/functions/fsm.h"
 #include "bdd_ros2_interfaces/action/behaviour.hpp"
 #include "bdd_collab_bhv_cpp/conversions.hpp"
 #include "bdd_collab_bhv_cpp/fsm_behaviours.hpp"
 #include "bdd_collab_bhv_cpp/collab_pickplace_node.hpp"
+#include "bdd_collab_bhv_cpp/utils.hpp"
 
 #define TICK_MILI_SECS 1        // 1kHz
 #define HEARTBEAT_MILI_SECS 500 // 2Hz
@@ -47,6 +48,8 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption
 
     declare_parameter<std::string>("bhv_server_name", "bhv_server");
     declare_parameter<std::string>("exec_context", "mockup");
+    declare_parameter<std::string>("event_topic", "");
+    declare_parameter<std::string>("topic_config", "");
 
     auto execCtxStr = get_parameter("exec_context").as_string();
     if (execCtxStr.compare("mockup") == 0) {
@@ -105,6 +108,32 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption
     mBhvServerPtr                = rclcpp_action::create_server<Behaviour>(
       this, serverName, goalHandler, cancelHandler, accepted_handler
     );
+
+    const std::string eventTopic = get_parameter("event_topic").as_string();
+    if (eventTopic.empty()) {
+        throw std::runtime_error("'event_topic' parameter is empty or not provided");
+    }
+    RCLCPP_INFO(this->get_logger(), "Publishing events on topic: %s", eventTopic.c_str());
+    mEventPublisher = this->create_publisher<Event>(eventTopic, rclcpp::QoS(10));
+
+    const std::string topicConfigPath = get_parameter("topic_config").as_string();
+    if (topicConfigPath.empty()) {
+        throw std::runtime_error("'topic_config' parameter is empty or not provided");
+    }
+    try {
+        auto topicConfig   = bcb::load_topics(topicConfigPath);
+        mLocatedPickTopic  = topicConfig["located_at_pick"];
+        mIsHeldTopic       = topicConfig["is_held"];
+        mLocatedPlaceTopic = topicConfig["located_at_place"];
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to load topic config: %s", e.what());
+        throw;
+    }
+    mLocatedPickPublisher =
+      this->create_publisher<TrinaryStamped>(mLocatedPickTopic, rclcpp::QoS(10));
+    mIsHeldPublisher = this->create_publisher<TrinaryStamped>(mIsHeldTopic, rclcpp::QoS(10));
+    mLocatedPlacePublisher =
+      this->create_publisher<TrinaryStamped>(mLocatedPlaceTopic, rclcpp::QoS(10));
 }
 
 void bcb::CollabPickplaceNode::start_fsm()
@@ -137,15 +166,23 @@ void bcb::CollabPickplaceNode::fsm_loop()
     std::unique_ptr<BehaviourInterface> bhvInfPtr = nullptr;
     switch (mExecCtx) {
     case ExecutionType::Mockup:
-        bhvInfPtr = std::make_unique<MockupCollabBehaviour>(now, HEARTBEAT_MILI_SECS);
+        bhvInfPtr = std::make_unique<MockupCollabBehaviour>(
+          now, HEARTBEAT_MILI_SECS, mLocatedPickPublisher, mIsHeldPublisher, mLocatedPlacePublisher
+        );
         break;
     default:
         throw std::runtime_error(
           std::format("Unhandled execution context type: '{}'", exec_type_to_str(mExecCtx))
         );
     }
 
+    auto  response = std::make_shared<Behaviour::Result>();
+    auto  feedback = std::make_shared<Behaviour::Feedback>();
+    Event evt_msg;
+
     while (rclcpp::ok() && mFsmLoopRunning.load(std::memory_order_acquire)) {
+        now = this->get_clock()->now();
+
         if (!processingGoal) {
             activeGoal = std::move(mPendingGoal);
             mPendingGoal.reset();
@@ -155,12 +192,62 @@ void bcb::CollabPickplaceNode::fsm_loop()
             std::lock_guard<std::mutex> lockFsm(mFsmMutex);
             produce_event(mFsmPtr->eventData, collab_pickplace::E_STEP);
 
+            response->result.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id;
+
+            // Event publishing
+            for (unsigned int evt : CollabPickplaceNode::EXPORTED_EVENTS) {
+                if (!consume_event(mFsmPtr->eventData, evt)) continue;
+                evt_msg.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id;
+                evt_msg.stamp               = now;
+                evt_msg.uri                 = collab_pickplace::EVENT_URIS[evt];
+                mEventPublisher->publish(evt_msg);
+            }
+
             // Goal handling
             if (activeGoal && !processingGoal) {
                 produce_event(mFsmPtr->eventData, collab_pickplace::E_NEW_GOAL);
                 processingGoal = true;
             }
             if (consume_event(mFsmPtr->eventData, collab_pickplace::E_GOAL_FINISHED)) {
+                response->result.stamp         = now;
+                response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE;
+
+                if (auto goalHandle = activeGoal->mGoalHandlerPtr) {
+                    goalHandle->succeed(response);
+                } else {
+                    RCLCPP_ERROR(
+                      this->get_logger(),
+                      "Goal finished but no active goal handle: %s",
+                      uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str()
+                    );
+                }
+
+                processingGoal = false;
+                activeGoal.reset();
+            }
+
+            if (mCancelRequested.load(std::memory_order_acquire)) {
+                produce_event(mFsmPtr->eventData, collab_pickplace::E_GOAL_CANCELLED);
+                mCancelRequested.store(false, std::memory_order_release);
+                RCLCPP_INFO(
+                  this->get_logger(),
+                  "Goal cancel requested: %s",
+                  uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str()
+                );
+
+                response->result.stamp         = now;
+                response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE;
+
+                if (auto goalHandle = activeGoal->mGoalHandlerPtr) {
+                    goalHandle->canceled(response);
+                } else {
+                    RCLCPP_ERROR(
+                      this->get_logger(),
+                      "Goal cancel requested but no active goal handle: %s",
+                      uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str()
+                    );
+                }
+
                 processingGoal = false;
                 activeGoal.reset();
             }
@@ -170,7 +257,16 @@ void bcb::CollabPickplaceNode::fsm_loop()
             }
 
             // Behaviour & FSM update
-            bhvInfPtr->step(nodePtr, mFsmPtr.get());
+            if (activeGoal && processingGoal) {
+                bhvInfPtr->step(
+                  nodePtr,
+                  mFsmPtr.get(),
+                  activeGoal->mGoalCopy.scenario_context_id,
+                  activeGoal->mGoalHandlerPtr
+                );
+            } else {
+                bhvInfPtr->step(nodePtr, mFsmPtr.get());
+            }
             fsm_step_nbx(mFsmPtr.get());
             reconfig_event_buffers(mFsmPtr->eventData);
         }