From c82d834cb5b646bbd4d5922ef78b4c469c2ab040 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Mon, 4 May 2026 16:08:40 +0200 Subject: [PATCH 01/12] add result response and goal cancelling to goal handling in FSM loop --- .../collab_pickplace_node.hpp | 4 +- src/collab_pickplace_node.cpp | 43 ++++++++++++++++++- 2 files changed, 44 insertions(+), 3 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index f643c3a..dc3e94f 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -67,8 +67,8 @@ class CollabPickplaceNode : public rclcpp::Node // Shared data between server handlers & FSM loop struct GoalData { - std::weak_ptr mGoalHandlerPtr; - Behaviour::Goal mGoalCopy; + std::shared_ptr mGoalHandlerPtr; + Behaviour::Goal mGoalCopy; }; std::optional mPendingGoal; diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 8ad83a1..e68e8b7 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -154,12 +154,53 @@ void bcb::CollabPickplaceNode::fsm_loop() std::lock_guard lockFsm(mFsmMutex); produce_event(mFsmPtr->eventData, E_STEP); + auto response = std::make_shared(); + response->result.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; + auto feedback = std::make_shared(); + feedback->scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; + // Goal handling if (activeGoal && !processingGoal) { produce_event(mFsmPtr->eventData, E_NEW_GOAL); processingGoal = true; } - if (consume_event(mFsmPtr->eventData, E_GOAL_FINISHED)) { + if (activeGoal && consume_event(mFsmPtr->eventData, E_GOAL_FINISHED)) { + response->result.stamp = this->get_clock()->now(); + response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; + RCLCPP_INFO( + this->get_logger(), + "Goal finished: %s", + uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() + ); + + auto goal_handle = activeGoal->mGoalHandlerPtr; + if (goal_handle) { + goal_handle->succeed(response); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + + processingGoal = false; + activeGoal.reset(); + } + + if (mCancelRequested.load(std::memory_order_acquire)) { + produce_event(mFsmPtr->eventData, 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 = this->get_clock()->now(); + response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE; + + auto goal_handle = activeGoal->mGoalHandlerPtr; + if (goal_handle) { + goal_handle->canceled(response); + RCLCPP_INFO(this->get_logger(), "Goal cancelled"); + } + processingGoal = false; activeGoal.reset(); } From eddca6f6feca6fd0bee8519e2b28b838be28013f Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Tue, 5 May 2026 15:03:43 +0200 Subject: [PATCH 02/12] add heartbeat feedback publishing to FSM loop --- src/collab_pickplace_node.cpp | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index e68e8b7..5f597eb 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -133,6 +133,9 @@ void bcb::CollabPickplaceNode::fsm_loop() auto now = this->get_clock()->now(); auto nxtTick = now + period; + auto heartbeatPeriod = rclcpp::Duration(std::chrono::milliseconds(HEARTBEAT_MILI_SECS)); + auto nextHeartbeat = now + heartbeatPeriod; + std::unique_ptr bhvInfPtr = nullptr; switch (mExecCtx) { case ExecutionType::Mockup: @@ -145,6 +148,8 @@ void bcb::CollabPickplaceNode::fsm_loop() } while (rclcpp::ok() && mFsmLoopRunning.load(std::memory_order_acquire)) { + now = this->get_clock()->now(); + if (!processingGoal) { activeGoal = std::move(mPendingGoal); mPendingGoal.reset(); @@ -167,17 +172,9 @@ void bcb::CollabPickplaceNode::fsm_loop() if (activeGoal && consume_event(mFsmPtr->eventData, E_GOAL_FINISHED)) { response->result.stamp = this->get_clock()->now(); response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - RCLCPP_INFO( - this->get_logger(), - "Goal finished: %s", - uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() - ); auto goal_handle = activeGoal->mGoalHandlerPtr; - if (goal_handle) { - goal_handle->succeed(response); - RCLCPP_INFO(this->get_logger(), "Goal succeeded"); - } + if (goal_handle) { goal_handle->succeed(response); } processingGoal = false; activeGoal.reset(); @@ -196,15 +193,22 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE; auto goal_handle = activeGoal->mGoalHandlerPtr; - if (goal_handle) { - goal_handle->canceled(response); - RCLCPP_INFO(this->get_logger(), "Goal cancelled"); - } + if (goal_handle) { goal_handle->canceled(response); } processingGoal = false; activeGoal.reset(); } + if (now >= nextHeartbeat && activeGoal && processingGoal) { + feedback->status = std::format( + "current state: {}", mFsmPtr->states[mFsmPtr->currentStateIndex].name + ); + + auto goal_handle = activeGoal->mGoalHandlerPtr; + if (goal_handle) { goal_handle->publish_feedback(feedback); } + while (nextHeartbeat < now) nextHeartbeat += heartbeatPeriod; + } + if (mFsmPtr->currentStateIndex == S_IDLE) { mFsmIdle.store(true, std::memory_order_release); } From 37f14879d74174e54634d155e4edef65ea447960 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 6 May 2026 14:27:13 +0200 Subject: [PATCH 03/12] rename collab_pickplace_fsm and update includes --- .../{collab_pickplace.hpp => collab_pickplace_fsm.hpp} | 0 include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp | 2 +- src/collab_pickplace_node.cpp | 2 +- src/fsm_behaviours.cpp | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) rename include/bdd_collab_bhv_cpp/{collab_pickplace.hpp => collab_pickplace_fsm.hpp} (100%) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp similarity index 100% rename from include/bdd_collab_bhv_cpp/collab_pickplace.hpp rename to include/bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index eb681ae..128bd4f 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -24,7 +24,7 @@ #include #include #include "bdd_ros2_interfaces/action/behaviour.hpp" -#include "bdd_collab_bhv_cpp/collab_pickplace.hpp" +#include "bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp" namespace bdd_collab_bhv { diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 9640dff..1dd6def 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -26,7 +26,7 @@ #include #include #include -#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" diff --git a/src/fsm_behaviours.cpp b/src/fsm_behaviours.cpp index 73277d9..f86fc4a 100644 --- a/src/fsm_behaviours.cpp +++ b/src/fsm_behaviours.cpp @@ -15,7 +15,7 @@ #include #include #include "coord2b/functions/event_loop.h" -#include "bdd_collab_bhv_cpp/collab_pickplace.hpp" +#include "bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp" #include "bdd_collab_bhv_cpp/fsm_behaviours.hpp" namespace bcb = bdd_collab_bhv; From 47b438f495bbf06527c61a276818b4d4f66df7de Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 6 May 2026 15:43:12 +0200 Subject: [PATCH 04/12] add publishers for trinary topics --- .../collab_pickplace_node.hpp | 4 +++ src/collab_pickplace_node.cpp | 28 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index 128bd4f..bf235bb 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -59,6 +59,10 @@ class CollabPickplaceNode : public rclcpp::Node private: rclcpp_action::Server::SharedPtr mBhvServerPtr; + rclcpp::Publisher::SharedPtr mLocatedPickPublisher; + rclcpp::Publisher::SharedPtr mIsHeldPublisher; + rclcpp::Publisher::SharedPtr mLocatedPlacePublisher; + std::thread mFsmThread; std::mutex mFsmMutex; std::mutex mGoalMutex; diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 1dd6def..601eac7 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -37,6 +37,10 @@ #define TICK_MILI_SECS 1 // 1kHz #define HEARTBEAT_MILI_SECS 500 // 2Hz +#define TOPIC_LOCATED_PICK "/obs_policy/located_at_pick_ws" +#define TOPIC_IS_HELD "/obs_policy/is_held" +#define TOPIC_LOCATED_PLACE "/obs_policy/located_at_place_ws" + namespace bcb = bdd_collab_bhv; bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOptions) @@ -105,6 +109,16 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption mBhvServerPtr = rclcpp_action::create_server( this, serverName, goalHandler, cancelHandler, accepted_handler ); + + mLocatedPickPublisher = this->create_publisher( + TOPIC_LOCATED_PICK, rclcpp::QoS(10) + ); + mIsHeldPublisher = this->create_publisher( + TOPIC_IS_HELD, rclcpp::QoS(10) + ); + mLocatedPlacePublisher = this->create_publisher( + TOPIC_LOCATED_PLACE, rclcpp::QoS(10) + ); } void bcb::CollabPickplaceNode::start_fsm() @@ -165,6 +179,11 @@ void bcb::CollabPickplaceNode::fsm_loop() auto feedback = std::make_shared(); feedback->scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; + bdd_ros2_interfaces::msg::TrinaryStamped trinaryMsg; + trinaryMsg.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; + trinaryMsg.stamp = now; + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; + // Goal handling if (activeGoal && !processingGoal) { produce_event(mFsmPtr->eventData, collab_pickplace::E_NEW_GOAL); @@ -209,6 +228,15 @@ void bcb::CollabPickplaceNode::fsm_loop() auto goal_handle = activeGoal->mGoalHandlerPtr; if (goal_handle) { goal_handle->publish_feedback(feedback); } + + if (mFsmPtr->currentStateIndex == collab_pickplace::S_TOUCH_TABLE) { + mLocatedPickPublisher->publish(trinaryMsg); + } else if (mFsmPtr->currentStateIndex == collab_pickplace::S_GRASP) { + mIsHeldPublisher->publish(trinaryMsg); + } else if (mFsmPtr->currentStateIndex == collab_pickplace::S_RELEASE) { + mLocatedPlacePublisher->publish(trinaryMsg); + } + while (nextHeartbeat < now) nextHeartbeat += heartbeatPeriod; } From c4780062ed38b3766911c99c62a0dafe14510eb2 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 6 May 2026 16:08:53 +0200 Subject: [PATCH 05/12] add event topic parameter and update publishers in CollabPickplaceNode --- .../collab_pickplace_node.hpp | 12 ++++++++--- src/collab_pickplace_node.cpp | 20 +++++++++---------- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index bf235bb..1c5276e 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -24,6 +24,8 @@ #include #include #include "bdd_ros2_interfaces/action/behaviour.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; + using Event = bdd_ros2_interfaces::msg::Event; + using TrinaryStamped = bdd_ros2_interfaces::msg::TrinaryStamped; explicit CollabPickplaceNode(const rclcpp::NodeOptions &pOptions = rclcpp::NodeOptions()); @@ -59,9 +63,11 @@ class CollabPickplaceNode : public rclcpp::Node private: rclcpp_action::Server::SharedPtr mBhvServerPtr; - rclcpp::Publisher::SharedPtr mLocatedPickPublisher; - rclcpp::Publisher::SharedPtr mIsHeldPublisher; - rclcpp::Publisher::SharedPtr mLocatedPlacePublisher; + rclcpp::Publisher::SharedPtr mEventPublisher; + + rclcpp::Publisher::SharedPtr mLocatedPickPublisher; + rclcpp::Publisher::SharedPtr mIsHeldPublisher; + rclcpp::Publisher::SharedPtr mLocatedPlacePublisher; std::thread mFsmThread; std::mutex mFsmMutex; diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 601eac7..e7f1f4c 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -51,6 +51,7 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption declare_parameter("bhv_server_name", "bhv_server"); declare_parameter("exec_context", "mockup"); + declare_parameter("event_topic", "/bdd/events"); auto execCtxStr = get_parameter("exec_context").as_string(); if (execCtxStr.compare("mockup") == 0) { @@ -110,15 +111,14 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption this, serverName, goalHandler, cancelHandler, accepted_handler ); - mLocatedPickPublisher = this->create_publisher( - TOPIC_LOCATED_PICK, rclcpp::QoS(10) - ); - mIsHeldPublisher = this->create_publisher( - TOPIC_IS_HELD, rclcpp::QoS(10) - ); - mLocatedPlacePublisher = this->create_publisher( - TOPIC_LOCATED_PLACE, rclcpp::QoS(10) - ); + const std::string eventTopic = get_parameter("event_topic").as_string(); + mEventPublisher = this->create_publisher(eventTopic, rclcpp::QoS(10)); + + mLocatedPickPublisher = + this->create_publisher(TOPIC_LOCATED_PICK, rclcpp::QoS(10)); + mIsHeldPublisher = this->create_publisher(TOPIC_IS_HELD, rclcpp::QoS(10)); + mLocatedPlacePublisher = + this->create_publisher(TOPIC_LOCATED_PLACE, rclcpp::QoS(10)); } void bcb::CollabPickplaceNode::start_fsm() @@ -179,7 +179,7 @@ void bcb::CollabPickplaceNode::fsm_loop() auto feedback = std::make_shared(); feedback->scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; - bdd_ros2_interfaces::msg::TrinaryStamped trinaryMsg; + TrinaryStamped trinaryMsg; trinaryMsg.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; trinaryMsg.stamp = now; trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; From e69d7c25ee6145733e67f0057127a23411061183 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Mon, 11 May 2026 15:08:25 +0200 Subject: [PATCH 06/12] add event publishing of exported events in CollabPickplaceNode --- .../bdd_collab_bhv_cpp/collab_pickplace_node.hpp | 9 +++++++++ src/collab_pickplace_node.cpp | 15 +++++++++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index 1c5276e..3424833 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -93,6 +93,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_ diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index e7f1f4c..6d008b9 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -184,6 +184,17 @@ void bcb::CollabPickplaceNode::fsm_loop() trinaryMsg.stamp = now; trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; + // Event publishing + for (unsigned int evt : CollabPickplaceNode::EXPORTED_EVENTS) { + if (consume_event(mFsmPtr->eventData, evt)) { + Event evt_msg; + 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); @@ -192,7 +203,7 @@ void bcb::CollabPickplaceNode::fsm_loop() if ( activeGoal && consume_event(mFsmPtr->eventData, collab_pickplace::E_GOAL_FINISHED) ) { - response->result.stamp = this->get_clock()->now(); + response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; auto goal_handle = activeGoal->mGoalHandlerPtr; @@ -211,7 +222,7 @@ void bcb::CollabPickplaceNode::fsm_loop() uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() ); - response->result.stamp = this->get_clock()->now(); + response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE; auto goal_handle = activeGoal->mGoalHandlerPtr; From a0c9b1224539b66948ed8722bf97bf3433fd8da1 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 13 May 2026 15:12:37 +0200 Subject: [PATCH 07/12] add loading trinary topics from config file --- CMakeLists.txt | 5 +++ config/mockup_bhv_topics.yaml | 3 ++ .../collab_pickplace_node.hpp | 4 ++ include/bdd_collab_bhv_cpp/utils.hpp | 45 +++++++++++++++++++ launch/launch_mockup_behaviour.yaml | 10 +++++ src/collab_pickplace_node.cpp | 37 ++++++++++----- 6 files changed, 94 insertions(+), 10 deletions(-) create mode 100644 config/mockup_bhv_topics.yaml create mode 100644 include/bdd_collab_bhv_cpp/utils.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fd64467..34f387c 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/config/mockup_bhv_topics.yaml b/config/mockup_bhv_topics.yaml new file mode 100644 index 0000000..6ccb5e4 --- /dev/null +++ b/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" diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index 3424833..19f2fd7 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -74,6 +74,10 @@ class CollabPickplaceNode : public rclcpp::Node std::mutex mGoalMutex; ExecutionType mExecCtx; + std::string mLocatedPickTopic; + std::string mIsHeldTopic; + std::string mLocatedPlaceTopic; + // Shared data between server handlers & FSM loop struct GoalData { diff --git a/include/bdd_collab_bhv_cpp/utils.hpp b/include/bdd_collab_bhv_cpp/utils.hpp new file mode 100644 index 0000000..dfeaa81 --- /dev/null +++ b/include/bdd_collab_bhv_cpp/utils.hpp @@ -0,0 +1,45 @@ +#ifndef BDD_COLLAB_BHV__UTILS_HPP +#define BDD_COLLAB_BHV__UTILS_HPP + +#include +#include +#include + +namespace bdd_collab_bhv { + +struct TopicConfig +{ + std::string located_at_pick; + std::string is_held; + std::string located_at_place; +}; + +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(); + topics.is_held = root["is_held"].as(); + topics.located_at_place = root["located_at_place"].as(); + + 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 \ No newline at end of file diff --git a/launch/launch_mockup_behaviour.yaml b/launch/launch_mockup_behaviour.yaml index c5577c3..faedeae 100644 --- a/launch/launch_mockup_behaviour.yaml +++ b/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)" \ No newline at end of file diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 6d008b9..14d4843 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -33,14 +33,11 @@ #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 -#define TOPIC_LOCATED_PICK "/obs_policy/located_at_pick_ws" -#define TOPIC_IS_HELD "/obs_policy/is_held" -#define TOPIC_LOCATED_PLACE "/obs_policy/located_at_place_ws" - namespace bcb = bdd_collab_bhv; bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOptions) @@ -51,7 +48,8 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption declare_parameter("bhv_server_name", "bhv_server"); declare_parameter("exec_context", "mockup"); - declare_parameter("event_topic", "/bdd/events"); + declare_parameter("event_topic", ""); + declare_parameter("topic_config", ""); auto execCtxStr = get_parameter("exec_context").as_string(); if (execCtxStr.compare("mockup") == 0) { @@ -112,13 +110,32 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption ); const std::string eventTopic = get_parameter("event_topic").as_string(); - mEventPublisher = this->create_publisher(eventTopic, rclcpp::QoS(10)); - + if (!eventTopic.empty()) { + RCLCPP_INFO(this->get_logger(), "Publishing events on topic: %s", eventTopic.c_str()); + } else { + throw std::runtime_error("'event_topic' parameter is empty or not provided"); + } + mEventPublisher = this->create_publisher(eventTopic, rclcpp::QoS(10)); + + const std::string topicConfigPath = get_parameter("topic_config").as_string(); + if (!topicConfigPath.empty()) { + 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; + } + } else { + throw std::runtime_error("'topic_config' parameter is empty or not provided"); + } mLocatedPickPublisher = - this->create_publisher(TOPIC_LOCATED_PICK, rclcpp::QoS(10)); - mIsHeldPublisher = this->create_publisher(TOPIC_IS_HELD, rclcpp::QoS(10)); + this->create_publisher(mLocatedPickTopic, rclcpp::QoS(10)); + mIsHeldPublisher = this->create_publisher(mIsHeldTopic, rclcpp::QoS(10)); mLocatedPlacePublisher = - this->create_publisher(TOPIC_LOCATED_PLACE, rclcpp::QoS(10)); + this->create_publisher(mLocatedPlaceTopic, rclcpp::QoS(10)); } void bcb::CollabPickplaceNode::start_fsm() From 55944a8079411797b378bbb58af0d9548ee2b46a Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 20 May 2026 12:13:43 +0200 Subject: [PATCH 08/12] refactor topic handling and minor fixes in CollabPickplaceNode and update load_topics function to use map --- include/bdd_collab_bhv_cpp/utils.hpp | 18 ++++------ src/collab_pickplace_node.cpp | 51 ++++++++++++++-------------- 2 files changed, 33 insertions(+), 36 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/utils.hpp b/include/bdd_collab_bhv_cpp/utils.hpp index dfeaa81..97bdcfd 100644 --- a/include/bdd_collab_bhv_cpp/utils.hpp +++ b/include/bdd_collab_bhv_cpp/utils.hpp @@ -2,17 +2,13 @@ #define BDD_COLLAB_BHV__UTILS_HPP #include +#include #include #include namespace bdd_collab_bhv { -struct TopicConfig -{ - std::string located_at_pick; - std::string is_held; - std::string located_at_place; -}; +using TopicConfig = std::map; inline TopicConfig load_topics(const std::string &file_path) { @@ -20,13 +16,13 @@ inline TopicConfig load_topics(const std::string &file_path) YAML::Node root = YAML::LoadFile(file_path); TopicConfig topics; - topics.located_at_pick = root["located_at_pick"].as(); - topics.is_held = root["is_held"].as(); - topics.located_at_place = root["located_at_place"].as(); + topics["located_at_pick"] = root["located_at_pick"].as(); + topics["is_held"] = root["is_held"].as(); + topics["located_at_place"] = root["located_at_place"].as(); if ( - topics.located_at_pick.empty() || topics.is_held.empty() - || topics.located_at_place.empty() + 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 " diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 14d4843..af7b6f9 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -110,27 +110,25 @@ bcb::CollabPickplaceNode::CollabPickplaceNode(const rclcpp::NodeOptions &pOption ); const std::string eventTopic = get_parameter("event_topic").as_string(); - if (!eventTopic.empty()) { - RCLCPP_INFO(this->get_logger(), "Publishing events on topic: %s", eventTopic.c_str()); - } else { + 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(eventTopic, rclcpp::QoS(10)); const std::string topicConfigPath = get_parameter("topic_config").as_string(); - if (!topicConfigPath.empty()) { - 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; - } - } else { + 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(mLocatedPickTopic, rclcpp::QoS(10)); mIsHeldPublisher = this->create_publisher(mIsHeldTopic, rclcpp::QoS(10)); @@ -179,6 +177,11 @@ void bcb::CollabPickplaceNode::fsm_loop() ); } + auto response = std::make_shared(); + auto feedback = std::make_shared(); + TrinaryStamped trinaryMsg; + Event evt_msg; + while (rclcpp::ok() && mFsmLoopRunning.load(std::memory_order_acquire)) { now = this->get_clock()->now(); @@ -191,25 +194,20 @@ void bcb::CollabPickplaceNode::fsm_loop() std::lock_guard lockFsm(mFsmMutex); produce_event(mFsmPtr->eventData, collab_pickplace::E_STEP); - auto response = std::make_shared(); response->result.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; - auto feedback = std::make_shared(); feedback->scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; - TrinaryStamped trinaryMsg; trinaryMsg.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; trinaryMsg.stamp = now; - trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::UNKNOWN; // Event publishing for (unsigned int evt : CollabPickplaceNode::EXPORTED_EVENTS) { - if (consume_event(mFsmPtr->eventData, evt)) { - Event evt_msg; - 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); - } + 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 @@ -258,10 +256,13 @@ void bcb::CollabPickplaceNode::fsm_loop() if (goal_handle) { goal_handle->publish_feedback(feedback); } if (mFsmPtr->currentStateIndex == collab_pickplace::S_TOUCH_TABLE) { + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; mLocatedPickPublisher->publish(trinaryMsg); } else if (mFsmPtr->currentStateIndex == collab_pickplace::S_GRASP) { + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; mIsHeldPublisher->publish(trinaryMsg); } else if (mFsmPtr->currentStateIndex == collab_pickplace::S_RELEASE) { + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; mLocatedPlacePublisher->publish(trinaryMsg); } From e31a4fd02b12d7efe2d8347d7530134814b1205b Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Wed, 27 May 2026 17:02:10 +0200 Subject: [PATCH 09/12] refactor coordination node, move trinary publishing to fsm behaviours --- .../collab_pickplace_node.hpp | 4 +- include/bdd_collab_bhv_cpp/fsm_behaviours.hpp | 16 ++++- src/collab_pickplace_node.cpp | 63 ++++++++----------- src/fsm_behaviours.cpp | 19 +++++- 4 files changed, 59 insertions(+), 43 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index 19f2fd7..9a498ea 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -81,8 +81,8 @@ class CollabPickplaceNode : public rclcpp::Node // Shared data between server handlers & FSM loop struct GoalData { - std::shared_ptr mGoalHandlerPtr; - Behaviour::Goal mGoalCopy; + std::weak_ptr mGoalHandlerPtr; + Behaviour::Goal mGoalCopy; }; std::optional mPendingGoal; diff --git a/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp b/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp index 1b850b4..46f6c5a 100644 --- a/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp +++ b/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp @@ -18,7 +18,9 @@ #include #include #include +#include "rclcpp/publisher.hpp" #include "coord2b/types/fsm.h" +#include "bdd_ros2_interfaces/msg/trinary_stamped.hpp" namespace bdd_collab_bhv { @@ -32,13 +34,25 @@ class BehaviourInterface 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::SharedPtr pLocatedPickPublisher, + rclcpp::Publisher::SharedPtr pIsHeldPublisher, + rclcpp::Publisher::SharedPtr pLocatedPlacePublisher + ); void step(std::shared_ptr pNodePtr, const struct fsm_nbx *pFsmPtr) override; private: rclcpp::Duration mHeartbeatPeriod; rclcpp::Time mNextHeartbeat; + + rclcpp::Publisher::SharedPtr mLocatedPickPublisher; + rclcpp::Publisher::SharedPtr mIsHeldPublisher; + rclcpp::Publisher::SharedPtr mLocatedPlacePublisher; }; } // namespace bdd_collab_bhv diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index af7b6f9..ae09915 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -169,7 +169,9 @@ void bcb::CollabPickplaceNode::fsm_loop() std::unique_ptr bhvInfPtr = nullptr; switch (mExecCtx) { case ExecutionType::Mockup: - bhvInfPtr = std::make_unique(now, HEARTBEAT_MILI_SECS); + bhvInfPtr = std::make_unique( + now, HEARTBEAT_MILI_SECS, mLocatedPickPublisher, mIsHeldPublisher, mLocatedPlacePublisher + ); break; default: throw std::runtime_error( @@ -177,10 +179,9 @@ void bcb::CollabPickplaceNode::fsm_loop() ); } - auto response = std::make_shared(); - auto feedback = std::make_shared(); - TrinaryStamped trinaryMsg; - Event evt_msg; + auto response = std::make_shared(); + auto feedback = std::make_shared(); + Event evt_msg; while (rclcpp::ok() && mFsmLoopRunning.load(std::memory_order_acquire)) { now = this->get_clock()->now(); @@ -197,10 +198,6 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; feedback->scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; - trinaryMsg.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; - trinaryMsg.stamp = now; - trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::UNKNOWN; - // Event publishing for (unsigned int evt : CollabPickplaceNode::EXPORTED_EVENTS) { if (!consume_event(mFsmPtr->eventData, evt)) continue; @@ -215,14 +212,19 @@ void bcb::CollabPickplaceNode::fsm_loop() produce_event(mFsmPtr->eventData, collab_pickplace::E_NEW_GOAL); processingGoal = true; } - if ( - activeGoal && consume_event(mFsmPtr->eventData, collab_pickplace::E_GOAL_FINISHED) - ) { + if (consume_event(mFsmPtr->eventData, collab_pickplace::E_GOAL_FINISHED)) { response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - auto goal_handle = activeGoal->mGoalHandlerPtr; - if (goal_handle) { goal_handle->succeed(response); } + if (auto goal_handle = activeGoal->mGoalHandlerPtr.lock()) { + goal_handle->succeed(response); + } else { + RCLCPP_ERROR( + this->get_logger(), + "Goal handler expired, no success response for scenario: %s", + uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() + ); + } processingGoal = false; activeGoal.reset(); @@ -240,35 +242,20 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE; - auto goal_handle = activeGoal->mGoalHandlerPtr; - if (goal_handle) { goal_handle->canceled(response); } + if (auto goal_handle = activeGoal->mGoalHandlerPtr.lock()) { + goal_handle->canceled(response); + } else { + RCLCPP_ERROR( + this->get_logger(), + "Goal handler expired, no cancel response for scenario: %s", + uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() + ); + } processingGoal = false; activeGoal.reset(); } - if (now >= nextHeartbeat && activeGoal && processingGoal) { - feedback->status = std::format( - "current state: {}", mFsmPtr->states[mFsmPtr->currentStateIndex].name - ); - - auto goal_handle = activeGoal->mGoalHandlerPtr; - if (goal_handle) { goal_handle->publish_feedback(feedback); } - - if (mFsmPtr->currentStateIndex == collab_pickplace::S_TOUCH_TABLE) { - trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - mLocatedPickPublisher->publish(trinaryMsg); - } else if (mFsmPtr->currentStateIndex == collab_pickplace::S_GRASP) { - trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - mIsHeldPublisher->publish(trinaryMsg); - } else if (mFsmPtr->currentStateIndex == collab_pickplace::S_RELEASE) { - trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - mLocatedPlacePublisher->publish(trinaryMsg); - } - - while (nextHeartbeat < now) nextHeartbeat += heartbeatPeriod; - } - if (mFsmPtr->currentStateIndex == collab_pickplace::S_IDLE) { mFsmIdle.store(true, std::memory_order_release); } diff --git a/src/fsm_behaviours.cpp b/src/fsm_behaviours.cpp index f86fc4a..8802fc0 100644 --- a/src/fsm_behaviours.cpp +++ b/src/fsm_behaviours.cpp @@ -20,8 +20,16 @@ namespace bcb = bdd_collab_bhv; -bcb::MockupCollabBehaviour::MockupCollabBehaviour(rclcpp::Time pNow, uint pHeartbeatDurMiliSec) - : mHeartbeatPeriod(std::chrono::milliseconds(pHeartbeatDurMiliSec)) +bcb::MockupCollabBehaviour::MockupCollabBehaviour( + rclcpp::Time pNow, + uint pHeartbeatDurMiliSec, + rclcpp::Publisher::SharedPtr pLocatedPickPublisher, + rclcpp::Publisher::SharedPtr pIsHeldPublisher, + rclcpp::Publisher::SharedPtr pLocatedPlacePublisher +) + : mHeartbeatPeriod(std::chrono::milliseconds(pHeartbeatDurMiliSec)), + mLocatedPickPublisher(pLocatedPickPublisher), mIsHeldPublisher(pIsHeldPublisher), + mLocatedPlacePublisher(pLocatedPlacePublisher) { mNextHeartbeat = pNow + mHeartbeatPeriod; } void bcb::MockupCollabBehaviour::step( @@ -36,15 +44,22 @@ void bcb::MockupCollabBehaviour::step( pNodePtr->get_logger(), "State: %s", pFsmPtr->states[pFsmPtr->currentStateIndex].name ); + TrinaryStamped trinaryMsg; + trinaryMsg.stamp = now; + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; + if (pFsmPtr->currentStateIndex == collab_pickplace::S_TOUCH_TABLE) { + mLocatedPickPublisher->publish(trinaryMsg); produce_event(pFsmPtr->eventData, collab_pickplace::E_TABLE_TOUCHED); } else if (pFsmPtr->currentStateIndex == collab_pickplace::S_SLIDE) { produce_event(pFsmPtr->eventData, collab_pickplace::E_OBJ_REACHED); } else if (pFsmPtr->currentStateIndex == collab_pickplace::S_GRASP) { + mIsHeldPublisher->publish(trinaryMsg); produce_event(pFsmPtr->eventData, collab_pickplace::E_GRASP_DONE); } else if (pFsmPtr->currentStateIndex == collab_pickplace::S_COLLAB_MOVE) { produce_event(pFsmPtr->eventData, collab_pickplace::E_PLACE_REACHED); } else if (pFsmPtr->currentStateIndex == collab_pickplace::S_RELEASE) { + mLocatedPlacePublisher->publish(trinaryMsg); produce_event(pFsmPtr->eventData, collab_pickplace::E_RELEASE_DONE); } else if (pFsmPtr->currentStateIndex == collab_pickplace::S_RECOVER) { produce_event(pFsmPtr->eventData, collab_pickplace::E_RECOVER_DONE); From ee577d041dd9a929eadd7a6f97a6a26bc692a134 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Mon, 1 Jun 2026 15:33:23 +0200 Subject: [PATCH 10/12] fix missing scenario context id in mockup trinary publishers and revert back to shared pointer for action server goal handler --- .../bdd_collab_bhv_cpp/collab_pickplace_node.hpp | 4 ++-- include/bdd_collab_bhv_cpp/fsm_behaviours.hpp | 13 +++++++++++-- src/collab_pickplace_node.cpp | 10 +++++----- src/fsm_behaviours.cpp | 10 ++++++---- 4 files changed, 24 insertions(+), 13 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp index 9a498ea..19f2fd7 100644 --- a/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp +++ b/include/bdd_collab_bhv_cpp/collab_pickplace_node.hpp @@ -81,8 +81,8 @@ class CollabPickplaceNode : public rclcpp::Node // Shared data between server handlers & FSM loop struct GoalData { - std::weak_ptr mGoalHandlerPtr; - Behaviour::Goal mGoalCopy; + std::shared_ptr mGoalHandlerPtr; + Behaviour::Goal mGoalCopy; }; std::optional mPendingGoal; diff --git a/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp b/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp index 46f6c5a..da82396 100644 --- a/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp +++ b/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp @@ -19,6 +19,7 @@ #include #include #include "rclcpp/publisher.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" #include "coord2b/types/fsm.h" #include "bdd_ros2_interfaces/msg/trinary_stamped.hpp" @@ -27,7 +28,11 @@ namespace bdd_collab_bhv { class BehaviourInterface { public: - virtual void step(std::shared_ptr pNodePtr, const struct fsm_nbx *pFsmPtr) = 0; + virtual void step( + std::shared_ptr pNodePtr, + const struct fsm_nbx *pFsmPtr, + const unique_identifier_msgs::msg::UUID &pScenarioContextId + ) = 0; virtual ~BehaviourInterface() = default; }; @@ -44,7 +49,11 @@ class MockupCollabBehaviour : public BehaviourInterface rclcpp::Publisher::SharedPtr pLocatedPlacePublisher ); - void step(std::shared_ptr pNodePtr, const struct fsm_nbx *pFsmPtr) override; + void step( + std::shared_ptr pNodePtr, + const struct fsm_nbx *pFsmPtr, + const unique_identifier_msgs::msg::UUID &pScenarioContextId + ) override; private: rclcpp::Duration mHeartbeatPeriod; diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index ae09915..e67390b 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -216,12 +216,12 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - if (auto goal_handle = activeGoal->mGoalHandlerPtr.lock()) { + if (auto goal_handle = activeGoal->mGoalHandlerPtr) { goal_handle->succeed(response); } else { RCLCPP_ERROR( this->get_logger(), - "Goal handler expired, no success response for scenario: %s", + "Goal finished but no active goal handle: %s", uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() ); } @@ -242,12 +242,12 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE; - if (auto goal_handle = activeGoal->mGoalHandlerPtr.lock()) { + if (auto goal_handle = activeGoal->mGoalHandlerPtr) { goal_handle->canceled(response); } else { RCLCPP_ERROR( this->get_logger(), - "Goal handler expired, no cancel response for scenario: %s", + "Goal cancel requested but no active goal handle: %s", uuid_to_hex(activeGoal->mGoalCopy.scenario_context_id).c_str() ); } @@ -261,7 +261,7 @@ void bcb::CollabPickplaceNode::fsm_loop() } // Behaviour & FSM update - bhvInfPtr->step(nodePtr, mFsmPtr.get()); + bhvInfPtr->step(nodePtr, mFsmPtr.get(), activeGoal->mGoalCopy.scenario_context_id); fsm_step_nbx(mFsmPtr.get()); reconfig_event_buffers(mFsmPtr->eventData); } diff --git a/src/fsm_behaviours.cpp b/src/fsm_behaviours.cpp index 8802fc0..eab9efa 100644 --- a/src/fsm_behaviours.cpp +++ b/src/fsm_behaviours.cpp @@ -33,8 +33,9 @@ bcb::MockupCollabBehaviour::MockupCollabBehaviour( { mNextHeartbeat = pNow + mHeartbeatPeriod; } void bcb::MockupCollabBehaviour::step( - std::shared_ptr pNodePtr, - const struct fsm_nbx *pFsmPtr + std::shared_ptr pNodePtr, + const struct fsm_nbx *pFsmPtr, + const unique_identifier_msgs::msg::UUID &pScenarioContextId ) { auto now = pNodePtr->get_clock()->now(); @@ -45,8 +46,9 @@ void bcb::MockupCollabBehaviour::step( ); TrinaryStamped trinaryMsg; - trinaryMsg.stamp = now; - trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; + trinaryMsg.stamp = now; + trinaryMsg.scenario_context_id = pScenarioContextId; + trinaryMsg.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; if (pFsmPtr->currentStateIndex == collab_pickplace::S_TOUCH_TABLE) { mLocatedPickPublisher->publish(trinaryMsg); From 5a4f115005c87c3c19dda784b6742e81f50b7812 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Mon, 1 Jun 2026 16:00:25 +0200 Subject: [PATCH 11/12] remove unused heartbeat period calculations in fsm_loop --- src/collab_pickplace_node.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index e67390b..657350c 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -163,9 +163,6 @@ void bcb::CollabPickplaceNode::fsm_loop() auto now = this->get_clock()->now(); auto nxtTick = now + period; - auto heartbeatPeriod = rclcpp::Duration(std::chrono::milliseconds(HEARTBEAT_MILI_SECS)); - auto nextHeartbeat = now + heartbeatPeriod; - std::unique_ptr bhvInfPtr = nullptr; switch (mExecCtx) { case ExecutionType::Mockup: From 76454b873ea6181750f970542d6e334df0cc66d4 Mon Sep 17 00:00:00 2001 From: Bastian Hunecke Date: Mon, 1 Jun 2026 17:38:48 +0200 Subject: [PATCH 12/12] add feedback publishing to MockupCollabBehaviour --- include/bdd_collab_bhv_cpp/fsm_behaviours.hpp | 22 ++++++++++++++----- src/collab_pickplace_node.cpp | 20 ++++++++++++----- src/fsm_behaviours.cpp | 12 +++++++++- 3 files changed, 41 insertions(+), 13 deletions(-) diff --git a/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp b/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp index da82396..15f6648 100644 --- a/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp +++ b/include/bdd_collab_bhv_cpp/fsm_behaviours.hpp @@ -18,20 +18,27 @@ #include #include #include +#include #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: + using UUID = unique_identifier_msgs::msg::UUID; + using Behaviour = bdd_ros2_interfaces::action::Behaviour; + using GoalHandleBehaviour = rclcpp_action::ServerGoalHandle; + virtual void step( - std::shared_ptr pNodePtr, - const struct fsm_nbx *pFsmPtr, - const unique_identifier_msgs::msg::UUID &pScenarioContextId + std::shared_ptr pNodePtr, + const struct fsm_nbx *pFsmPtr, + const UUID &pScenarioContextId = UUID(), + std::shared_ptr pGoalHandlePtr = nullptr ) = 0; virtual ~BehaviourInterface() = default; }; @@ -50,15 +57,18 @@ class MockupCollabBehaviour : public BehaviourInterface ); void step( - std::shared_ptr pNodePtr, - const struct fsm_nbx *pFsmPtr, - const unique_identifier_msgs::msg::UUID &pScenarioContextId + std::shared_ptr pNodePtr, + const struct fsm_nbx *pFsmPtr, + const UUID &pScenarioContextId = UUID(), + std::shared_ptr pGoalHandlePtr = nullptr ) override; private: rclcpp::Duration mHeartbeatPeriod; rclcpp::Time mNextHeartbeat; + std::shared_ptr mFeedbackPtr; + rclcpp::Publisher::SharedPtr mLocatedPickPublisher; rclcpp::Publisher::SharedPtr mIsHeldPublisher; rclcpp::Publisher::SharedPtr mLocatedPlacePublisher; diff --git a/src/collab_pickplace_node.cpp b/src/collab_pickplace_node.cpp index 657350c..6682757 100644 --- a/src/collab_pickplace_node.cpp +++ b/src/collab_pickplace_node.cpp @@ -193,7 +193,6 @@ void bcb::CollabPickplaceNode::fsm_loop() produce_event(mFsmPtr->eventData, collab_pickplace::E_STEP); response->result.scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; - feedback->scenario_context_id = activeGoal->mGoalCopy.scenario_context_id; // Event publishing for (unsigned int evt : CollabPickplaceNode::EXPORTED_EVENTS) { @@ -213,8 +212,8 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::TRUE; - if (auto goal_handle = activeGoal->mGoalHandlerPtr) { - goal_handle->succeed(response); + if (auto goalHandle = activeGoal->mGoalHandlerPtr) { + goalHandle->succeed(response); } else { RCLCPP_ERROR( this->get_logger(), @@ -239,8 +238,8 @@ void bcb::CollabPickplaceNode::fsm_loop() response->result.stamp = now; response->result.trinary.value = bdd_ros2_interfaces::msg::Trinary::FALSE; - if (auto goal_handle = activeGoal->mGoalHandlerPtr) { - goal_handle->canceled(response); + if (auto goalHandle = activeGoal->mGoalHandlerPtr) { + goalHandle->canceled(response); } else { RCLCPP_ERROR( this->get_logger(), @@ -258,7 +257,16 @@ void bcb::CollabPickplaceNode::fsm_loop() } // Behaviour & FSM update - bhvInfPtr->step(nodePtr, mFsmPtr.get(), activeGoal->mGoalCopy.scenario_context_id); + 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); } diff --git a/src/fsm_behaviours.cpp b/src/fsm_behaviours.cpp index eab9efa..0b07817 100644 --- a/src/fsm_behaviours.cpp +++ b/src/fsm_behaviours.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include "coord2b/functions/event_loop.h" #include "bdd_collab_bhv_cpp/collab_pickplace_fsm.hpp" @@ -28,6 +29,7 @@ bcb::MockupCollabBehaviour::MockupCollabBehaviour( rclcpp::Publisher::SharedPtr pLocatedPlacePublisher ) : mHeartbeatPeriod(std::chrono::milliseconds(pHeartbeatDurMiliSec)), + mFeedbackPtr(std::make_shared()), mLocatedPickPublisher(pLocatedPickPublisher), mIsHeldPublisher(pIsHeldPublisher), mLocatedPlacePublisher(pLocatedPlacePublisher) { mNextHeartbeat = pNow + mHeartbeatPeriod; } @@ -35,7 +37,8 @@ bcb::MockupCollabBehaviour::MockupCollabBehaviour( void bcb::MockupCollabBehaviour::step( std::shared_ptr pNodePtr, const struct fsm_nbx *pFsmPtr, - const unique_identifier_msgs::msg::UUID &pScenarioContextId + const unique_identifier_msgs::msg::UUID &pScenarioContextId, + std::shared_ptr pGoalHandlePtr ) { auto now = pNodePtr->get_clock()->now(); @@ -45,6 +48,13 @@ void bcb::MockupCollabBehaviour::step( pNodePtr->get_logger(), "State: %s", pFsmPtr->states[pFsmPtr->currentStateIndex].name ); + if (pGoalHandlePtr) { + mFeedbackPtr->scenario_context_id = pScenarioContextId; + mFeedbackPtr->status = + std::format("current state: {}", pFsmPtr->states[pFsmPtr->currentStateIndex].name); + pGoalHandlePtr->publish_feedback(mFeedbackPtr); + } + TrinaryStamped trinaryMsg; trinaryMsg.stamp = now; trinaryMsg.scenario_context_id = pScenarioContextId;