feat(behaviors): add pose conversion, snapshot subscribers, Bool/Empty publishers

Adds nine behaviors required by external workspaces composing pose-based
teleop pipelines (e.g. lab_sim's Quest controller teleoperation Objective):

Pose / vector primitives
- PoseToVectors — explodes a geometry_msgs/Pose into separate translation
  (x,y,z) and orientation (x,y,z,w) float ports. Lets BT-XML compose poses
  through scalar Script math without needing a custom Behavior.
- VectorsToPose — inverse of PoseToVectors; rebuilds a Pose from the seven
  scalar ports.

Snapshot / latest subscriber variants
- GetBool — streaming std_msgs/Bool subscriber; outputs the primitive bool
  on each tick. Uses std::optional internally so a default-constructed
  Bool is never written to the blackboard before the first message
  arrives (avoids a cold-start race against pre-seeded values).
- GetBoolInstance — one-shot variant; subscribes on start, blocks until
  the first message, tears down the subscription and returns SUCCESS.
  Outputs the full std_msgs/Bool message (useful when downstream
  consumers want the message wrapper rather than the primitive).
- GetEmptyInstance — one-shot std_msgs/Empty subscriber; useful as a
  fire-on-event gate in Parallel constructs.
- GetOdomInstance — one-shot nav_msgs/Odometry subscriber.
- GetOdomLatest — streaming Odometry subscriber with std::optional
  cold-start safety (analogous to GetBool above).

Message publishing
- PublishBool — std_msgs/Bool publisher with a primitive bool input port.
- PublishEmpty — std_msgs/Empty publisher for firing one-shot event
  signals from a BT (heartbeats, fire-on-event gates that downstream
  subscribers can latch onto). Pairs with GetEmptyInstance.

Build / test
- CMakeLists.txt: nine new .cpp source files added; THIS_PACKAGE_INCLUDE_DEPENDS
  picks up nav_msgs and std_msgs.
- package.xml: nav_msgs and std_msgs depends added.
- test/test_behavior_plugins.cpp: PoseToVectors and VectorsToPose added
  to the load-plugin smoke check.
- test/test_pose_vector_conversion.cpp: new gtest suite covering the
  decompose / compose math, including identity, round-trip, and edge
  cases.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

Author: nbbrooks

CMakeLists.txt

@@ -4,7 +4,7 @@ project(experimental_behaviors CXX)
 find_package(moveit_studio_common REQUIRED)
 moveit_studio_package()
 
-set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs geometry_msgs moveit_pro_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected trajectory_msgs moveit_pro_base)
+set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs geometry_msgs moveit_pro_behavior_interface pluginlib moveit_studio_common behaviortree_cpp nav_msgs std_msgs tl_expected trajectory_msgs moveit_pro_base)
 foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${package} REQUIRED)
 endforeach()
@@ -22,6 +22,15 @@ add_library(
   src/get_blackboard_by_key.cpp
   src/set_blackboard_by_key.cpp
   src/trajectory_to_path.cpp
+  src/pose_to_vectors.cpp
+  src/vectors_to_pose.cpp
+  src/get_odom_latest.cpp
+  src/publish_bool.cpp
+  src/publish_empty.cpp
+  src/get_bool.cpp
+  src/get_bool_instance.cpp
+  src/get_empty_instance.cpp
+  src/get_odom_instance.cpp
   src/register_behaviors.cpp)
 target_include_directories(
   experimental_behaviors

include/experimental_behaviors/get_bool.hpp

@@ -0,0 +1,54 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <optional>
+#include <shared_mutex>
+
+#include <behaviortree_cpp/action_node.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+/**
+ * @brief Long-lived `std_msgs/Bool` subscriber that writes the latest received value to the
+ *        blackboard every tick. Returns RUNNING forever; halted by the parent on parallel
+ *        completion.
+ *
+ * @details Uses `std::optional<bool>` for the cached value and skips `setOutput` until the
+ *          first real message arrives. Without this, a default-constructed `false` would be
+ *          written on the first onRunning tick, clobbering any pre-seeded blackboard value
+ *          before the publisher has delivered a real sample.
+ *
+ * | Data Port Name   | Port Type | Object Type |
+ * | ---------------- | --------- | ----------- |
+ * | bool_topic_name  | input     | std::string |
+ * | subscribed_bool  | output    | bool        |
+ */
+class GetBool : public moveit_pro::behaviors::SharedResourcesNode<BT::StatefulActionNode>
+{
+public:
+  GetBool(const std::string& name, const BT::NodeConfiguration& config,
+          const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+private:
+  BT::NodeStatus onStart() override;
+  BT::NodeStatus onRunning() override;
+  void onHalted() override;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr bool_subscriber_;
+  std::shared_mutex bool_mutex_;
+  std::optional<bool> current_bool_value_;
+};
+}  // namespace experimental_behaviors

include/experimental_behaviors/get_bool_instance.hpp

@@ -0,0 +1,52 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <optional>
+#include <shared_mutex>
+#include <string>
+
+#include <behaviortree_cpp/action_node.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+/**
+ * @brief Snapshot-style `std_msgs/Bool` subscriber. Blocks until the first message arrives,
+ *        then returns SUCCESS and tears down its subscription. Outputs the full Bool
+ *        message (not the primitive bool) — useful when downstream consumers need the
+ *        ROS message wrapper. For streaming the primitive value, use `GetBool` instead.
+ *
+ * | Data Port Name           | Port Type | Object Type         |
+ * | ------------------------ | --------- | ------------------- |
+ * | bool_topic_name          | input     | std::string         |
+ * | subscribed_bool_instance | output    | std_msgs::msg::Bool |
+ */
+class GetBoolInstance : public moveit_pro::behaviors::SharedResourcesNode<BT::StatefulActionNode>
+{
+public:
+  GetBoolInstance(const std::string& name, const BT::NodeConfiguration& config,
+                  const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+private:
+  BT::NodeStatus onStart() override;
+  BT::NodeStatus onRunning() override;
+  void onHalted() override;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr bool_subscriber_;
+  std::shared_mutex bool_mutex_;
+  std::optional<std_msgs::msg::Bool> current_bool_;
+  std::string bool_topic_name_;
+};
+}  // namespace experimental_behaviors

include/experimental_behaviors/get_empty_instance.hpp

@@ -0,0 +1,48 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <shared_mutex>
+
+#include <behaviortree_cpp/action_node.h>
+#include <std_msgs/msg/empty.hpp>
+
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+/**
+ * @brief Snapshot-style `std_msgs/Empty` subscriber. Blocks until the first message arrives,
+ *        then returns SUCCESS and tears down its subscription. Use for one-shot event
+ *        triggers where downstream logic must not run until the trigger fires.
+ *
+ * | Data Port Name    | Port Type | Object Type |
+ * | ----------------- | --------- | ----------- |
+ * | empty_topic_name  | input     | std::string |
+ * | message_received  | output    | bool        |
+ * | message_count     | output    | uint64_t    |
+ */
+class GetEmptyInstance : public moveit_pro::behaviors::SharedResourcesNode<BT::StatefulActionNode>
+{
+public:
+  GetEmptyInstance(const std::string& name, const BT::NodeConfiguration& config,
+                   const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus onStart() override;
+  BT::NodeStatus onRunning() override;
+  void onHalted() override;
+
+private:
+  std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Empty>> empty_subscriber_;
+  std::shared_mutex empty_mutex_;
+  uint64_t message_count_;
+};
+}  // namespace experimental_behaviors

include/experimental_behaviors/get_odom_instance.hpp

@@ -0,0 +1,52 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <optional>
+#include <shared_mutex>
+#include <string>
+
+#include <behaviortree_cpp/action_node.h>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+/**
+ * @brief Snapshot-style `nav_msgs/Odometry` subscriber. Subscribes on start, blocks until
+ *        the first message arrives, returns SUCCESS, and tears down its subscription. Use
+ *        for one-shot pose snapshots where downstream math must not race against
+ *        subscription cold-start.
+ *
+ * | Data Port Name           | Port Type | Object Type             |
+ * | ------------------------ | --------- | ----------------------- |
+ * | odom_topic_name          | input     | std::string             |
+ * | subscribed_odom_instance | output    | nav_msgs::msg::Odometry |
+ */
+class GetOdomInstance : public moveit_pro::behaviors::SharedResourcesNode<BT::StatefulActionNode>
+{
+public:
+  GetOdomInstance(const std::string& name, const BT::NodeConfiguration& config,
+                  const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+private:
+  BT::NodeStatus onStart() override;
+  BT::NodeStatus onRunning() override;
+  void onHalted() override;
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscriber_;
+  std::shared_mutex odom_mutex_;
+  std::optional<nav_msgs::msg::Odometry> current_odometry_;
+  std::string odom_topic_name_;
+};
+}  // namespace experimental_behaviors

include/experimental_behaviors/get_odom_latest.hpp

@@ -0,0 +1,61 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <optional>
+#include <shared_mutex>
+#include <string>
+
+#include <behaviortree_cpp/action_node.h>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+/**
+ * @brief Long-lived odometry subscriber that publishes the latest received message every tick.
+ *
+ * @details Like the core `GetOdom` behavior in shape — `StatefulActionNode`, returns RUNNING
+ *          forever, halted by the parent on parallel completion — but does NOT write anything
+ *          to its output ports until at least one real message has arrived. This avoids a
+ *          cold-start race where downstream consumers (e.g. CalculatePoseOffset) see a
+ *          default-constructed Odometry with an empty `header.frame_id` on early ticks and
+ *          fail the tree.
+ *
+ *          Intended to be used as a producer in a Parallel, with the consumer reading
+ *          `odometry_pose` from the blackboard. Pair with a seed (e.g. ConvertOdomToPoseStamped
+ *          on a fresh GetOdomInstance snapshot) so the consumer has a valid value to read on
+ *          its first tick before this behavior has received anything.
+ *
+ * | Data Port Name       | Port Type | Object Type                     |
+ * | -------------------- | --------- | ------------------------------- |
+ * | odometry_topic_name  | input     | std::string                     |
+ * | subscribed_odometry  | output    | nav_msgs::msg::Odometry         |
+ * | odometry_pose        | output    | geometry_msgs::msg::PoseStamped |
+ */
+class GetOdomLatest final : public moveit_pro::behaviors::SharedResourcesNode<BT::StatefulActionNode>
+{
+public:
+  GetOdomLatest(const std::string& name, const BT::NodeConfiguration& config,
+                const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus onStart() override;
+  BT::NodeStatus onRunning() override;
+  void onHalted() override;
+
+private:
+  std::shared_ptr<rclcpp::Subscription<nav_msgs::msg::Odometry>> odometry_subscriber_;
+  std::shared_mutex odometry_mutex_;
+  std::optional<nav_msgs::msg::Odometry> current_odometry_;
+};
+}  // namespace experimental_behaviors

include/experimental_behaviors/pose_to_vectors.hpp

@@ -0,0 +1,49 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <behaviortree_cpp/action_node.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+namespace detail
+{
+/// @brief Decomposes a Pose into translation [x, y, z] and quaternion [x, y, z, w] vectors.
+/// Pure function — no I/O, no failure modes. Companion to composePose() in vectors_to_pose.hpp.
+[[nodiscard]] std::pair<std::vector<double>, std::vector<double>> decomposePose(const geometry_msgs::msg::Pose& pose);
+}  // namespace detail
+
+/**
+ * @brief Decomposes a PoseStamped into translation_xyz and quaternion_xyzw vectors compatible
+ *        with the std::vector<double> ports used by core behaviors like TransformPose.
+ *
+ * @details Companion to VectorsToPose.
+ *
+ * | Data Port Name   | Port Type | Object Type                     |
+ * | ---------------- | --------- | ------------------------------- |
+ * | pose_stamped     | input     | geometry_msgs::msg::PoseStamped |
+ * | translation_xyz  | output    | std::vector<double> (size 3)    |
+ * | quaternion_xyzw  | output    | std::vector<double> (size 4)    |
+ */
+class PoseToVectors final : public moveit_pro::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+public:
+  PoseToVectors(const std::string& name, const BT::NodeConfiguration& config,
+                const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus tick() override;
+};
+}  // namespace experimental_behaviors

include/experimental_behaviors/publish_bool.hpp

@@ -0,0 +1,48 @@
+// Copyright 2026 PickNik Inc.
+// All rights reserved.
+//
+// Unauthorized copying of this code base via any medium is strictly prohibited.
+// Proprietary and confidential.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <behaviortree_cpp/action_node.h>
+#include <rclcpp/publisher.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
+
+namespace experimental_behaviors
+{
+/**
+ * @brief Publishes a `std_msgs/Bool` message to the given topic.
+ *
+ * @details Mirror of the existing core `PublishString` and `PublishEmpty` behaviors, for
+ *          the `Bool` message type. The publisher is created lazily on the first tick that
+ *          uses a given topic, and re-created if the topic name changes between ticks.
+ *
+ * | Data Port Name | Port Type | Object Type   |
+ * | -------------- | --------- | ------------- |
+ * | topic_name     | input     | std::string   |
+ * | value          | input     | bool          |
+ */
+class PublishBool final : public moveit_pro::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+public:
+  PublishBool(const std::string& name, const BT::NodeConfiguration& config,
+              const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus tick() override;
+
+private:
+  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> publisher_;
+  std::string current_topic_name_;
+};
+}  // namespace experimental_behaviors