From a2298560a37f46ab670ed2145ecb8b1de78dbcda Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 22 Dec 2025 15:54:45 -0700 Subject: [PATCH 01/27] GetEmpty now exits as soon as the first empty message is received Former-commit-id: 309591d2ae170e48ebdd1a2351fae8f836678e80 --- src/get_empty/src/get_empty.cpp | 101 ++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 src/get_empty/src/get_empty.cpp diff --git a/src/get_empty/src/get_empty.cpp b/src/get_empty/src/get_empty.cpp new file mode 100644 index 000000000..de74c01f5 --- /dev/null +++ b/src/get_empty/src/get_empty.cpp @@ -0,0 +1,101 @@ +#include + +#include "spdlog/spdlog.h" +#include "fmt/format.h" + +namespace get_empty +{ +GetEmpty::GetEmpty( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) + , message_count_(0) +{ +} + +BT::PortsList GetEmpty::providedPorts() +{ + return BT::PortsList({ + BT::InputPort("empty_topic_name", "empty_topic", + "The name of the std_msgs::msg::Empty topic to subscribe to."), + BT::OutputPort("message_received", "{message_received}", + "Boolean indicating if at least one message has been received."), + BT::OutputPort("message_count", "{message_count}", + "Total count of empty messages received.") + }); +} + +BT::KeyValueVector GetEmpty::metadata() +{ + return { + {"description", "Subscribe to an empty message topic and track received messages on the blackboard."}, + {"subcategory", "Messages"} + }; +} + +BT::NodeStatus GetEmpty::onStart() +{ + // Get the topic name from input port + const auto topic_name_result = getInput("empty_topic_name"); + if (!topic_name_result) + { + shared_resources_->logger->publishFailureMessage( + name(), fmt::format("Failed to get required value from input port 'empty_topic_name': {}", + topic_name_result.error())); + return BT::NodeStatus::FAILURE; + } + + const std::string empty_topic_name = topic_name_result.value(); + + // Reset message count when starting + message_count_ = 0; + + // Subscribe to the empty topic + empty_subscriber_ = shared_resources_->node->create_subscription( + empty_topic_name, rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::Empty::SharedPtr msg) { + (void)msg; // Empty message has no data + std::unique_lock lock(empty_mutex_); + message_count_++; + }); + + shared_resources_->logger->publishInfoMessage( + name(), fmt::format("Subscribed to empty topic: {}", empty_topic_name)); + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus GetEmpty::onRunning() +{ + uint64_t count; + { + std::shared_lock lock(empty_mutex_); + count = message_count_; + } + + setOutput("message_received", count > 0); + setOutput("message_count", count); + + if (count > 0) + { + // Stop listening once condition is satisfied + empty_subscriber_.reset(); + + shared_resources_->logger->publishInfoMessage( + name(), "Empty message received — exiting behavior successfully."); + + return BT::NodeStatus::SUCCESS; + } + + return BT::NodeStatus::RUNNING; +} + +void GetEmpty::onHalted() +{ + // Stop subscribers + empty_subscriber_.reset(); + + spdlog::info("GetEmpty behavior halted."); +} + +} // namespace get_empty \ No newline at end of file From d93cc17d1034f7b8874adeaf44ace01d468be07b Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 22 Dec 2025 16:56:17 -0700 Subject: [PATCH 02/27] correctly computes the transform between the controller frame and the end effector frame Former-commit-id: 4fdb8fde918381c88569ef4e8dd5e48caa1f4d3c --- .../objectives/teleop_with_meta.xml | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 src/factory_sim/objectives/teleop_with_meta.xml diff --git a/src/factory_sim/objectives/teleop_with_meta.xml b/src/factory_sim/objectives/teleop_with_meta.xml new file mode 100644 index 000000000..15031c5a5 --- /dev/null +++ b/src/factory_sim/objectives/teleop_with_meta.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From e70c9e6cea8678e78c481d7a3a47990f21b96933 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 23 Dec 2025 15:40:42 +0000 Subject: [PATCH 03/27] added get bool behavior Former-commit-id: 13fa436cfb54aff14048846c8e90b8677797c595 --- src/get_bool/CMakeLists.txt | 46 +++++++++++ src/get_bool/behavior_plugin.yaml | 4 + src/get_bool/get_bool_plugin_description.xml | 7 ++ src/get_bool/include/get_bool/get_bool.hpp | 73 +++++++++++++++++ src/get_bool/package.xml | 30 +++++++ src/get_bool/src/get_bool.cpp | 82 ++++++++++++++++++++ src/get_bool/src/register_behaviors.cpp | 24 ++++++ src/get_bool/test/CMakeLists.txt | 5 ++ src/get_bool/test/test_behavior_plugins.cpp | 37 +++++++++ 9 files changed, 308 insertions(+) create mode 100644 src/get_bool/CMakeLists.txt create mode 100644 src/get_bool/behavior_plugin.yaml create mode 100644 src/get_bool/get_bool_plugin_description.xml create mode 100644 src/get_bool/include/get_bool/get_bool.hpp create mode 100644 src/get_bool/package.xml create mode 100644 src/get_bool/src/get_bool.cpp create mode 100644 src/get_bool/src/register_behaviors.cpp create mode 100644 src/get_bool/test/CMakeLists.txt create mode 100644 src/get_bool/test/test_behavior_plugins.cpp diff --git a/src/get_bool/CMakeLists.txt b/src/get_bool/CMakeLists.txt new file mode 100644 index 000000000..0e33c5604 --- /dev/null +++ b/src/get_bool/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(get_bool CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + get_bool + SHARED + src/get_bool.cpp + src/register_behaviors.cpp) +target_include_directories( + get_bool + PUBLIC $ + $) +ament_target_dependencies(get_bool + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS get_bool + EXPORT get_boolTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(get_bool) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface get_bool_plugin_description.xml) + +ament_export_targets(get_boolTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/get_bool/behavior_plugin.yaml b/src/get_bool/behavior_plugin.yaml new file mode 100644 index 000000000..672e785cd --- /dev/null +++ b/src/get_bool/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + get_bool: + - "get_bool::GetBoolBehaviorsLoader" diff --git a/src/get_bool/get_bool_plugin_description.xml b/src/get_bool/get_bool_plugin_description.xml new file mode 100644 index 000000000..a3029807f --- /dev/null +++ b/src/get_bool/get_bool_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/get_bool/include/get_bool/get_bool.hpp b/src/get_bool/include/get_bool/get_bool.hpp new file mode 100644 index 000000000..58947e3ae --- /dev/null +++ b/src/get_bool/include/get_bool/get_bool.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include + +#include +#include + +#include +#include + +// This header includes the SharedResourcesNode type +#include + +namespace get_bool +{ +/** + * @brief Subscribes to a std_msgs::msg::Bool topic and outputs the boolean value to the blackboard. + */ +class GetBool : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the get_bool behavior. + * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode. + * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. + */ + GetBool(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @brief Implementation of the required providedPorts() function for the get_bool Behavior. + * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList. + * This function returns a list of ports with their names and port info, which is used internally by the behavior tree. + * @return A BT::PortsList containing the input port for the topic name and output port for the boolean value. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + +private: + /** + * @brief Called when the behavior is started. Sets up the subscription to the bool topic. + * @return BT::NodeStatus::RUNNING if successful, BT::NodeStatus::FAILURE otherwise. + */ + BT::NodeStatus onStart() override; + + /** + * @brief Called while the behavior is running. Updates the output port with the current boolean value. + * @return BT::NodeStatus::RUNNING to continue execution. + */ + BT::NodeStatus onRunning() override; + + /** + * @brief Called when the behavior is halted. Cleans up the subscription. + */ + void onHalted() override; + + /** @brief Subscriber for the bool topic. */ + rclcpp::Subscription::SharedPtr bool_subscriber_; + + /** @brief Mutex for thread-safe access to the current bool value. */ + std::shared_mutex bool_mutex_; + + /** @brief The most recently received boolean value. */ + bool current_bool_value_{ false }; +}; +} // namespace get_bool \ No newline at end of file diff --git a/src/get_bool/package.xml b/src/get_bool/package.xml new file mode 100644 index 000000000..cbc657e60 --- /dev/null +++ b/src/get_bool/package.xml @@ -0,0 +1,30 @@ + + + get_bool + 0.0.0 + Listens to a bool message + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/get_bool/src/get_bool.cpp b/src/get_bool/src/get_bool.cpp new file mode 100644 index 000000000..aa13659f8 --- /dev/null +++ b/src/get_bool/src/get_bool.cpp @@ -0,0 +1,82 @@ +// Copyright 2025 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include + +#include "fmt/format.h" + +#include "moveit_studio_behavior_interface/get_required_ports.hpp" +#include "moveit_studio_behavior_interface/metadata_fields.hpp" + +namespace get_bool +{ + +// Port names. +constexpr auto kPortIdBoolTopicName = "bool_topic_name"; +constexpr auto kPortIdBoolValue = "subscribed_bool"; + +GetBool::GetBool(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList GetBool::providedPorts() +{ + return BT::PortsList({ BT::InputPort(kPortIdBoolTopicName, "bool_topic", + "The name of the std_msgs::msg::Bool topic to subscribe to."), + BT::OutputPort(kPortIdBoolValue, "{subscribed_bool}", + "Subscribed boolean value.") }); +} + +BT::KeyValueVector GetBool::metadata() +{ + return { { moveit_studio::behaviors::kSubcategoryMetadataKey, "User Created Behaviors" }, + { moveit_studio::behaviors::kDescriptionMetadataKey, + "Subscribe to a bool message and store it on the blackboard." } }; +} + +BT::NodeStatus GetBool::onStart() +{ + const auto ports = moveit_studio::behaviors::getRequiredInputs(getInput(kPortIdBoolTopicName)); + + if (!ports.has_value()) + { + shared_resources_->logger->publishFailureMessage( + name(), fmt::format("Failed to get required value from input data port: {}", ports.error())); + return BT::NodeStatus::FAILURE; + } + + const auto& [bool_topic_name] = ports.value(); + + // Subscribe to the bool topic. + bool_subscriber_ = shared_resources_->node->create_subscription( + bool_topic_name, rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Bool::SharedPtr msg) { + std::unique_lock lock(bool_mutex_); + current_bool_value_ = msg->data; + }); + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus GetBool::onRunning() +{ + // Update output port. + { + std::shared_lock lock(bool_mutex_); + setOutput(kPortIdBoolValue, current_bool_value_); + } + + return BT::NodeStatus::RUNNING; +} + +void GetBool::onHalted() +{ + // Stop subscriber. + bool_subscriber_.reset(); +} + +} // namespace get_bool \ No newline at end of file diff --git a/src/get_bool/src/register_behaviors.cpp b/src/get_bool/src/register_behaviors.cpp new file mode 100644 index 000000000..849c1ab3c --- /dev/null +++ b/src/get_bool/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace get_bool +{ +class GetBoolBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "GetBool", shared_resources); + + } +}; +} // namespace get_bool + +PLUGINLIB_EXPORT_CLASS(get_bool::GetBoolBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/get_bool/test/CMakeLists.txt b/src/get_bool/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/get_bool/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/get_bool/test/test_behavior_plugins.cpp b/src/get_bool/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..aad71d513 --- /dev/null +++ b/src/get_bool/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("get_bool::GetBoolBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "GetBool", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 02b28a66fea2f9489176b7b9f81cb27388b1a444 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 29 Dec 2025 10:54:39 -0700 Subject: [PATCH 04/27] new objectives Former-commit-id: 9785c806dd07cb5063e5c14b7865051bba9e8b51 --- .../CMakeLists.txt | 46 ++++++++++ .../behavior_plugin.yaml | 4 + ...o_transform_stamped_plugin_description.xml | 7 ++ ...vert_pose_stamped_to_transform_stamped.hpp | 47 ++++++++++ .../package.xml | 30 +++++++ ...vert_pose_stamped_to_transform_stamped.cpp | 82 +++++++++++++++++ .../src/register_behaviors.cpp | 24 +++++ .../test/CMakeLists.txt | 5 ++ .../test/test_behavior_plugins.cpp | 37 ++++++++ .../CMakeLists.txt | 46 ++++++++++ .../behavior_plugin.yaml | 4 + ...orm_stamped_to_odom_plugin_description.xml | 7 ++ .../convert_transform_stamped_to_odom.hpp | 47 ++++++++++ .../package.xml | 30 +++++++ .../src/convert_transform_stamped_to_odom.cpp | 88 +++++++++++++++++++ .../src/register_behaviors.cpp | 23 +++++ .../test/CMakeLists.txt | 5 ++ .../test/test_behavior_plugins.cpp | 37 ++++++++ src/factory_sim/objectives/teleopwithmeta.xml | 43 +++++++++ .../objectives/transformquestframe.xml | 28 ++++++ 20 files changed, 640 insertions(+) create mode 100644 src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt create mode 100644 src/convert_pose_stamped_to_transform_stamped/behavior_plugin.yaml create mode 100644 src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml create mode 100644 src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp create mode 100644 src/convert_pose_stamped_to_transform_stamped/package.xml create mode 100644 src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp create mode 100644 src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp create mode 100644 src/convert_pose_stamped_to_transform_stamped/test/CMakeLists.txt create mode 100644 src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp create mode 100644 src/convert_transform_stamped_to_odom/CMakeLists.txt create mode 100644 src/convert_transform_stamped_to_odom/behavior_plugin.yaml create mode 100644 src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml create mode 100644 src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp create mode 100644 src/convert_transform_stamped_to_odom/package.xml create mode 100644 src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp create mode 100644 src/convert_transform_stamped_to_odom/src/register_behaviors.cpp create mode 100644 src/convert_transform_stamped_to_odom/test/CMakeLists.txt create mode 100644 src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp create mode 100644 src/factory_sim/objectives/teleopwithmeta.xml create mode 100644 src/factory_sim/objectives/transformquestframe.xml diff --git a/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt b/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt new file mode 100644 index 000000000..fc6ce25d2 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(convert_pose_stamped_to_transform_stamped CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + convert_pose_stamped_to_transform_stamped + SHARED + src/convert_pose_stamped_to_transform_stamped.cpp + src/register_behaviors.cpp) +target_include_directories( + convert_pose_stamped_to_transform_stamped + PUBLIC $ + $) +ament_target_dependencies(convert_pose_stamped_to_transform_stamped + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS convert_pose_stamped_to_transform_stamped + EXPORT convert_pose_stamped_to_transform_stampedTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(convert_pose_stamped_to_transform_stamped) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface convert_pose_stamped_to_transform_stamped_plugin_description.xml) + +ament_export_targets(convert_pose_stamped_to_transform_stampedTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/convert_pose_stamped_to_transform_stamped/behavior_plugin.yaml b/src/convert_pose_stamped_to_transform_stamped/behavior_plugin.yaml new file mode 100644 index 000000000..328a332f0 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + convert_pose_stamped_to_transform_stamped: + - "convert_pose_stamped_to_transform_stamped::ConvertPoseStampedToTransformStampedBehaviorsLoader" diff --git a/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml b/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml new file mode 100644 index 000000000..9fd2fdb80 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp b/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp new file mode 100644 index 000000000..a2ff62bb7 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include + +// This header includes the SharedResourcesNode type +#include + +namespace convert_pose_stamped_to_transform_stamped +{ +/** + * @brief Converts a geometry_msgs::msg::PoseStamped message into a geometry_msgs::msg::TransformStamped message. + */ +class ConvertPoseStampedToTransformStamped : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the convert_pose_stamped_to_transform_stamped behavior. + * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode. + * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. + */ + ConvertPoseStampedToTransformStamped(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @brief Implementation of the required providedPorts() function for the convert_pose_stamped_to_transform_stamped Behavior. + * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList. + * This function returns a list of ports with their names and port info, which is used internally by the behavior tree. + * @return A BT::PortsList containing the input ports for PoseStamped and child_frame_id, and output port for TransformStamped. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Implementation of BT::SyncActionNode::tick() for ConvertPoseStampedToTransformStamped. + * @details This function is where the Behavior performs its work when the behavior tree is being run. Since ConvertPoseStampedToTransformStamped is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly. + */ + BT::NodeStatus tick() override; +}; +} // namespace convert_pose_stamped_to_transform_stamped \ No newline at end of file diff --git a/src/convert_pose_stamped_to_transform_stamped/package.xml b/src/convert_pose_stamped_to_transform_stamped/package.xml new file mode 100644 index 000000000..1a6386d4b --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/package.xml @@ -0,0 +1,30 @@ + + + convert_pose_stamped_to_transform_stamped + 0.0.0 + Convert pose stamped to transform stamped. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp b/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp new file mode 100644 index 000000000..c426cde91 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp @@ -0,0 +1,82 @@ +// Copyright 2025 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include + +#include +#include + +#include "spdlog/spdlog.h" +#include "moveit_studio_behavior_interface/get_required_ports.hpp" + +#include +#include + +namespace +{ +constexpr auto kPortIDPoseStamped = "pose_stamped"; +constexpr auto kPortIDTransformStamped = "transform_stamped"; +constexpr auto kPortIDChildFrameId = "child_frame_id"; +} // namespace + +namespace convert_pose_stamped_to_transform_stamped +{ + +ConvertPoseStampedToTransformStamped::ConvertPoseStampedToTransformStamped( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList ConvertPoseStampedToTransformStamped::providedPorts() +{ + return BT::PortsList( + { BT::InputPort(kPortIDPoseStamped, "{pose_stamped}", + "The geometry_msgs::msg::PoseStamped message to convert."), + BT::InputPort(kPortIDChildFrameId, "child_frame", + "The child frame ID to set in the TransformStamped message."), + BT::OutputPort(kPortIDTransformStamped, "{transform_stamped}", + "The converted geometry_msgs::msg::TransformStamped message.") }); +} + +BT::KeyValueVector ConvertPoseStampedToTransformStamped::metadata() +{ + return { { "description", "Converts a geometry_msgs::msg::PoseStamped message into a " + "geometry_msgs::msg::TransformStamped message." }, + { "subcategory", "Conversions" } }; +} + +BT::NodeStatus ConvertPoseStampedToTransformStamped::tick() +{ + const auto ports = moveit_studio::behaviors::getRequiredInputs( + getInput(kPortIDPoseStamped), + getInput(kPortIDChildFrameId)); + + if (!ports.has_value()) + { + spdlog::warn("Failed to get required value from input data port: {}", ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& [pose_stamped, child_frame_id] = ports.value(); + + geometry_msgs::msg::TransformStamped transform_out; + transform_out.header = pose_stamped.header; + transform_out.child_frame_id = child_frame_id; + + // Convert the pose to transform + transform_out.transform.translation.x = pose_stamped.pose.position.x; + transform_out.transform.translation.y = pose_stamped.pose.position.y; + transform_out.transform.translation.z = pose_stamped.pose.position.z; + transform_out.transform.rotation = pose_stamped.pose.orientation; + + setOutput(kPortIDTransformStamped, transform_out); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace convert_pose_stamped_to_transform_stamped \ No newline at end of file diff --git a/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp b/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp new file mode 100644 index 000000000..f19bb6ef4 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace convert_pose_stamped_to_transform_stamped +{ +class ConvertPoseStampedToTransformStampedBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "ConvertPoseStampedToTransformStamped", shared_resources); + + } +}; +} // namespace convert_pose_stamped_to_transform_stamped + +PLUGINLIB_EXPORT_CLASS(convert_pose_stamped_to_transform_stamped::ConvertPoseStampedToTransformStampedBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/convert_pose_stamped_to_transform_stamped/test/CMakeLists.txt b/src/convert_pose_stamped_to_transform_stamped/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp b/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..eb954b224 --- /dev/null +++ b/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("convert_pose_stamped_to_transform_stamped::ConvertPoseStampedToTransformStampedBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ConvertPoseStampedToTransformStamped", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/convert_transform_stamped_to_odom/CMakeLists.txt b/src/convert_transform_stamped_to_odom/CMakeLists.txt new file mode 100644 index 000000000..2d9b8ff72 --- /dev/null +++ b/src/convert_transform_stamped_to_odom/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(convert_transform_stamped_to_odom CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + convert_transform_stamped_to_odom + SHARED + src/convert_transform_stamped_to_odom.cpp + src/register_behaviors.cpp) +target_include_directories( + convert_transform_stamped_to_odom + PUBLIC $ + $) +ament_target_dependencies(convert_transform_stamped_to_odom + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS convert_transform_stamped_to_odom + EXPORT convert_transform_stamped_to_odomTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(convert_transform_stamped_to_odom) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface convert_transform_stamped_to_odom_plugin_description.xml) + +ament_export_targets(convert_transform_stamped_to_odomTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/convert_transform_stamped_to_odom/behavior_plugin.yaml b/src/convert_transform_stamped_to_odom/behavior_plugin.yaml new file mode 100644 index 000000000..d5eb14f9a --- /dev/null +++ b/src/convert_transform_stamped_to_odom/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + convert_transform_stamped_to_odom: + - "convert_transform_stamped_to_odom::ConvertTransformStampedToOdomBehaviorsLoader" diff --git a/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml b/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml new file mode 100644 index 000000000..df6d107a9 --- /dev/null +++ b/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp b/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp new file mode 100644 index 000000000..ca884859c --- /dev/null +++ b/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include + +// This header includes the SharedResourcesNode type +#include + +namespace convert_transform_stamped_to_odom +{ +/** + * @brief Converts a geometry_msgs::msg::TransformStamped message into a nav_msgs::msg::Odometry message. + */ +class ConvertTransformStampedToOdom : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the convert_transform_stamped_to_odom behavior. + * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode. + * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. + */ + ConvertTransformStampedToOdom(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @brief Implementation of the required providedPorts() function for the convert_transform_stamped_to_odom Behavior. + * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList. + * This function returns a list of ports with their names and port info, which is used internally by the behavior tree. + * @return A BT::PortsList containing the input port for TransformStamped and output port for Odometry. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Implementation of BT::SyncActionNode::tick() for ConvertTransformStampedToOdom. + * @details This function is where the Behavior performs its work when the behavior tree is being run. Since ConvertTransformStampedToOdom is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly. + */ + BT::NodeStatus tick() override; +}; +} // namespace convert_transform_stamped_to_odom \ No newline at end of file diff --git a/src/convert_transform_stamped_to_odom/package.xml b/src/convert_transform_stamped_to_odom/package.xml new file mode 100644 index 000000000..8d20bec79 --- /dev/null +++ b/src/convert_transform_stamped_to_odom/package.xml @@ -0,0 +1,30 @@ + + + convert_transform_stamped_to_odom + 0.0.0 + Converts a stamped transform message to an odom message. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp b/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp new file mode 100644 index 000000000..52c1d29c1 --- /dev/null +++ b/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp @@ -0,0 +1,88 @@ +// Copyright 2025 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include + +#include +#include + +#include "spdlog/spdlog.h" +#include "moveit_studio_behavior_interface/get_required_ports.hpp" + +#include +#include + +namespace +{ +constexpr auto kPortIDTransformStamped = "transform_stamped"; +constexpr auto kPortIDOdometry = "odometry"; +} // namespace + +namespace convert_transform_stamped_to_odom +{ + +ConvertTransformStampedToOdom::ConvertTransformStampedToOdom( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList ConvertTransformStampedToOdom::providedPorts() +{ + return BT::PortsList( + { BT::InputPort(kPortIDTransformStamped, "{transform_stamped}", + "The geometry_msgs::msg::TransformStamped message to " + "convert."), + BT::OutputPort(kPortIDOdometry, "{odometry}", + "The converted nav_msgs::msg::Odometry message.") }); +} + +BT::KeyValueVector ConvertTransformStampedToOdom::metadata() +{ + return { { "description", "Converts a geometry_msgs::msg::TransformStamped message into a " + "nav_msgs::msg::Odometry message." }, + { "subcategory", "Conversions" } }; +} + +BT::NodeStatus ConvertTransformStampedToOdom::tick() +{ + const auto ports = moveit_studio::behaviors::getRequiredInputs( + getInput(kPortIDTransformStamped)); + + if (!ports.has_value()) + { + spdlog::warn("Failed to get required value from input data port: {}", ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& transform_stamped = std::get<0>(ports.value()); + + nav_msgs::msg::Odometry odom_out; + odom_out.header = transform_stamped.header; + odom_out.child_frame_id = transform_stamped.child_frame_id; + + shared_resources_->logger->publishInfoMessage( + name(), "Converting transform to pose message ..."); + + // Convert the transform to pose + odom_out.pose.pose.position.x = transform_stamped.transform.translation.x; + odom_out.pose.pose.position.y = transform_stamped.transform.translation.y; + odom_out.pose.pose.position.z = transform_stamped.transform.translation.z; + odom_out.pose.pose.orientation = transform_stamped.transform.rotation; + + // shared_resources_->logger->publishInfoMessage( + // name(), fmt::format("This is pose.position.x: {}, pose.position.y: {}, pose.position.z: {}", + // odom_out.pose.pose.position.x, odom_out.pose.pose.position.y, odom_out.pose.pose.position.z)); + + // Note: Twist (velocity) data is not available from TransformStamped, so it remains zero-initialized + + setOutput(kPortIDOdometry, odom_out); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace convert_transform_stamped_to_odom \ No newline at end of file diff --git a/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp b/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp new file mode 100644 index 000000000..254fcb376 --- /dev/null +++ b/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp @@ -0,0 +1,23 @@ +#include +#include +#include + +#include + +#include + +namespace convert_transform_stamped_to_odom +{ +class ConvertTransformStampedToOdomBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "ConvertTransformStampedToOdom", shared_resources); + } +}; +} // namespace convert_transform_stamped_to_odom + +PLUGINLIB_EXPORT_CLASS(convert_transform_stamped_to_odom::ConvertTransformStampedToOdomBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); \ No newline at end of file diff --git a/src/convert_transform_stamped_to_odom/test/CMakeLists.txt b/src/convert_transform_stamped_to_odom/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/convert_transform_stamped_to_odom/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp b/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..f971853d7 --- /dev/null +++ b/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("convert_transform_stamped_to_odom::ConvertTransformStampedToOdomBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ConvertTransformStampedToOdom", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/factory_sim/objectives/teleopwithmeta.xml b/src/factory_sim/objectives/teleopwithmeta.xml new file mode 100644 index 000000000..365d1f4d3 --- /dev/null +++ b/src/factory_sim/objectives/teleopwithmeta.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/factory_sim/objectives/transformquestframe.xml b/src/factory_sim/objectives/transformquestframe.xml new file mode 100644 index 000000000..74ec3bd4b --- /dev/null +++ b/src/factory_sim/objectives/transformquestframe.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + From a5eb306210f407f0df7cfc8c2610c51235a048d1 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 29 Dec 2025 17:12:35 -0700 Subject: [PATCH 05/27] I think the transformation part is working, now moving on to using track moving frame Former-commit-id: 555ba1cdde29e65e039ed6975d30ecad0c3f54c3 --- .../objectives/teleop_with_meta.xml | 50 +++-- src/factory_sim/objectives/teleopwithmeta.xml | 43 ----- .../objectives/transformquestframe.xml | 7 +- src/get_odom_instance/CMakeLists.txt | 46 +++++ src/get_odom_instance/behavior_plugin.yaml | 4 + .../get_odom_instance_plugin_description.xml | 7 + .../get_odom_instance/get_odom_instance.hpp | 54 ++++++ src/get_odom_instance/package.xml | 30 +++ .../src/get_odom_instance.cpp | 108 +++++++++++ .../src/register_behaviors.cpp | 24 +++ src/get_odom_instance/test/CMakeLists.txt | 5 + .../test/test_behavior_plugins.cpp | 37 ++++ src/publish_odom/CMakeLists.txt | 46 +++++ src/publish_odom/behavior_plugin.yaml | 4 + .../include/publish_odom/publish_odom.hpp | 74 ++++++++ src/publish_odom/package.xml | 30 +++ .../publish_odom_plugin_description.xml | 7 + src/publish_odom/src/publish_odom.cpp | 104 ++++++++++ src/publish_odom/src/register_behaviors.cpp | 24 +++ src/publish_odom/test/CMakeLists.txt | 5 + .../test/test_behavior_plugins.cpp | 37 ++++ src/transform_odom_with_pose/CMakeLists.txt | 46 +++++ .../behavior_plugin.yaml | 4 + .../transform_odom_with_pose.hpp | 79 ++++++++ src/transform_odom_with_pose/package.xml | 30 +++ .../src/register_behaviors.cpp | 24 +++ .../src/transform_odom_with_pose.cpp | 179 ++++++++++++++++++ .../test/CMakeLists.txt | 5 + .../test/test_behavior_plugins.cpp | 37 ++++ ...form_odom_with_pose_plugin_description.xml | 7 + 30 files changed, 1082 insertions(+), 75 deletions(-) delete mode 100644 src/factory_sim/objectives/teleopwithmeta.xml create mode 100644 src/get_odom_instance/CMakeLists.txt create mode 100644 src/get_odom_instance/behavior_plugin.yaml create mode 100644 src/get_odom_instance/get_odom_instance_plugin_description.xml create mode 100644 src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp create mode 100644 src/get_odom_instance/package.xml create mode 100644 src/get_odom_instance/src/get_odom_instance.cpp create mode 100644 src/get_odom_instance/src/register_behaviors.cpp create mode 100644 src/get_odom_instance/test/CMakeLists.txt create mode 100644 src/get_odom_instance/test/test_behavior_plugins.cpp create mode 100644 src/publish_odom/CMakeLists.txt create mode 100644 src/publish_odom/behavior_plugin.yaml create mode 100644 src/publish_odom/include/publish_odom/publish_odom.hpp create mode 100644 src/publish_odom/package.xml create mode 100644 src/publish_odom/publish_odom_plugin_description.xml create mode 100644 src/publish_odom/src/publish_odom.cpp create mode 100644 src/publish_odom/src/register_behaviors.cpp create mode 100644 src/publish_odom/test/CMakeLists.txt create mode 100644 src/publish_odom/test/test_behavior_plugins.cpp create mode 100644 src/transform_odom_with_pose/CMakeLists.txt create mode 100644 src/transform_odom_with_pose/behavior_plugin.yaml create mode 100644 src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp create mode 100644 src/transform_odom_with_pose/package.xml create mode 100644 src/transform_odom_with_pose/src/register_behaviors.cpp create mode 100644 src/transform_odom_with_pose/src/transform_odom_with_pose.cpp create mode 100644 src/transform_odom_with_pose/test/CMakeLists.txt create mode 100644 src/transform_odom_with_pose/test/test_behavior_plugins.cpp create mode 100644 src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml diff --git a/src/factory_sim/objectives/teleop_with_meta.xml b/src/factory_sim/objectives/teleop_with_meta.xml index 15031c5a5..78e425d7f 100644 --- a/src/factory_sim/objectives/teleop_with_meta.xml +++ b/src/factory_sim/objectives/teleop_with_meta.xml @@ -1,34 +1,30 @@ - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - + diff --git a/src/factory_sim/objectives/teleopwithmeta.xml b/src/factory_sim/objectives/teleopwithmeta.xml deleted file mode 100644 index 365d1f4d3..000000000 --- a/src/factory_sim/objectives/teleopwithmeta.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/factory_sim/objectives/transformquestframe.xml b/src/factory_sim/objectives/transformquestframe.xml index 74ec3bd4b..3e5f28f78 100644 --- a/src/factory_sim/objectives/transformquestframe.xml +++ b/src/factory_sim/objectives/transformquestframe.xml @@ -14,15 +14,12 @@ - - - + - - + diff --git a/src/get_odom_instance/CMakeLists.txt b/src/get_odom_instance/CMakeLists.txt new file mode 100644 index 000000000..5ab88eaf3 --- /dev/null +++ b/src/get_odom_instance/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(get_odom_instance CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + get_odom_instance + SHARED + src/get_odom_instance.cpp + src/register_behaviors.cpp) +target_include_directories( + get_odom_instance + PUBLIC $ + $) +ament_target_dependencies(get_odom_instance + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS get_odom_instance + EXPORT get_odom_instanceTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(get_odom_instance) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface get_odom_instance_plugin_description.xml) + +ament_export_targets(get_odom_instanceTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/get_odom_instance/behavior_plugin.yaml b/src/get_odom_instance/behavior_plugin.yaml new file mode 100644 index 000000000..a2b170c5a --- /dev/null +++ b/src/get_odom_instance/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + get_odom_instance: + - "get_odom_instance::GetOdomInstanceBehaviorsLoader" diff --git a/src/get_odom_instance/get_odom_instance_plugin_description.xml b/src/get_odom_instance/get_odom_instance_plugin_description.xml new file mode 100644 index 000000000..3e2bdb689 --- /dev/null +++ b/src/get_odom_instance/get_odom_instance_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp new file mode 100644 index 000000000..61fdd5105 --- /dev/null +++ b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace get_odom_instance +{ +/** + * @brief Subscribes to a single odometry message and publishes it to the blackboard. + * + * The behavior exits successfully after the first message is received. + */ +class GetOdomInstance + : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + GetOdomInstance(const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + static BT::KeyValueVector metadata(); + +private: + /** @brief Called once when the behavior starts. Sets up the subscription. */ + BT::NodeStatus onStart() override; + + /** + * @brief Called while the behavior is running. + * @return RUNNING until an odometry message is received, then SUCCESS. + */ + BT::NodeStatus onRunning() override; + + /** @brief Called when the behavior is halted. Cleans up the subscription. */ + void onHalted() override; + + /** @brief Subscriber for the odom topic. */ + rclcpp::Subscription::SharedPtr odom_subscriber_; + + /** @brief Mutex for thread-safe access to odometry data. */ + std::shared_mutex odom_mutex_; + + /** @brief Latest received odometry message, if any. */ + std::optional current_odometry_; +}; + +} // namespace get_odom_instance diff --git a/src/get_odom_instance/package.xml b/src/get_odom_instance/package.xml new file mode 100644 index 000000000..6809b7847 --- /dev/null +++ b/src/get_odom_instance/package.xml @@ -0,0 +1,30 @@ + + + get_odom_instance + 0.0.0 + Gets the most recent odom message, saves to blackboard, then exits. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/get_odom_instance/src/get_odom_instance.cpp b/src/get_odom_instance/src/get_odom_instance.cpp new file mode 100644 index 000000000..1c8636b93 --- /dev/null +++ b/src/get_odom_instance/src/get_odom_instance.cpp @@ -0,0 +1,108 @@ +#include + +#include + +#include "fmt/format.h" + +#include "moveit_studio_behavior_interface/get_required_ports.hpp" +#include "moveit_studio_behavior_interface/metadata_fields.hpp" + +namespace get_odom_instance +{ + +constexpr auto kPortIdOdomTopicName = "odom_topic_name"; +constexpr auto kPortIdOdomValue = "subscribed_odom_instance"; + +GetOdomInstance::GetOdomInstance( + const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode( + name, config, shared_resources) +{ +} + +BT::PortsList GetOdomInstance::providedPorts() +{ + return { + BT::InputPort( + kPortIdOdomTopicName, + "odom", + "The name of the nav_msgs::msg::Odometry topic to subscribe to."), + BT::OutputPort( + kPortIdOdomValue, + "{subscribed_odom_instance}", + "Subscribed odometry message.") + }; +} + +BT::KeyValueVector GetOdomInstance::metadata() +{ + return { + { moveit_studio::behaviors::kSubcategoryMetadataKey, + "User Created Behaviors" }, + { moveit_studio::behaviors::kDescriptionMetadataKey, + "Subscribe to a single odometry message and store it on the blackboard." } + }; +} + +BT::NodeStatus GetOdomInstance::onStart() +{ + const auto ports = + moveit_studio::behaviors::getRequiredInputs( + getInput(kPortIdOdomTopicName)); + + if (!ports.has_value()) + { + shared_resources_->logger->publishFailureMessage( + name(), + fmt::format("Failed to get required input port: {}", ports.error())); + return BT::NodeStatus::FAILURE; + } + + const auto& [odom_topic_name] = ports.value(); + + { + std::unique_lock lock(odom_mutex_); + current_odometry_.reset(); + } + + odom_subscriber_ = + shared_resources_->node->create_subscription( + odom_topic_name, + rclcpp::SystemDefaultsQoS(), + [this](const nav_msgs::msg::Odometry::SharedPtr msg) + { + std::unique_lock lock(odom_mutex_); + current_odometry_ = *msg; + }); + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus GetOdomInstance::onRunning() +{ + std::optional odom_copy; + + { + std::shared_lock lock(odom_mutex_); + if (!current_odometry_) + { + return BT::NodeStatus::RUNNING; + } + odom_copy = current_odometry_; + } + + setOutput(kPortIdOdomValue, *odom_copy); + return BT::NodeStatus::SUCCESS; +} + +void GetOdomInstance::onHalted() +{ + odom_subscriber_.reset(); + + std::unique_lock lock(odom_mutex_); + current_odometry_.reset(); +} + +} // namespace get_odom_instance diff --git a/src/get_odom_instance/src/register_behaviors.cpp b/src/get_odom_instance/src/register_behaviors.cpp new file mode 100644 index 000000000..9efe7bf85 --- /dev/null +++ b/src/get_odom_instance/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace get_odom_instance +{ +class GetOdomInstanceBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "GetOdomInstance", shared_resources); + + } +}; +} // namespace get_odom_instance + +PLUGINLIB_EXPORT_CLASS(get_odom_instance::GetOdomInstanceBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/get_odom_instance/test/CMakeLists.txt b/src/get_odom_instance/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/get_odom_instance/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/get_odom_instance/test/test_behavior_plugins.cpp b/src/get_odom_instance/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..6b57e8026 --- /dev/null +++ b/src/get_odom_instance/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("get_odom_instance::GetOdomInstanceBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "GetOdomInstance", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/publish_odom/CMakeLists.txt b/src/publish_odom/CMakeLists.txt new file mode 100644 index 000000000..0f4c2194c --- /dev/null +++ b/src/publish_odom/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(publish_odom CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + publish_odom + SHARED + src/publish_odom.cpp + src/register_behaviors.cpp) +target_include_directories( + publish_odom + PUBLIC $ + $) +ament_target_dependencies(publish_odom + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS publish_odom + EXPORT publish_odomTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(publish_odom) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface publish_odom_plugin_description.xml) + +ament_export_targets(publish_odomTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/publish_odom/behavior_plugin.yaml b/src/publish_odom/behavior_plugin.yaml new file mode 100644 index 000000000..35122885b --- /dev/null +++ b/src/publish_odom/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + publish_odom: + - "publish_odom::PublishOdomBehaviorsLoader" diff --git a/src/publish_odom/include/publish_odom/publish_odom.hpp b/src/publish_odom/include/publish_odom/publish_odom.hpp new file mode 100644 index 000000000..cee24630f --- /dev/null +++ b/src/publish_odom/include/publish_odom/publish_odom.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace publish_odom +{ + +/** + * @brief Publish a nav_msgs::msg::Odometry message to a ROS 2 topic. + * + * @details + * This Behavior publishes an Odometry message provided via a behavior tree + * input port to a specified topic. The publisher is created lazily on the + * first tick and reused on subsequent ticks. + * + * This Behavior is synchronous and must return quickly. + */ +class PublishOdom + : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the PublishOdom Behavior. + * + * @param name The name of this Behavior instance in the behavior tree. + * @param config Runtime configuration provided by the behavior tree factory. + * @param shared_resources Shared BehaviorContext owned by the Studio Agent. + */ + PublishOdom( + const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + /** + * @brief Define the input ports for the PublishOdom Behavior. + * + * Ports: + * - Input: nav_msgs::msg::Odometry ("message") + * - Input: std::string ("topic_name") + * - Input: size_t ("queue_size") + * + * @return The list of ports used by this Behavior. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Metadata for display in the MoveIt Studio Developer Tool. + * + * @return A key-value vector describing the Behavior. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Publish the odometry message. + * + * @return BT::NodeStatus::SUCCESS if published successfully, + * BT::NodeStatus::FAILURE otherwise. + */ + BT::NodeStatus tick() override; + +private: + /// Cached publisher (created on first tick) + rclcpp::Publisher::SharedPtr publisher_; + + /// Cached topic name to detect changes + std::string topic_name_; +}; + +} // namespace publish_odom diff --git a/src/publish_odom/package.xml b/src/publish_odom/package.xml new file mode 100644 index 000000000..317e4e28f --- /dev/null +++ b/src/publish_odom/package.xml @@ -0,0 +1,30 @@ + + + publish_odom + 0.0.0 + Publishes odometry messages + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/publish_odom/publish_odom_plugin_description.xml b/src/publish_odom/publish_odom_plugin_description.xml new file mode 100644 index 000000000..0555b55e2 --- /dev/null +++ b/src/publish_odom/publish_odom_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/publish_odom/src/publish_odom.cpp b/src/publish_odom/src/publish_odom.cpp new file mode 100644 index 000000000..866e987c2 --- /dev/null +++ b/src/publish_odom/src/publish_odom.cpp @@ -0,0 +1,104 @@ +#include + +#include + +#include + +namespace publish_odom +{ + +PublishOdom::PublishOdom( + const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode( + name, config, shared_resources) +{ +} + +BT::PortsList PublishOdom::providedPorts() +{ + return { + BT::InputPort( + "message", + nav_msgs::msg::Odometry(), + "The odometry message to publish"), + BT::InputPort( + "topic_name", + "", + "The topic the odometry message should be published to."), + BT::InputPort( + "queue_size", + 1, + "The queue size for the publisher.") + }; +} + +BT::KeyValueVector PublishOdom::metadata() +{ + return { + { moveit_studio::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, + { moveit_studio::behaviors::kDescriptionMetadataKey, + "Publish a nav_msgs::msg::Odometry message to a ROS 2 topic." } + }; +} + +BT::NodeStatus PublishOdom::tick() +{ + // --- Read inputs -------------------------------------------------- + auto msg_res = getInput("message"); + auto topic_res = getInput("topic_name"); + auto queue_res = getInput("queue_size"); + + if (!msg_res || !topic_res || !queue_res) + { + shared_resources_->logger->publishFailureMessage( + name(), "Missing required input port(s) for PublishOdom."); + return BT::NodeStatus::FAILURE; + } + + const auto& msg = msg_res.value(); + const auto& topic_name = topic_res.value(); + const auto queue_size = queue_res.value(); + + if (topic_name.empty()) + { + shared_resources_->logger->publishFailureMessage( + name(), "Topic name is empty."); + return BT::NodeStatus::FAILURE; + } + + // --- Create or update publisher ---------------------------------- + if (!publisher_ || topic_name != topic_name_) + { + publisher_ = shared_resources_->node->create_publisher( + topic_name, queue_size); + + topic_name_ = topic_name; + + shared_resources_->logger->publishInfoMessage( + name(), + "Created odometry publisher on topic '" + topic_name_ + "'"); + } + + // --- Publish ------------------------------------------------------ + publisher_->publish(msg); + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format("Published odometry message has child frame id: '{}'", msg.child_frame_id)); + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format("Published odometry message has frame id: '{}'", msg.header.frame_id)); + + // Optional: lightweight debug (console only) + // spdlog::debug( + // "[PublishOdom] Published odometry on '{}', frame_id='{}'", + // topic_name_, + // msg.header.frame_id); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace publish_odom diff --git a/src/publish_odom/src/register_behaviors.cpp b/src/publish_odom/src/register_behaviors.cpp new file mode 100644 index 000000000..42d82245f --- /dev/null +++ b/src/publish_odom/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace publish_odom +{ +class PublishOdomBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "PublishOdom", shared_resources); + + } +}; +} // namespace publish_odom + +PLUGINLIB_EXPORT_CLASS(publish_odom::PublishOdomBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/publish_odom/test/CMakeLists.txt b/src/publish_odom/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/publish_odom/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/publish_odom/test/test_behavior_plugins.cpp b/src/publish_odom/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..bf17bbb3d --- /dev/null +++ b/src/publish_odom/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("publish_odom::PublishOdomBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "PublishOdom", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/transform_odom_with_pose/CMakeLists.txt b/src/transform_odom_with_pose/CMakeLists.txt new file mode 100644 index 000000000..ee83dca62 --- /dev/null +++ b/src/transform_odom_with_pose/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(transform_odom_with_pose CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + transform_odom_with_pose + SHARED + src/transform_odom_with_pose.cpp + src/register_behaviors.cpp) +target_include_directories( + transform_odom_with_pose + PUBLIC $ + $) +ament_target_dependencies(transform_odom_with_pose + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS transform_odom_with_pose + EXPORT transform_odom_with_poseTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(transform_odom_with_pose) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface transform_odom_with_pose_plugin_description.xml) + +ament_export_targets(transform_odom_with_poseTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/transform_odom_with_pose/behavior_plugin.yaml b/src/transform_odom_with_pose/behavior_plugin.yaml new file mode 100644 index 000000000..b08221f6a --- /dev/null +++ b/src/transform_odom_with_pose/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + transform_odom_with_pose: + - "transform_odom_with_pose::TransformOdomWithPoseBehaviorsLoader" diff --git a/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp b/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp new file mode 100644 index 000000000..67cd5c73e --- /dev/null +++ b/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp @@ -0,0 +1,79 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace transform_odom_with_pose +{ + +/** + * @brief Transform an Odometry message into a different reference frame using + * a PoseStamped-defined transform. + * + * @details + * This Behavior applies a transform defined by a geometry_msgs::msg::PoseStamped + * to an input nav_msgs::msg::Odometry message. The pose component is fully + * transformed, while the twist component is rotated (not translated). + * + * This Behavior is synchronous and intended for lightweight, deterministic + * frame transformations (e.g., logging, frame alignment, or data conditioning). + */ +class TransformOdomWithPose + : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the TransformOdomWithPose Behavior. + * + * @param name The name of this Behavior instance in the behavior tree. + * @param config Runtime configuration provided by the behavior tree factory, + * including port mappings. + * @param shared_resources Shared BehaviorContext owned by the Studio Agent. + * + * @note Members of the base Behavior class are not initialized until after + * initialize() is called. They must not be used in the constructor. + */ + TransformOdomWithPose( + const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + /** + * @brief Define the input and output ports for this Behavior. + * + * Ports: + * - Input: nav_msgs::msg::Odometry ("odom_in") + * - Input: geometry_msgs::msg::PoseStamped ("transform_pose") + * - Output: nav_msgs::msg::Odometry ("odom_out") + * + * @return The list of ports used by this Behavior. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Metadata for display in the MoveIt Studio Developer Tool. + * + * @return A key-value vector describing the Behavior. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Execute the odometry transformation. + * + * @details + * This function retrieves the input odometry and transform pose from the + * behavior tree, applies the transform, and outputs the transformed odometry. + * Since this is a synchronous action, it must complete quickly and must not + * block. + * + * @return BT::NodeStatus::SUCCESS on successful transformation, + * BT::NodeStatus::FAILURE otherwise. + */ + BT::NodeStatus tick() override; +}; + +} // namespace transform_odom_with_pose diff --git a/src/transform_odom_with_pose/package.xml b/src/transform_odom_with_pose/package.xml new file mode 100644 index 000000000..765a88b80 --- /dev/null +++ b/src/transform_odom_with_pose/package.xml @@ -0,0 +1,30 @@ + + + transform_odom_with_pose + 0.0.0 + Transforms odometry messages with a transform defined by a pose stamped message in a different frame. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/transform_odom_with_pose/src/register_behaviors.cpp b/src/transform_odom_with_pose/src/register_behaviors.cpp new file mode 100644 index 000000000..98e6c9096 --- /dev/null +++ b/src/transform_odom_with_pose/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace transform_odom_with_pose +{ +class TransformOdomWithPoseBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "TransformOdomWithPose", shared_resources); + + } +}; +} // namespace transform_odom_with_pose + +PLUGINLIB_EXPORT_CLASS(transform_odom_with_pose::TransformOdomWithPoseBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp new file mode 100644 index 000000000..7130e125a --- /dev/null +++ b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp @@ -0,0 +1,179 @@ +#include + +#include +#include +#include + +#include "spdlog/spdlog.h" +#include + +namespace transform_odom_with_pose +{ + +TransformOdomWithPose::TransformOdomWithPose( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList TransformOdomWithPose::providedPorts() +{ + return { + BT::InputPort( + "odom_in", "Input odometry message"), + BT::InputPort( + "transform_pose", "PoseStamped defining the transform between frames"), + BT::OutputPort( + "odom_out", "Transformed odometry message") + }; +} + +BT::KeyValueVector TransformOdomWithPose::metadata() +{ + return { + {"description", + "Transforms an Odometry message into a different frame using a PoseStamped-defined transform."}, + {"subcategory", "User Created Behaviors"} + }; +} + +BT::NodeStatus TransformOdomWithPose::tick() +{ + // --- Get inputs ---------------------------------------------------- + auto odom_in_res = getInput("odom_in"); + auto pose_res = getInput("transform_pose"); + + if (!odom_in_res && !pose_res) + { + shared_resources_->logger->publishFailureMessage( + name(), "Missing both required inputs: odom_in or transform_pose! "); + return BT::NodeStatus::FAILURE; + } + + + if (!odom_in_res || !pose_res) + { + if (!odom_in_res){ + shared_resources_->logger->publishFailureMessage( + name(), "Missing required input: odom_in!"); + return BT::NodeStatus::FAILURE; + } + if (!pose_res){ + shared_resources_->logger->publishFailureMessage( + name(), "Missing required input: transform_pose!"); + return BT::NodeStatus::FAILURE; + } + } + + const auto& odom_in = odom_in_res.value(); + const auto& transform_pose = pose_res.value(); + + // --- Convert PoseStamped → TransformStamped ------------------------ + geometry_msgs::msg::TransformStamped transform; + transform.header = transform_pose.header; + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format( + "Transform header: frame_id='{}', stamp={}.{}", + transform_pose.header.frame_id, + transform_pose.header.stamp.sec, + transform_pose.header.stamp.nanosec)); + + transform.child_frame_id = odom_in.header.frame_id; + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format( + "Input odometry frame_id: '{}'", + odom_in.header.frame_id)); + + transform.transform.translation.x = transform_pose.pose.position.x; + transform.transform.translation.y = transform_pose.pose.position.y; + transform.transform.translation.z = transform_pose.pose.position.z; + transform.transform.rotation = transform_pose.pose.orientation; + + // --- Transform pose ------------------------------------------------ + geometry_msgs::msg::PoseStamped pose_in; + pose_in.header = odom_in.header; + pose_in.pose = odom_in.pose.pose; + + geometry_msgs::msg::PoseStamped pose_out; + tf2::doTransform(pose_in, pose_out, transform); + + // --- Transform twist (rotation only) ------------------------------- + tf2::Quaternion q; + tf2::fromMsg(transform.transform.rotation, q); + tf2::Matrix3x3 R(q); + + tf2::Vector3 lin_in( + odom_in.twist.twist.linear.x, + odom_in.twist.twist.linear.y, + odom_in.twist.twist.linear.z); + + tf2::Vector3 ang_in( + odom_in.twist.twist.angular.x, + odom_in.twist.twist.angular.y, + odom_in.twist.twist.angular.z); + + tf2::Vector3 lin_out = R * lin_in; + tf2::Vector3 ang_out = R * ang_in; + + // --- Assemble output odometry -------------------------------------- + nav_msgs::msg::Odometry odom_out; + odom_out.header.stamp = odom_in.header.stamp; + odom_out.header.frame_id = transform.header.frame_id; + odom_out.child_frame_id = odom_in.child_frame_id; + + odom_out.pose.pose = pose_out.pose; + + odom_out.pose.covariance = odom_in.pose.covariance; + + odom_out.twist.twist.linear.x = lin_out.x(); + odom_out.twist.twist.linear.y = lin_out.y(); + odom_out.twist.twist.linear.z = lin_out.z(); + + odom_out.twist.twist.angular.x = ang_out.x(); + odom_out.twist.twist.angular.y = ang_out.y(); + odom_out.twist.twist.angular.z = ang_out.z(); + + odom_out.twist.covariance = odom_in.twist.covariance; + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format("Odom out frame_id: '{}'", odom_out.header.frame_id)); + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format("Odom out child_frame_id: '{}'", odom_out.child_frame_id)); + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format( + "Odom out stamp: {}.{}", + odom_out.header.stamp.sec, + odom_out.header.stamp.nanosec)); + + shared_resources_->logger->publishInfoMessage( + name(), + fmt::format( + "Odom out pose: position=({:.3f}, {:.3f}, {:.3f}), " + "orientation=({:.3f}, {:.3f}, {:.3f}, {:.3f})", + odom_out.pose.pose.position.x, + odom_out.pose.pose.position.y, + odom_out.pose.pose.position.z, + odom_out.pose.pose.orientation.x, + odom_out.pose.pose.orientation.y, + odom_out.pose.pose.orientation.z, + odom_out.pose.pose.orientation.w)); + + // --- Output -------------------------------------------------------- + setOutput("odom_out", odom_out); + + shared_resources_->logger->publishInfoMessage( + name(), "Successfully transformed odometry."); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace transform_odom_with_pose diff --git a/src/transform_odom_with_pose/test/CMakeLists.txt b/src/transform_odom_with_pose/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/transform_odom_with_pose/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/transform_odom_with_pose/test/test_behavior_plugins.cpp b/src/transform_odom_with_pose/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..67f607993 --- /dev/null +++ b/src/transform_odom_with_pose/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("transform_odom_with_pose::TransformOdomWithPoseBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "TransformOdomWithPose", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml b/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml new file mode 100644 index 000000000..45c8e0f94 --- /dev/null +++ b/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml @@ -0,0 +1,7 @@ + + + + From 8f845f9f819c189aa8ad4b89b195c991f29465a7 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 30 Dec 2025 14:57:29 -0700 Subject: [PATCH 06/27] switched to lab sim, added visualization of the transformed controller frame Former-commit-id: 8fcc3ca1920a25b3d722d38705f1f9dbbe8b0968 --- .../CMakeLists.txt | 46 ++++++++ .../behavior_plugin.yaml | 4 + ...dom_to_pose_stamped_plugin_description.xml | 7 ++ .../convert_odom_to_pose_stamped.hpp | 49 +++++++++ src/convert_odom_to_pose_stamped/package.xml | 30 +++++ .../src/convert_odom_to_pose_stamped.cpp | 62 +++++++++++ .../src/register_behaviors.cpp | 24 ++++ .../test/CMakeLists.txt | 5 + .../test/test_behavior_plugins.cpp | 37 +++++++ .../objectives/teleop_with_meta.xml | 32 ++++-- src/get_empty/src/get_empty.cpp | 8 +- .../get_odom_instance/get_odom_instance.hpp | 2 + .../src/get_odom_instance.cpp | 42 ++++--- src/lab_sim/objectives/teleop_with_meta.xml | 39 +++++++ .../objectives/transformquestframe.xml | 26 +++++ src/publish_odom/src/publish_odom.cpp | 18 +-- src/publish_pose/CMakeLists.txt | 46 ++++++++ src/publish_pose/behavior_plugin.yaml | 4 + .../include/publish_pose/publish_pose.hpp | 61 ++++++++++ src/publish_pose/package.xml | 30 +++++ .../publish_pose_plugin_description.xml | 7 ++ src/publish_pose/src/publish_pose.cpp | 104 ++++++++++++++++++ src/publish_pose/src/register_behaviors.cpp | 24 ++++ src/publish_pose/test/CMakeLists.txt | 5 + .../test/test_behavior_plugins.cpp | 37 +++++++ .../src/transform_odom_with_pose.cpp | 76 ++++++------- 26 files changed, 750 insertions(+), 75 deletions(-) create mode 100644 src/convert_odom_to_pose_stamped/CMakeLists.txt create mode 100644 src/convert_odom_to_pose_stamped/behavior_plugin.yaml create mode 100644 src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml create mode 100644 src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp create mode 100644 src/convert_odom_to_pose_stamped/package.xml create mode 100644 src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp create mode 100644 src/convert_odom_to_pose_stamped/src/register_behaviors.cpp create mode 100644 src/convert_odom_to_pose_stamped/test/CMakeLists.txt create mode 100644 src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp create mode 100644 src/lab_sim/objectives/teleop_with_meta.xml create mode 100644 src/lab_sim/objectives/transformquestframe.xml create mode 100644 src/publish_pose/CMakeLists.txt create mode 100644 src/publish_pose/behavior_plugin.yaml create mode 100644 src/publish_pose/include/publish_pose/publish_pose.hpp create mode 100644 src/publish_pose/package.xml create mode 100644 src/publish_pose/publish_pose_plugin_description.xml create mode 100644 src/publish_pose/src/publish_pose.cpp create mode 100644 src/publish_pose/src/register_behaviors.cpp create mode 100644 src/publish_pose/test/CMakeLists.txt create mode 100644 src/publish_pose/test/test_behavior_plugins.cpp diff --git a/src/convert_odom_to_pose_stamped/CMakeLists.txt b/src/convert_odom_to_pose_stamped/CMakeLists.txt new file mode 100644 index 000000000..7fc6cb079 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(convert_odom_to_pose_stamped CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + convert_odom_to_pose_stamped + SHARED + src/convert_odom_to_pose_stamped.cpp + src/register_behaviors.cpp) +target_include_directories( + convert_odom_to_pose_stamped + PUBLIC $ + $) +ament_target_dependencies(convert_odom_to_pose_stamped + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS convert_odom_to_pose_stamped + EXPORT convert_odom_to_pose_stampedTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(convert_odom_to_pose_stamped) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface convert_odom_to_pose_stamped_plugin_description.xml) + +ament_export_targets(convert_odom_to_pose_stampedTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/convert_odom_to_pose_stamped/behavior_plugin.yaml b/src/convert_odom_to_pose_stamped/behavior_plugin.yaml new file mode 100644 index 000000000..c3de59118 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + convert_odom_to_pose_stamped: + - "convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader" diff --git a/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml b/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml new file mode 100644 index 000000000..dd57696a8 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp b/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp new file mode 100644 index 000000000..0e6e86e8b --- /dev/null +++ b/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + + + + + +namespace convert_odom_to_pose_stamped +{ +/** + * @brief TODO(...) + */ +class ConvertOdomToPoseStamped : public BT::SyncActionNode +{ +public: + /** + * @brief Constructor for the convert_odom_to_pose_stamped behavior. + * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. + */ + ConvertOdomToPoseStamped(const std::string& name, const BT::NodeConfiguration& config); + + + /** + * @brief Implementation of the required providedPorts() function for the convert_odom_to_pose_stamped Behavior. + * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList. + * This function returns a list of ports with their names and port info, which is used internally by the behavior tree. + * @return convert_odom_to_pose_stamped does not expose any ports, so this function returns an empty list. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Implementation of BT::SyncActionNode::tick() for ConvertOdomToPoseStamped. + * @details This function is where the Behavior performs its work when the behavior tree is being run. Since ConvertOdomToPoseStamped is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly. + */ + BT::NodeStatus tick() override; + +}; +} // namespace convert_odom_to_pose_stamped diff --git a/src/convert_odom_to_pose_stamped/package.xml b/src/convert_odom_to_pose_stamped/package.xml new file mode 100644 index 000000000..17aee5deb --- /dev/null +++ b/src/convert_odom_to_pose_stamped/package.xml @@ -0,0 +1,30 @@ + + + convert_odom_to_pose_stamped + 0.0.0 + This removes the twist part of the odom message so you can visualize/operate on the pose. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp b/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp new file mode 100644 index 000000000..e805e4e88 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp @@ -0,0 +1,62 @@ +#include + +#include "spdlog/spdlog.h" + +#include +#include + + +namespace +{ +constexpr auto kPortIDOdometry = "odometry"; +constexpr auto kPortIDPoseStamped = "pose_stamped"; +} // namespace + +namespace convert_odom_to_pose_stamped +{ +ConvertOdomToPoseStamped::ConvertOdomToPoseStamped(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) +{ +} + + +BT::PortsList ConvertOdomToPoseStamped::providedPorts() +{ + return BT::PortsList( + { BT::InputPort(kPortIDOdometry, "{odometry}", + "The gnav_msgs::msg::Odometry message to " + "convert."), + BT::OutputPort(kPortIDPoseStamped, "{pose_stamped}", + "The converted geometry_msgs::msg::PoseStamped message.") }); +} + +BT::KeyValueVector ConvertOdomToPoseStamped::metadata() +{ + return { { "description", "Converts a nav_msgs::msg::Odometry message into a " + "geometry_msgs::msg::PoseStamped message." }, + { "subcategory", "Conversions" } }; +} + +BT::NodeStatus ConvertOdomToPoseStamped::tick() +{ + const auto ports = moveit_studio::behaviors::getRequiredInputs( + getInput(kPortIDOdometry)); + + if (!ports.has_value()) + { + spdlog::warn("Failed to get required value from input data port: {}", ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& odom_msg = std::get<0>(ports.value()); + + geometry_msgs::msg::PoseStamped pose_out; + pose_out.header = odom_msg.header; + pose_out.pose = odom_msg.pose.pose; + + setOutput(kPortIDPoseStamped, pose_out); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace convert_odom_to_pose_stamped diff --git a/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp b/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp new file mode 100644 index 000000000..e0c0c3c69 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace convert_odom_to_pose_stamped +{ +class ConvertOdomToPoseStampedBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "ConvertOdomToPoseStamped"); + + } +}; +} // namespace convert_odom_to_pose_stamped + +PLUGINLIB_EXPORT_CLASS(convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/convert_odom_to_pose_stamped/test/CMakeLists.txt b/src/convert_odom_to_pose_stamped/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/convert_odom_to_pose_stamped/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp b/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..e928c91f2 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ConvertOdomToPoseStamped", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/factory_sim/objectives/teleop_with_meta.xml b/src/factory_sim/objectives/teleop_with_meta.xml index 78e425d7f..b173ab0ba 100644 --- a/src/factory_sim/objectives/teleop_with_meta.xml +++ b/src/factory_sim/objectives/teleop_with_meta.xml @@ -2,29 +2,43 @@ + + - + - + + + + + + - - - - - - + + + + + + + + + + + + + - + diff --git a/src/get_empty/src/get_empty.cpp b/src/get_empty/src/get_empty.cpp index de74c01f5..56b0791af 100644 --- a/src/get_empty/src/get_empty.cpp +++ b/src/get_empty/src/get_empty.cpp @@ -59,8 +59,8 @@ BT::NodeStatus GetEmpty::onStart() message_count_++; }); - shared_resources_->logger->publishInfoMessage( - name(), fmt::format("Subscribed to empty topic: {}", empty_topic_name)); + // shared_resources_->logger->publishInfoMessage( + // name(), fmt::format("Subscribed to empty topic: {}", empty_topic_name)); return BT::NodeStatus::RUNNING; } @@ -81,8 +81,8 @@ BT::NodeStatus GetEmpty::onRunning() // Stop listening once condition is satisfied empty_subscriber_.reset(); - shared_resources_->logger->publishInfoMessage( - name(), "Empty message received — exiting behavior successfully."); + // shared_resources_->logger->publishInfoMessage( + // name(), "Empty message received — exiting behavior successfully."); return BT::NodeStatus::SUCCESS; } diff --git a/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp index 61fdd5105..f888ab2e9 100644 --- a/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp +++ b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp @@ -49,6 +49,8 @@ class GetOdomInstance /** @brief Latest received odometry message, if any. */ std::optional current_odometry_; + + std::string odom_topic_name_; }; } // namespace get_odom_instance diff --git a/src/get_odom_instance/src/get_odom_instance.cpp b/src/get_odom_instance/src/get_odom_instance.cpp index 1c8636b93..84669e3f8 100644 --- a/src/get_odom_instance/src/get_odom_instance.cpp +++ b/src/get_odom_instance/src/get_odom_instance.cpp @@ -20,6 +20,15 @@ GetOdomInstance::GetOdomInstance( : moveit_studio::behaviors::SharedResourcesNode( name, config, shared_resources) { + odom_subscriber_ = + shared_resources_->node->create_subscription( + "odom", // temporary default; overridden in onStart if needed + rclcpp::SystemDefaultsQoS(), + [this](const nav_msgs::msg::Odometry::SharedPtr msg) + { + std::unique_lock lock(odom_mutex_); + current_odometry_ = *msg; + }); } BT::PortsList GetOdomInstance::providedPorts() @@ -52,31 +61,30 @@ BT::NodeStatus GetOdomInstance::onStart() moveit_studio::behaviors::getRequiredInputs( getInput(kPortIdOdomTopicName)); - if (!ports.has_value()) + if (!ports) { shared_resources_->logger->publishFailureMessage( - name(), - fmt::format("Failed to get required input port: {}", ports.error())); + name(), ports.error()); return BT::NodeStatus::FAILURE; } const auto& [odom_topic_name] = ports.value(); + // Recreate subscriber only if topic changed + if (!odom_subscriber_ || odom_topic_name != odom_topic_name_) { - std::unique_lock lock(odom_mutex_); - current_odometry_.reset(); + odom_topic_name_ = odom_topic_name; + odom_subscriber_ = + shared_resources_->node->create_subscription( + odom_topic_name_, + rclcpp::SystemDefaultsQoS(), + [this](const nav_msgs::msg::Odometry::SharedPtr msg) + { + std::unique_lock lock(odom_mutex_); + current_odometry_ = *msg; + }); } - odom_subscriber_ = - shared_resources_->node->create_subscription( - odom_topic_name, - rclcpp::SystemDefaultsQoS(), - [this](const nav_msgs::msg::Odometry::SharedPtr msg) - { - std::unique_lock lock(odom_mutex_); - current_odometry_ = *msg; - }); - return BT::NodeStatus::RUNNING; } @@ -94,6 +102,10 @@ BT::NodeStatus GetOdomInstance::onRunning() } setOutput(kPortIdOdomValue, *odom_copy); + + // Stop listening — snapshot behavior + odom_subscriber_.reset(); + return BT::NodeStatus::SUCCESS; } diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml new file mode 100644 index 000000000..9fb2cb4a4 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/transformquestframe.xml b/src/lab_sim/objectives/transformquestframe.xml new file mode 100644 index 000000000..b77d94412 --- /dev/null +++ b/src/lab_sim/objectives/transformquestframe.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/publish_odom/src/publish_odom.cpp b/src/publish_odom/src/publish_odom.cpp index 866e987c2..586ab5409 100644 --- a/src/publish_odom/src/publish_odom.cpp +++ b/src/publish_odom/src/publish_odom.cpp @@ -76,21 +76,21 @@ BT::NodeStatus PublishOdom::tick() topic_name_ = topic_name; - shared_resources_->logger->publishInfoMessage( - name(), - "Created odometry publisher on topic '" + topic_name_ + "'"); + // shared_resources_->logger->publishInfoMessage( + // name(), + // "Created odometry publisher on topic '" + topic_name_ + "'"); } // --- Publish ------------------------------------------------------ publisher_->publish(msg); - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format("Published odometry message has child frame id: '{}'", msg.child_frame_id)); + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format("Published odometry message has child frame id: '{}'", msg.child_frame_id)); - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format("Published odometry message has frame id: '{}'", msg.header.frame_id)); + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format("Published odometry message has frame id: '{}'", msg.header.frame_id)); // Optional: lightweight debug (console only) // spdlog::debug( diff --git a/src/publish_pose/CMakeLists.txt b/src/publish_pose/CMakeLists.txt new file mode 100644 index 000000000..e263261d4 --- /dev/null +++ b/src/publish_pose/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(publish_pose CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + publish_pose + SHARED + src/publish_pose.cpp + src/register_behaviors.cpp) +target_include_directories( + publish_pose + PUBLIC $ + $) +ament_target_dependencies(publish_pose + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS publish_pose + EXPORT publish_poseTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(publish_pose) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface publish_pose_plugin_description.xml) + +ament_export_targets(publish_poseTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/publish_pose/behavior_plugin.yaml b/src/publish_pose/behavior_plugin.yaml new file mode 100644 index 000000000..1287cb304 --- /dev/null +++ b/src/publish_pose/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + publish_pose: + - "publish_pose::PublishPoseBehaviorsLoader" diff --git a/src/publish_pose/include/publish_pose/publish_pose.hpp b/src/publish_pose/include/publish_pose/publish_pose.hpp new file mode 100644 index 000000000..cc46e7fae --- /dev/null +++ b/src/publish_pose/include/publish_pose/publish_pose.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +// This header includes the SharedResourcesNode type +#include + + +#include + +namespace publish_pose +{ +/** + * @brief TODO(...) + */ +class PublishPose : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the publish_pose behavior. + * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode. + * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree. + * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. + */ + PublishPose(const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + + /** + * @brief Implementation of the required providedPorts() function for the publish_pose Behavior. + * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList. + * This function returns a list of ports with their names and port info, which is used internally by the behavior tree. + * @return publish_pose does not expose any ports, so this function returns an empty list. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Implementation of BT::SyncActionNode::tick() for PublishPose. + * @details This function is where the Behavior performs its work when the behavior tree is being run. Since PublishPose is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly. + */ + BT::NodeStatus tick() override; + +private: + /// Cached publisher (created on first tick) + rclcpp::Publisher::SharedPtr publisher_; + + /// Cached topic name to detect changes + std::string topic_name_; + +}; +} // namespace publish_pose diff --git a/src/publish_pose/package.xml b/src/publish_pose/package.xml new file mode 100644 index 000000000..126cf0c3b --- /dev/null +++ b/src/publish_pose/package.xml @@ -0,0 +1,30 @@ + + + publish_pose + 0.0.0 + Publish pose message + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/publish_pose/publish_pose_plugin_description.xml b/src/publish_pose/publish_pose_plugin_description.xml new file mode 100644 index 000000000..09204e1c1 --- /dev/null +++ b/src/publish_pose/publish_pose_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/publish_pose/src/publish_pose.cpp b/src/publish_pose/src/publish_pose.cpp new file mode 100644 index 000000000..b9e8830b3 --- /dev/null +++ b/src/publish_pose/src/publish_pose.cpp @@ -0,0 +1,104 @@ +#include + +#include + +#include + +namespace publish_pose +{ + +PublishPose::PublishPose( + const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode( + name, config, shared_resources) +{ +} + +BT::PortsList PublishPose::providedPorts() +{ + return { + BT::InputPort( + "message", + geometry_msgs::msg::PoseStamped(), + "The pose stamped message to publish"), + BT::InputPort( + "topic_name", + "", + "The topic the pose stamped message should be published to."), + BT::InputPort( + "queue_size", + 1, + "The queue size for the publisher.") + }; +} + +BT::KeyValueVector PublishPose::metadata() +{ + return { + { moveit_studio::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, + { moveit_studio::behaviors::kDescriptionMetadataKey, + "Publish a geometry_msgs::msg::PoseStamped message to a ROS 2 topic." } + }; +} + +BT::NodeStatus PublishPose::tick() +{ + // --- Read inputs -------------------------------------------------- + auto msg_res = getInput("message"); + auto topic_res = getInput("topic_name"); + auto queue_res = getInput("queue_size"); + + if (!msg_res || !topic_res || !queue_res) + { + shared_resources_->logger->publishFailureMessage( + name(), "Missing required input port(s) for PublishPose."); + return BT::NodeStatus::FAILURE; + } + + const auto& msg = msg_res.value(); + const auto& topic_name = topic_res.value(); + const auto queue_size = queue_res.value(); + + if (topic_name.empty()) + { + shared_resources_->logger->publishFailureMessage( + name(), "Topic name is empty."); + return BT::NodeStatus::FAILURE; + } + + // --- Create or update publisher ---------------------------------- + if (!publisher_ || topic_name != topic_name_) + { + publisher_ = shared_resources_->node->create_publisher( + topic_name, queue_size); + + topic_name_ = topic_name; + + // shared_resources_->logger->publishInfoMessage( + // name(), + // "Created pose stamped publisher on topic '" + topic_name_ + "'"); + } + + // --- Publish ------------------------------------------------------ + publisher_->publish(msg); + + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format("Published pose stamped message has child frame id: '{}'", msg.child_frame_id)); + + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format("Published pose stamped message has frame id: '{}'", msg.header.frame_id)); + + // Optional: lightweight debug (console only) + // spdlog::debug( + // "[PublishPose] Published pose stamped on '{}', frame_id='{}'", + // topic_name_, + // msg.header.frame_id); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace publish_pose diff --git a/src/publish_pose/src/register_behaviors.cpp b/src/publish_pose/src/register_behaviors.cpp new file mode 100644 index 000000000..5dfb2897a --- /dev/null +++ b/src/publish_pose/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace publish_pose +{ +class PublishPoseBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "PublishPose", shared_resources); + + } +}; +} // namespace publish_pose + +PLUGINLIB_EXPORT_CLASS(publish_pose::PublishPoseBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/publish_pose/test/CMakeLists.txt b/src/publish_pose/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/publish_pose/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/publish_pose/test/test_behavior_plugins.cpp b/src/publish_pose/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..9acdb67d0 --- /dev/null +++ b/src/publish_pose/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("publish_pose::PublishPoseBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "PublishPose", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp index 7130e125a..12d512617 100644 --- a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp +++ b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp @@ -72,21 +72,21 @@ BT::NodeStatus TransformOdomWithPose::tick() // --- Convert PoseStamped → TransformStamped ------------------------ geometry_msgs::msg::TransformStamped transform; transform.header = transform_pose.header; - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format( - "Transform header: frame_id='{}', stamp={}.{}", - transform_pose.header.frame_id, - transform_pose.header.stamp.sec, - transform_pose.header.stamp.nanosec)); + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format( + // "Transform header: frame_id='{}', stamp={}.{}", + // transform_pose.header.frame_id, + // transform_pose.header.stamp.sec, + // transform_pose.header.stamp.nanosec)); transform.child_frame_id = odom_in.header.frame_id; - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format( - "Input odometry frame_id: '{}'", - odom_in.header.frame_id)); + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format( + // "Input odometry frame_id: '{}'", + // odom_in.header.frame_id)); transform.transform.translation.x = transform_pose.pose.position.x; transform.transform.translation.y = transform_pose.pose.position.y; @@ -139,33 +139,33 @@ BT::NodeStatus TransformOdomWithPose::tick() odom_out.twist.covariance = odom_in.twist.covariance; - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format("Odom out frame_id: '{}'", odom_out.header.frame_id)); + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format("Odom out frame_id: '{}'", odom_out.header.frame_id)); - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format("Odom out child_frame_id: '{}'", odom_out.child_frame_id)); - - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format( - "Odom out stamp: {}.{}", - odom_out.header.stamp.sec, - odom_out.header.stamp.nanosec)); - - shared_resources_->logger->publishInfoMessage( - name(), - fmt::format( - "Odom out pose: position=({:.3f}, {:.3f}, {:.3f}), " - "orientation=({:.3f}, {:.3f}, {:.3f}, {:.3f})", - odom_out.pose.pose.position.x, - odom_out.pose.pose.position.y, - odom_out.pose.pose.position.z, - odom_out.pose.pose.orientation.x, - odom_out.pose.pose.orientation.y, - odom_out.pose.pose.orientation.z, - odom_out.pose.pose.orientation.w)); + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format("Odom out child_frame_id: '{}'", odom_out.child_frame_id)); + + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format( + // "Odom out stamp: {}.{}", + // odom_out.header.stamp.sec, + // odom_out.header.stamp.nanosec)); + + // shared_resources_->logger->publishInfoMessage( + // name(), + // fmt::format( + // "Odom out pose: position=({:.3f}, {:.3f}, {:.3f}), " + // "orientation=({:.3f}, {:.3f}, {:.3f}, {:.3f})", + // odom_out.pose.pose.position.x, + // odom_out.pose.pose.position.y, + // odom_out.pose.pose.position.z, + // odom_out.pose.pose.orientation.x, + // odom_out.pose.pose.orientation.y, + // odom_out.pose.pose.orientation.z, + // odom_out.pose.pose.orientation.w)); // --- Output -------------------------------------------------------- setOutput("odom_out", odom_out); From 5acc640fc22e7d7dd5c815ace7aa4414aab58e9c Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 30 Dec 2025 15:55:25 -0700 Subject: [PATCH 07/27] new trasnform quest frame subtree Former-commit-id: 68a8f8aa76052fb7f1ed9e2007a5661f64cdab6b --- .../objectives/transformquestframe.xml | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/lab_sim/objectives/transformquestframe.xml b/src/lab_sim/objectives/transformquestframe.xml index b77d94412..d58aefd3a 100644 --- a/src/lab_sim/objectives/transformquestframe.xml +++ b/src/lab_sim/objectives/transformquestframe.xml @@ -2,25 +2,35 @@ - + - + - - - - - + + + + + + + + + + + + + + + + - - + From cc7bb138e6fcdb790395c4f18d834ad49d395ec7 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 30 Dec 2025 19:02:53 -0700 Subject: [PATCH 08/27] finally got the transforms sorted out :) Former-commit-id: ea77333e03668cbadddec5527871d9c289ca9732 --- src/lab_sim/objectives/teleop_with_meta.xml | 68 ++++++++++++--------- 1 file changed, 40 insertions(+), 28 deletions(-) diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 9fb2cb4a4..06b9391e1 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -1,37 +1,49 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + - + From a0881e516b3f781ef56ea6ff6bfc52c2613561fe Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 30 Dec 2025 19:38:08 -0700 Subject: [PATCH 09/27] yay realignmet works, transform works and the continuous publishing works! Former-commit-id: 6d9f5137ec47813f1dd38f9780de4b4f9b45621b --- .../objectives/debug_transformation.xml | 18 ++ .../objectives/get_transform_frame_pose.xml | 34 +--- src/lab_sim/objectives/teleop_with_meta.xml | 39 ++++- .../objectives/transformquestframe.xml | 18 +- .../src/transform_odom_with_pose.cpp | 157 ++++++------------ 5 files changed, 118 insertions(+), 148 deletions(-) create mode 100644 src/lab_sim/objectives/debug_transformation.xml diff --git a/src/lab_sim/objectives/debug_transformation.xml b/src/lab_sim/objectives/debug_transformation.xml new file mode 100644 index 000000000..e21a25e3f --- /dev/null +++ b/src/lab_sim/objectives/debug_transformation.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/get_transform_frame_pose.xml b/src/lab_sim/objectives/get_transform_frame_pose.xml index 67346b315..6f2b5872b 100644 --- a/src/lab_sim/objectives/get_transform_frame_pose.xml +++ b/src/lab_sim/objectives/get_transform_frame_pose.xml @@ -1,38 +1,20 @@ - + - + - - + + - - - - + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 06b9391e1..f55b92b2d 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -1,6 +1,6 @@ - + @@ -19,13 +19,39 @@ + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -34,11 +60,14 @@ + - + + + diff --git a/src/lab_sim/objectives/transformquestframe.xml b/src/lab_sim/objectives/transformquestframe.xml index d58aefd3a..57dc25c60 100644 --- a/src/lab_sim/objectives/transformquestframe.xml +++ b/src/lab_sim/objectives/transformquestframe.xml @@ -8,29 +8,17 @@ - + - - - - - - - - - - - - - - + + diff --git a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp index 12d512617..d26355e7e 100644 --- a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp +++ b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp @@ -7,6 +7,9 @@ #include "spdlog/spdlog.h" #include +#include +#include + namespace transform_odom_with_pose { @@ -23,7 +26,11 @@ BT::PortsList TransformOdomWithPose::providedPorts() BT::InputPort( "odom_in", "Input odometry message"), BT::InputPort( - "transform_pose", "PoseStamped defining the transform between frames"), + "ee_pose_at_grip", "EE pose in world frame at grip time"), + BT::InputPort( + "controller_pose_at_grip", "Controller pose in world frame at grip time"), // NEW + BT::InputPort( + "destination_child_frame", "The child frame you want the transformed odom message to have"), BT::OutputPort( "odom_out", "Transformed odometry message") }; @@ -40,71 +47,58 @@ BT::KeyValueVector TransformOdomWithPose::metadata() BT::NodeStatus TransformOdomWithPose::tick() { - // --- Get inputs ---------------------------------------------------- auto odom_in_res = getInput("odom_in"); - auto pose_res = getInput("transform_pose"); + auto ee_pose_res = getInput("ee_pose_at_grip"); + auto controller_pose_res = getInput("controller_pose_at_grip"); + auto child_frame_res = getInput("destination_child_frame"); - if (!odom_in_res && !pose_res) + if (!odom_in_res || !ee_pose_res || !controller_pose_res || !child_frame_res) { shared_resources_->logger->publishFailureMessage( - name(), "Missing both required inputs: odom_in or transform_pose! "); + name(), "Missing required input(s)!"); return BT::NodeStatus::FAILURE; } + const auto& odom_in = odom_in_res.value(); + const auto& ee_pose_at_grip = ee_pose_res.value(); + const auto& controller_pose_at_grip = controller_pose_res.value(); + const auto& child_frame_id = child_frame_res.value(); - if (!odom_in_res || !pose_res) - { - if (!odom_in_res){ - shared_resources_->logger->publishFailureMessage( - name(), "Missing required input: odom_in!"); - return BT::NodeStatus::FAILURE; - } - if (!pose_res){ - shared_resources_->logger->publishFailureMessage( - name(), "Missing required input: transform_pose!"); - return BT::NodeStatus::FAILURE; - } - } + // --- Get poses at grip time (both in world frame) ------------------- + tf2::Transform T_world_controller_grip; + tf2::fromMsg(controller_pose_at_grip.pose, T_world_controller_grip); - const auto& odom_in = odom_in_res.value(); - const auto& transform_pose = pose_res.value(); - - // --- Convert PoseStamped → TransformStamped ------------------------ - geometry_msgs::msg::TransformStamped transform; - transform.header = transform_pose.header; - // shared_resources_->logger->publishInfoMessage( - // name(), - // fmt::format( - // "Transform header: frame_id='{}', stamp={}.{}", - // transform_pose.header.frame_id, - // transform_pose.header.stamp.sec, - // transform_pose.header.stamp.nanosec)); - - transform.child_frame_id = odom_in.header.frame_id; - - // shared_resources_->logger->publishInfoMessage( - // name(), - // fmt::format( - // "Input odometry frame_id: '{}'", - // odom_in.header.frame_id)); - - transform.transform.translation.x = transform_pose.pose.position.x; - transform.transform.translation.y = transform_pose.pose.position.y; - transform.transform.translation.z = transform_pose.pose.position.z; - transform.transform.rotation = transform_pose.pose.orientation; - - // --- Transform pose ------------------------------------------------ - geometry_msgs::msg::PoseStamped pose_in; - pose_in.header = odom_in.header; - pose_in.pose = odom_in.pose.pose; - - geometry_msgs::msg::PoseStamped pose_out; - tf2::doTransform(pose_in, pose_out, transform); - - // --- Transform twist (rotation only) ------------------------------- - tf2::Quaternion q; - tf2::fromMsg(transform.transform.rotation, q); - tf2::Matrix3x3 R(q); + tf2::Transform T_world_ee_grip; + tf2::fromMsg(ee_pose_at_grip.pose, T_world_ee_grip); + + // --- Compute the fixed offset from controller to EE ----------------- + // T_controller_ee = T_world_controller^(-1) * T_world_ee + tf2::Transform T_controller_ee = T_world_controller_grip.inverse() * T_world_ee_grip; + + // --- Get current controller pose from odometry ---------------------- + tf2::Transform T_world_controller_now; + tf2::fromMsg(odom_in.pose.pose, T_world_controller_now); + + // --- Apply offset to get current EE pose ---------------------------- + tf2::Transform T_world_ee_now = T_world_controller_now * T_controller_ee; + + // --- Assemble output odometry --------------------------------------- + nav_msgs::msg::Odometry odom_out; + odom_out.header.stamp = odom_in.header.stamp; + odom_out.header.frame_id = ee_pose_at_grip.header.frame_id; // world + odom_out.child_frame_id = child_frame_id; + + geometry_msgs::msg::Pose ee_pose_msg; + ee_pose_msg.position.x = T_world_ee_now.getOrigin().x(); + ee_pose_msg.position.y = T_world_ee_now.getOrigin().y(); + ee_pose_msg.position.z = T_world_ee_now.getOrigin().z(); + ee_pose_msg.orientation = tf2::toMsg(T_world_ee_now.getRotation()); + + odom_out.pose.pose = ee_pose_msg; + odom_out.pose.covariance = odom_in.pose.covariance; + + // --- Rotate twist into EE frame ------------------------------------- + tf2::Matrix3x3 R(T_controller_ee.getRotation()); tf2::Vector3 lin_in( odom_in.twist.twist.linear.x, @@ -119,59 +113,18 @@ BT::NodeStatus TransformOdomWithPose::tick() tf2::Vector3 lin_out = R * lin_in; tf2::Vector3 ang_out = R * ang_in; - // --- Assemble output odometry -------------------------------------- - nav_msgs::msg::Odometry odom_out; - odom_out.header.stamp = odom_in.header.stamp; - odom_out.header.frame_id = transform.header.frame_id; - odom_out.child_frame_id = odom_in.child_frame_id; - - odom_out.pose.pose = pose_out.pose; - - odom_out.pose.covariance = odom_in.pose.covariance; - - odom_out.twist.twist.linear.x = lin_out.x(); - odom_out.twist.twist.linear.y = lin_out.y(); - odom_out.twist.twist.linear.z = lin_out.z(); - + odom_out.twist.twist.linear.x = lin_out.x(); + odom_out.twist.twist.linear.y = lin_out.y(); + odom_out.twist.twist.linear.z = lin_out.z(); odom_out.twist.twist.angular.x = ang_out.x(); odom_out.twist.twist.angular.y = ang_out.y(); odom_out.twist.twist.angular.z = ang_out.z(); - odom_out.twist.covariance = odom_in.twist.covariance; - // shared_resources_->logger->publishInfoMessage( - // name(), - // fmt::format("Odom out frame_id: '{}'", odom_out.header.frame_id)); - - // shared_resources_->logger->publishInfoMessage( - // name(), - // fmt::format("Odom out child_frame_id: '{}'", odom_out.child_frame_id)); - - // shared_resources_->logger->publishInfoMessage( - // name(), - // fmt::format( - // "Odom out stamp: {}.{}", - // odom_out.header.stamp.sec, - // odom_out.header.stamp.nanosec)); - - // shared_resources_->logger->publishInfoMessage( - // name(), - // fmt::format( - // "Odom out pose: position=({:.3f}, {:.3f}, {:.3f}), " - // "orientation=({:.3f}, {:.3f}, {:.3f}, {:.3f})", - // odom_out.pose.pose.position.x, - // odom_out.pose.pose.position.y, - // odom_out.pose.pose.position.z, - // odom_out.pose.pose.orientation.x, - // odom_out.pose.pose.orientation.y, - // odom_out.pose.pose.orientation.z, - // odom_out.pose.pose.orientation.w)); - - // --- Output -------------------------------------------------------- setOutput("odom_out", odom_out); shared_resources_->logger->publishInfoMessage( - name(), "Successfully transformed odometry."); + name(), "Successfully transformed odometry using pose transform."); return BT::NodeStatus::SUCCESS; } From 259be8fb2dc4bbc8aa7072d86a8e821f29e3c520 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 31 Dec 2025 12:16:20 -0700 Subject: [PATCH 10/27] tripped the rotation from the EE transform + added track moving frame but tracking not working Former-commit-id: 5ad89af5cfa6a3aa8e5553ac0e6e6eb166b671fd --- .../CMakeLists.txt | 46 ++++++++++ .../behavior_plugin.yaml | 4 + ...ose_stamped_to_odom_plugin_description.xml | 7 ++ .../convert_pose_stamped_to_odom.hpp | 26 ++++++ src/convert_pose_stamped_to_odom/package.xml | 30 +++++++ .../src/convert_pose_stamped_to_odom.cpp | 83 +++++++++++++++++++ .../src/register_behaviors.cpp | 24 ++++++ .../test/CMakeLists.txt | 5 ++ .../test/test_behavior_plugins.cpp | 37 +++++++++ .../objectives/debug_transformation.xml | 2 +- src/lab_sim/objectives/teleop_with_meta.xml | 77 ++++++++--------- .../objectives/transformquestframe.xml | 27 ++++-- 12 files changed, 321 insertions(+), 47 deletions(-) create mode 100644 src/convert_pose_stamped_to_odom/CMakeLists.txt create mode 100644 src/convert_pose_stamped_to_odom/behavior_plugin.yaml create mode 100644 src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml create mode 100644 src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp create mode 100644 src/convert_pose_stamped_to_odom/package.xml create mode 100644 src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp create mode 100644 src/convert_pose_stamped_to_odom/src/register_behaviors.cpp create mode 100644 src/convert_pose_stamped_to_odom/test/CMakeLists.txt create mode 100644 src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp diff --git a/src/convert_pose_stamped_to_odom/CMakeLists.txt b/src/convert_pose_stamped_to_odom/CMakeLists.txt new file mode 100644 index 000000000..878528373 --- /dev/null +++ b/src/convert_pose_stamped_to_odom/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(convert_pose_stamped_to_odom CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + convert_pose_stamped_to_odom + SHARED + src/convert_pose_stamped_to_odom.cpp + src/register_behaviors.cpp) +target_include_directories( + convert_pose_stamped_to_odom + PUBLIC $ + $) +ament_target_dependencies(convert_pose_stamped_to_odom + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS convert_pose_stamped_to_odom + EXPORT convert_pose_stamped_to_odomTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(convert_pose_stamped_to_odom) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface convert_pose_stamped_to_odom_plugin_description.xml) + +ament_export_targets(convert_pose_stamped_to_odomTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/convert_pose_stamped_to_odom/behavior_plugin.yaml b/src/convert_pose_stamped_to_odom/behavior_plugin.yaml new file mode 100644 index 000000000..54b69a8d1 --- /dev/null +++ b/src/convert_pose_stamped_to_odom/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + convert_pose_stamped_to_odom: + - "convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader" diff --git a/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml b/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml new file mode 100644 index 000000000..3756752d9 --- /dev/null +++ b/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp b/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp new file mode 100644 index 000000000..8e7272a0a --- /dev/null +++ b/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +#include +#include // BT::PortsList, BT::KeyValueVector + +namespace convert_pose_stamped_to_odom +{ +/** + * @brief TODO(...) + */ +class ConvertPoseStampedToOdom : public BT::SyncActionNode +{ +public: + ConvertPoseStampedToOdom(const std::string& name, + const BT::NodeConfiguration& config); + + static BT::PortsList providedPorts(); + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; +}; + +} // namespace convert_pose_stamped_to_odom \ No newline at end of file diff --git a/src/convert_pose_stamped_to_odom/package.xml b/src/convert_pose_stamped_to_odom/package.xml new file mode 100644 index 000000000..168c135b4 --- /dev/null +++ b/src/convert_pose_stamped_to_odom/package.xml @@ -0,0 +1,30 @@ + + + convert_pose_stamped_to_odom + 0.0.0 + This converts a pose stamped message to an odometry message (leaves the twist part 0). + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_studio_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp b/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp new file mode 100644 index 000000000..0ed9b397b --- /dev/null +++ b/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp @@ -0,0 +1,83 @@ +#include + +#include "spdlog/spdlog.h" +#include + +#include +#include + +namespace +{ + constexpr auto kPortIDPoseStamped = "pose_stamped"; + constexpr auto kPortIDOdometry = "odom"; + constexpr auto kPortIDChildFrame = "child_frame"; +} // namespace + +namespace convert_pose_stamped_to_odom +{ +ConvertPoseStampedToOdom::ConvertPoseStampedToOdom(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) +{ +} + +BT::PortsList ConvertPoseStampedToOdom::providedPorts() +{ + return { + BT::InputPort( + kPortIDPoseStamped, "{pose_stamped}", + "The PoseStamped message to convert."), + + BT::InputPort( + kPortIDChildFrame, "child_frame", + "Optional child_frame_id for the Odometry message."), + + BT::OutputPort( + kPortIDOdometry, "{odom}", + "The converted Odometry message") + }; +} + +BT::KeyValueVector ConvertPoseStampedToOdom::metadata() +{ + return { { "description", "Converts a geometry_msgs::msg::PoseStamped message into a " + "nav_msgs::msg::Odometry message, leaving the Twist field 0." }, + { "subcategory", "Conversions" } }; +} + +BT::NodeStatus ConvertPoseStampedToOdom::tick() +{ + // Required input + const auto ports = moveit_studio::behaviors::getRequiredInputs( + getInput(kPortIDPoseStamped)); + + if (!ports.has_value()) + { + spdlog::warn("Failed to get PoseStamped input: {}", ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& pose_stamped = std::get<0>(ports.value()); + + nav_msgs::msg::Odometry odom_out; + odom_out.header = pose_stamped.header; + odom_out.pose.pose = pose_stamped.pose; + + // Optional child_frame_id + if (auto child_frame = getInput(kPortIDChildFrame)) + { + odom_out.child_frame_id = child_frame.value(); + } + else + { + // Optional fallback — perfectly valid + odom_out.child_frame_id = ""; + } + + // Twist left zero-initialized intentionally + // odom_out.twist.twist.{linear,angular} == 0 + + setOutput(kPortIDOdometry, odom_out); + return BT::NodeStatus::SUCCESS; +} + +} // namespace convert_pose_stamped_to_odom diff --git a/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp b/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp new file mode 100644 index 000000000..f619780dc --- /dev/null +++ b/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace convert_pose_stamped_to_odom +{ +class ConvertPoseStampedToOdomBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "ConvertPoseStampedToOdom"); + + } +}; +} // namespace convert_pose_stamped_to_odom + +PLUGINLIB_EXPORT_CLASS(convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/convert_pose_stamped_to_odom/test/CMakeLists.txt b/src/convert_pose_stamped_to_odom/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/convert_pose_stamped_to_odom/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp b/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..0b155e618 --- /dev/null +++ b/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ConvertPoseStampedToOdom", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/lab_sim/objectives/debug_transformation.xml b/src/lab_sim/objectives/debug_transformation.xml index e21a25e3f..6ec85ed9d 100644 --- a/src/lab_sim/objectives/debug_transformation.xml +++ b/src/lab_sim/objectives/debug_transformation.xml @@ -4,7 +4,7 @@ - + diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index f55b92b2d..39b494cd5 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -5,21 +5,10 @@ - - - - - - - - - - - - - + + @@ -31,38 +20,50 @@ - - - + + + + + + + + + - - - - - - - - - - + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/transformquestframe.xml b/src/lab_sim/objectives/transformquestframe.xml index 57dc25c60..8c4408359 100644 --- a/src/lab_sim/objectives/transformquestframe.xml +++ b/src/lab_sim/objectives/transformquestframe.xml @@ -4,21 +4,32 @@ - - + + - - - - + + + + + + + + + + + + + + + - - + + From 64b6b612c6f3c3f88fae7fedc9f865ca3b6673bd Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 31 Dec 2025 16:42:03 -0700 Subject: [PATCH 11/27] I think it's working but the arm droops if u let go of the clutch / the rotation of the EE is not being handled Former-commit-id: af403d7eca4c3827af3341fbd28a0210fded9e5e --- .../objectives/automask_from_camera.xml | 10 +- src/lab_sim/objectives/teleop_with_meta.xml | 108 ++++++++++++------ 2 files changed, 74 insertions(+), 44 deletions(-) diff --git a/src/factory_sim/objectives/automask_from_camera.xml b/src/factory_sim/objectives/automask_from_camera.xml index 3737ae62c..2fbcf7098 100644 --- a/src/factory_sim/objectives/automask_from_camera.xml +++ b/src/factory_sim/objectives/automask_from_camera.xml @@ -1,4 +1,4 @@ - + - - - + + + diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 39b494cd5..20db9cef6 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -5,69 +5,101 @@ - + + - - - + + - + - + - - - + + + + + + + + + - + - + - + - + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + From 04cf0d0b08f94f58231f48f66886a2ac1cf11d93 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 31 Dec 2025 17:06:34 -0700 Subject: [PATCH 12/27] committing before trying to apply relative transformation Former-commit-id: 2b3c0e7ac1a76ec4b0fee40fa474960629fa39c0 --- src/lab_sim/objectives/teleop_with_meta.xml | 33 ++++++++++++++++----- 1 file changed, 26 insertions(+), 7 deletions(-) diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 20db9cef6..457f73965 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -70,23 +70,42 @@ - - + + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + From 8c99e313e6bfc18b6cd5aa54778ced0c2d6bd2fe Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Fri, 2 Jan 2026 10:40:37 -0700 Subject: [PATCH 13/27] seems to only track safe poses. Inconsistent FP tracking behavior. Need to mess more with rotation, and gripping not addressed yet. Former-commit-id: f4d254434344392843b57895063b7f5fb3b41d8e --- src/lab_sim/objectives/teleop_with_meta.xml | 90 +++++++++++---------- 1 file changed, 48 insertions(+), 42 deletions(-) diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 457f73965..1efaff0b3 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -28,23 +28,20 @@ - + - - - - - + + - - - - - - + + + + + + @@ -56,31 +53,26 @@ - - - - - - - - - - - - - - - - - + + + + + + + + + + - - + + + + - + @@ -89,34 +81,48 @@ - + + + - + + - + - - - + + + + + + + + + + + + + - + + + - - - + + From 2c9a92a21c3122c49644c67ea69d9da044b14a49 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Fri, 2 Jan 2026 11:53:56 -0700 Subject: [PATCH 14/27] committing before refactoring Former-commit-id: b2b44dfd32be9f37fd30b7c84fb69451827651d7 --- src/lab_sim/objectives/teleop_with_meta.xml | 66 ++++++++++----------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 1efaff0b3..85c8f9bcd 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -5,26 +5,26 @@ - + - - + + - + - - - + + + @@ -44,9 +44,14 @@ - + - + + + + + + @@ -91,39 +96,34 @@ - - - + + - - - - - - - - - - + + + - + + - - - - - - - - - - + + + + + + + + + + + + From 0181fcfe39ae41fd2374f5b094ff3163713a6bef Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Fri, 2 Jan 2026 15:59:36 -0700 Subject: [PATCH 15/27] at least the tracking / realignment works Former-commit-id: cd866ede6e75095fcd6a277bdf7ac6627ac890f8 --- .../objectives/old_teleop_with_meta.xml | 143 ++++++++++++++++++ .../republish_pose_stamped_in_world_frame.xml | 22 +++ .../objectives/rising_edge_detection.xml | 41 +++++ src/lab_sim/objectives/teleop_with_meta.xml | 120 +++++++-------- ...track_moving_frame_using_odom_instance.xml | 44 ++++++ .../objectives/transformquestframe.xml | 3 +- 6 files changed, 307 insertions(+), 66 deletions(-) create mode 100644 src/lab_sim/objectives/old_teleop_with_meta.xml create mode 100644 src/lab_sim/objectives/republish_pose_stamped_in_world_frame.xml create mode 100644 src/lab_sim/objectives/rising_edge_detection.xml create mode 100644 src/lab_sim/objectives/track_moving_frame_using_odom_instance.xml diff --git a/src/lab_sim/objectives/old_teleop_with_meta.xml b/src/lab_sim/objectives/old_teleop_with_meta.xml new file mode 100644 index 000000000..fb8f8e6e5 --- /dev/null +++ b/src/lab_sim/objectives/old_teleop_with_meta.xml @@ -0,0 +1,143 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/republish_pose_stamped_in_world_frame.xml b/src/lab_sim/objectives/republish_pose_stamped_in_world_frame.xml new file mode 100644 index 000000000..ee0ceb147 --- /dev/null +++ b/src/lab_sim/objectives/republish_pose_stamped_in_world_frame.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/rising_edge_detection.xml b/src/lab_sim/objectives/rising_edge_detection.xml new file mode 100644 index 000000000..7651bc7c2 --- /dev/null +++ b/src/lab_sim/objectives/rising_edge_detection.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index 85c8f9bcd..af8f50f17 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -5,54 +5,65 @@ - - - + + + + + - - + + + + + + + + - + - + + + - + + + - - + + - - - - + + - + - + - + - + + + + + + + - + - - - - - - - + + @@ -71,64 +82,43 @@ - - - - - + - - - + - + + + - - - - - + + - - - - - - - - - - - - - - - - - - - - - + - - - + + + + + + + + + + - diff --git a/src/lab_sim/objectives/track_moving_frame_using_odom_instance.xml b/src/lab_sim/objectives/track_moving_frame_using_odom_instance.xml new file mode 100644 index 000000000..0d36ebe3d --- /dev/null +++ b/src/lab_sim/objectives/track_moving_frame_using_odom_instance.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/transformquestframe.xml b/src/lab_sim/objectives/transformquestframe.xml index 8c4408359..0b3d29c55 100644 --- a/src/lab_sim/objectives/transformquestframe.xml +++ b/src/lab_sim/objectives/transformquestframe.xml @@ -23,13 +23,14 @@ - + + From 698f2605417da6a9cb877ed16402b079b068ceac Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Fri, 2 Jan 2026 17:20:17 -0700 Subject: [PATCH 16/27] this one tracks for like the first 2 secs then gives up Former-commit-id: d1b4ffff3464692be66717e3815e41ce40240b2a --- .../objectives/test_track_moving_frame.xml | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 src/lab_sim/objectives/test_track_moving_frame.xml diff --git a/src/lab_sim/objectives/test_track_moving_frame.xml b/src/lab_sim/objectives/test_track_moving_frame.xml new file mode 100644 index 000000000..a1effbf31 --- /dev/null +++ b/src/lab_sim/objectives/test_track_moving_frame.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + From 058c15cb2e120a427597c1c3ef07e3eb9c9e3039 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Mon, 5 Jan 2026 08:41:00 -0700 Subject: [PATCH 17/27] added rising edge detection and new teleop with meta Former-commit-id: db4a3ae7df5f73871059f101824a2c5f1b21bdde --- .../objectives/rising_edge_detection.xml | 2 +- src/lab_sim/objectives/teleop_with_meta.xml | 43 ++++++++----------- 2 files changed, 20 insertions(+), 25 deletions(-) diff --git a/src/lab_sim/objectives/rising_edge_detection.xml b/src/lab_sim/objectives/rising_edge_detection.xml index 7651bc7c2..721bffdc8 100644 --- a/src/lab_sim/objectives/rising_edge_detection.xml +++ b/src/lab_sim/objectives/rising_edge_detection.xml @@ -8,7 +8,7 @@ - + diff --git a/src/lab_sim/objectives/teleop_with_meta.xml b/src/lab_sim/objectives/teleop_with_meta.xml index af8f50f17..d2156a674 100644 --- a/src/lab_sim/objectives/teleop_with_meta.xml +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -3,11 +3,12 @@ + - + @@ -15,10 +16,7 @@ - - - - + @@ -51,12 +49,12 @@ - - + + - - + + @@ -88,36 +86,33 @@ + + - - - - - - - - - - - - - + + + - - + + + + + + + From fed9b1571d59f40fced77d0af7ae9119135c57fa Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Sun, 25 Jan 2026 21:49:10 -0700 Subject: [PATCH 18/27] Add objective that publishes what seems to be the correct gripper target pose from the controller odom info --- .../control/picknik_ur.ros2_control.yaml | 32 --- src/lab_sim/config/moveit/ompl_planning.yaml | 32 +++ src/lab_sim/config/moveit/stomp_planning.yaml | 21 ++ .../teleop_with_meta_via_pose_v4.xml | 188 ++++++++++++++++++ 4 files changed, 241 insertions(+), 32 deletions(-) create mode 100644 src/lab_sim/config/moveit/ompl_planning.yaml create mode 100644 src/lab_sim/config/moveit/stomp_planning.yaml create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v4.xml diff --git a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml index 022396adb..64b41849d 100644 --- a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml @@ -13,8 +13,6 @@ controller_manager: type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController velocity_force_controller: type: velocity_force_controller/VelocityForceController - joint_velocity_controller: - type: joint_velocity_controller/JointVelocityController joint_state_broadcaster: @@ -216,33 +214,3 @@ velocity_force_controller: # command_joints: [] # The type of command interface to use. Supported values are "position" and "velocity". command_interfaces: ["position"] - -joint_velocity_controller: - ros__parameters: - # Joint group to control. - planning_group_name: manipulator - # Maximum joint-space velocities. - max_joint_velocity: - - 1.0 - - 1.0 - - 1.0 - - 1.0 - - 1.0 - - 1.0 - - 1.0 - # Maximum joint-space accelerations. - max_joint_acceleration: - - 2.0 - - 2.0 - - 2.0 - - 2.0 - - 2.0 - - 2.0 - - 2.0 - command_interfaces: ["position"] - # Padding (in radians) to add to joint position limits as a safety margin. - joint_limit_position_tolerance: 0.02 - # Timeout in seconds after which the controller will stop motion if no new commands are received. - command_timeout: 0.2 - # Rate in Hz at which the controller will publish the state. - state_publish_rate: 20 diff --git a/src/lab_sim/config/moveit/ompl_planning.yaml b/src/lab_sim/config/moveit/ompl_planning.yaml new file mode 100644 index 000000000..186158d58 --- /dev/null +++ b/src/lab_sim/config/moveit/ompl_planning.yaml @@ -0,0 +1,32 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + +planner_configs: + APSConfigDefault: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 32 # Number of hybrid paths generated per iteration + num_planners: 16 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() +manipulator: + default_planner_config: RRTConnectkConfigDefault + planner_configs: + - APSConfigDefault + - RRTConnectkConfigDefault + enforce_constrained_state_space: true + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 diff --git a/src/lab_sim/config/moveit/stomp_planning.yaml b/src/lab_sim/config/moveit/stomp_planning.yaml new file mode 100644 index 000000000..7447a8ff0 --- /dev/null +++ b/src/lab_sim/config/moveit/stomp_planning.yaml @@ -0,0 +1,21 @@ +planning_plugins: + - stomp_moveit/StompPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + +stomp_moveit: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + exponentiated_cost_sensitivity: 0.8 + control_cost_weight: 0.1 + delta_t: 0.1 diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v4.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v4.xml new file mode 100644 index 000000000..b868d44a5 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v4.xml @@ -0,0 +1,188 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 832ef32095c520232c08d02ef3119b729a414b5f Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Mon, 26 Jan 2026 12:47:11 -0700 Subject: [PATCH 19/27] Add start of pubilsh odom objective --- .../teleop_with_meta_via_pose_v5.xml | 213 ++++++++++++++++++ 1 file changed, 213 insertions(+) create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml new file mode 100644 index 000000000..0fe79ea88 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml @@ -0,0 +1,213 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8ab43a0e074f8787396ea45cb5896e51499999ec Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Tue, 27 Jan 2026 12:42:15 -0700 Subject: [PATCH 20/27] v5 is successfully moving the arm in 8.10.1 (with some changes to work with v8 and not v9). Need to also run in shell: ros2 run tf2_ros static_transform_publisher 0.2 0 0 0 0 0 1 world quest and ros2 bag play quest_telop_0.db3 --start-offset 15 --- .../teleop_with_meta_via_pose_v5.xml | 252 +++++++++--------- 1 file changed, 133 insertions(+), 119 deletions(-) diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml index 0fe79ea88..a9e5f56a1 100644 --- a/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml @@ -59,147 +59,161 @@ - - - - - + + + + + + - - - - - - + + - + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + From 531946e340eb17361c5221206581d78730cf8e9f Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Tue, 10 Feb 2026 11:15:00 -0700 Subject: [PATCH 21/27] some trouble with tracking, but implemented the clutch in behavior... about to try and enforce safety --- .gitignore | 1 + .../CMakeLists.txt | 4 +- ...dom_to_pose_stamped_plugin_description.xml | 2 +- .../convert_odom_to_pose_stamped.hpp | 2 +- src/convert_odom_to_pose_stamped/package.xml | 2 +- .../src/convert_odom_to_pose_stamped.cpp | 2 +- .../src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- .../CMakeLists.txt | 4 +- ...ose_stamped_to_odom_plugin_description.xml | 2 +- .../convert_pose_stamped_to_odom.hpp | 2 +- src/convert_pose_stamped_to_odom/package.xml | 2 +- .../src/convert_pose_stamped_to_odom.cpp | 2 +- .../src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- .../CMakeLists.txt | 4 +- ...o_transform_stamped_plugin_description.xml | 2 +- ...vert_pose_stamped_to_transform_stamped.hpp | 8 +- .../package.xml | 2 +- ...vert_pose_stamped_to_transform_stamped.cpp | 8 +- .../src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- .../CMakeLists.txt | 4 +- ...orm_stamped_to_odom_plugin_description.xml | 2 +- .../convert_transform_stamped_to_odom.hpp | 8 +- .../package.xml | 2 +- .../src/convert_transform_stamped_to_odom.cpp | 8 +- .../src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- src/get_bool/CMakeLists.txt | 4 +- src/get_bool/get_bool_plugin_description.xml | 2 +- src/get_bool/include/get_bool/get_bool.hpp | 8 +- src/get_bool/package.xml | 2 +- src/get_bool/src/get_bool.cpp | 14 +-- src/get_bool/src/register_behaviors.cpp | 12 +- src/get_bool/test/test_behavior_plugins.cpp | 8 +- src/get_empty/CMakeLists.txt | 46 +++++++ src/get_empty/behavior_plugin.yaml | 4 + .../get_empty_plugin_description.xml | 7 ++ src/get_empty/include/get_empty/get_empty.hpp | 34 ++++++ src/get_empty/package.xml | 30 +++++ src/get_empty/src/get_empty.cpp | 4 +- src/get_empty/src/register_behaviors.cpp | 27 ++++ src/get_empty/test/CMakeLists.txt | 5 + src/get_empty/test/test_behavior_plugins.cpp | 37 ++++++ src/get_odom_instance/CMakeLists.txt | 4 +- .../get_odom_instance_plugin_description.xml | 2 +- .../get_odom_instance/get_odom_instance.hpp | 8 +- src/get_odom_instance/package.xml | 2 +- .../src/get_odom_instance.cpp | 14 +-- .../src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- .../teleop_with_meta_via_pose_v6.xml | 115 ++++++++++++++++++ src/publish_odom/CMakeLists.txt | 4 +- .../include/publish_odom/publish_odom.hpp | 8 +- src/publish_odom/package.xml | 2 +- .../publish_odom_plugin_description.xml | 2 +- src/publish_odom/src/publish_odom.cpp | 10 +- src/publish_odom/src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- src/publish_pose/CMakeLists.txt | 4 +- .../include/publish_pose/publish_pose.hpp | 8 +- src/publish_pose/package.xml | 2 +- .../publish_pose_plugin_description.xml | 2 +- src/publish_pose/src/publish_pose.cpp | 10 +- src/publish_pose/src/register_behaviors.cpp | 12 +- .../test/test_behavior_plugins.cpp | 8 +- src/transform_odom_with_pose/CMakeLists.txt | 4 +- .../transform_odom_with_pose.hpp | 8 +- src/transform_odom_with_pose/package.xml | 2 +- .../src/register_behaviors.cpp | 12 +- .../src/transform_odom_with_pose.cpp | 4 +- .../test/test_behavior_plugins.cpp | 8 +- ...form_odom_with_pose_plugin_description.xml | 2 +- 74 files changed, 500 insertions(+), 194 deletions(-) create mode 100644 src/get_empty/CMakeLists.txt create mode 100644 src/get_empty/behavior_plugin.yaml create mode 100644 src/get_empty/get_empty_plugin_description.xml create mode 100644 src/get_empty/include/get_empty/get_empty.hpp create mode 100644 src/get_empty/package.xml create mode 100644 src/get_empty/src/register_behaviors.cpp create mode 100644 src/get_empty/test/CMakeLists.txt create mode 100644 src/get_empty/test/test_behavior_plugins.cpp create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml diff --git a/.gitignore b/.gitignore index ab9749485..af0180076 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ build/ install/ log/ .vscode/ +src/ROS-TCP-Endpoint/ diff --git a/src/convert_odom_to_pose_stamped/CMakeLists.txt b/src/convert_odom_to_pose_stamped/CMakeLists.txt index 7fc6cb079..4cbdd8ca4 100644 --- a/src/convert_odom_to_pose_stamped/CMakeLists.txt +++ b/src/convert_odom_to_pose_stamped/CMakeLists.txt @@ -4,7 +4,7 @@ project(convert_odom_to_pose_stamped CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface convert_odom_to_pose_stamped_plugin_description.xml) + moveit_pro_behavior_interface convert_odom_to_pose_stamped_plugin_description.xml) ament_export_targets(convert_odom_to_pose_stampedTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml b/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml index dd57696a8..8d3b8bc08 100644 --- a/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml +++ b/src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp b/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp index 0e6e86e8b..722d71966 100644 --- a/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp +++ b/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include +#include diff --git a/src/convert_odom_to_pose_stamped/package.xml b/src/convert_odom_to_pose_stamped/package.xml index 17aee5deb..b34243cdd 100644 --- a/src/convert_odom_to_pose_stamped/package.xml +++ b/src/convert_odom_to_pose_stamped/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp b/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp index e805e4e88..3543843bb 100644 --- a/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp +++ b/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp @@ -39,7 +39,7 @@ BT::KeyValueVector ConvertOdomToPoseStamped::metadata() BT::NodeStatus ConvertOdomToPoseStamped::tick() { - const auto ports = moveit_studio::behaviors::getRequiredInputs( + const auto ports = moveit_pro::behaviors::getRequiredInputs( getInput(kPortIDOdometry)); if (!ports.has_value()) diff --git a/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp b/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp index e0c0c3c69..4f5750605 100644 --- a/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp +++ b/src/convert_odom_to_pose_stamped/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace convert_odom_to_pose_stamped { -class ConvertOdomToPoseStampedBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class ConvertOdomToPoseStampedBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "ConvertOdomToPoseStamped"); + moveit_pro::behaviors::registerBehavior(factory, "ConvertOdomToPoseStamped"); } }; } // namespace convert_odom_to_pose_stamped PLUGINLIB_EXPORT_CLASS(convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp b/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp index e928c91f2..51a8791be 100644 --- a/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp +++ b/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/convert_pose_stamped_to_odom/CMakeLists.txt b/src/convert_pose_stamped_to_odom/CMakeLists.txt index 878528373..5bb76d75d 100644 --- a/src/convert_pose_stamped_to_odom/CMakeLists.txt +++ b/src/convert_pose_stamped_to_odom/CMakeLists.txt @@ -4,7 +4,7 @@ project(convert_pose_stamped_to_odom CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface convert_pose_stamped_to_odom_plugin_description.xml) + moveit_pro_behavior_interface convert_pose_stamped_to_odom_plugin_description.xml) ament_export_targets(convert_pose_stamped_to_odomTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml b/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml index 3756752d9..f1f5833cd 100644 --- a/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml +++ b/src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp b/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp index 8e7272a0a..6b6453f11 100644 --- a/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp +++ b/src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include // BT::PortsList, BT::KeyValueVector diff --git a/src/convert_pose_stamped_to_odom/package.xml b/src/convert_pose_stamped_to_odom/package.xml index 168c135b4..a496f4a90 100644 --- a/src/convert_pose_stamped_to_odom/package.xml +++ b/src/convert_pose_stamped_to_odom/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp b/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp index 0ed9b397b..d85b81165 100644 --- a/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp +++ b/src/convert_pose_stamped_to_odom/src/convert_pose_stamped_to_odom.cpp @@ -47,7 +47,7 @@ BT::KeyValueVector ConvertPoseStampedToOdom::metadata() BT::NodeStatus ConvertPoseStampedToOdom::tick() { // Required input - const auto ports = moveit_studio::behaviors::getRequiredInputs( + const auto ports = moveit_pro::behaviors::getRequiredInputs( getInput(kPortIDPoseStamped)); if (!ports.has_value()) diff --git a/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp b/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp index f619780dc..73c3a80f5 100644 --- a/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp +++ b/src/convert_pose_stamped_to_odom/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace convert_pose_stamped_to_odom { -class ConvertPoseStampedToOdomBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class ConvertPoseStampedToOdomBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "ConvertPoseStampedToOdom"); + moveit_pro::behaviors::registerBehavior(factory, "ConvertPoseStampedToOdom"); } }; } // namespace convert_pose_stamped_to_odom PLUGINLIB_EXPORT_CLASS(convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp b/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp index 0b155e618..2d13fcbbe 100644 --- a/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp +++ b/src/convert_pose_stamped_to_odom/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt b/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt index fc6ce25d2..7563c1275 100644 --- a/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt +++ b/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt @@ -4,7 +4,7 @@ project(convert_pose_stamped_to_transform_stamped CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface convert_pose_stamped_to_transform_stamped_plugin_description.xml) + moveit_pro_behavior_interface convert_pose_stamped_to_transform_stamped_plugin_description.xml) ament_export_targets(convert_pose_stamped_to_transform_stampedTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml b/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml index 9fd2fdb80..0235c808e 100644 --- a/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml +++ b/src/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp b/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp index a2ff62bb7..d4a092e5c 100644 --- a/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp +++ b/src/convert_pose_stamped_to_transform_stamped/include/convert_pose_stamped_to_transform_stamped/convert_pose_stamped_to_transform_stamped.hpp @@ -1,17 +1,17 @@ #pragma once #include -#include +#include // This header includes the SharedResourcesNode type -#include +#include namespace convert_pose_stamped_to_transform_stamped { /** * @brief Converts a geometry_msgs::msg::PoseStamped message into a geometry_msgs::msg::TransformStamped message. */ -class ConvertPoseStampedToTransformStamped : public moveit_studio::behaviors::SharedResourcesNode +class ConvertPoseStampedToTransformStamped : public moveit_pro::behaviors::SharedResourcesNode { public: /** @@ -21,7 +21,7 @@ class ConvertPoseStampedToTransformStamped : public moveit_studio::behaviors::Sh * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode. * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. */ - ConvertPoseStampedToTransformStamped(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + ConvertPoseStampedToTransformStamped(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** * @brief Implementation of the required providedPorts() function for the convert_pose_stamped_to_transform_stamped Behavior. diff --git a/src/convert_pose_stamped_to_transform_stamped/package.xml b/src/convert_pose_stamped_to_transform_stamped/package.xml index 1a6386d4b..d04e5653c 100644 --- a/src/convert_pose_stamped_to_transform_stamped/package.xml +++ b/src/convert_pose_stamped_to_transform_stamped/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp b/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp index c426cde91..9254e78a5 100644 --- a/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp +++ b/src/convert_pose_stamped_to_transform_stamped/src/convert_pose_stamped_to_transform_stamped.cpp @@ -10,7 +10,7 @@ #include #include "spdlog/spdlog.h" -#include "moveit_studio_behavior_interface/get_required_ports.hpp" +#include "moveit_pro_behavior_interface/get_required_ports.hpp" #include #include @@ -27,8 +27,8 @@ namespace convert_pose_stamped_to_transform_stamped ConvertPoseStampedToTransformStamped::ConvertPoseStampedToTransformStamped( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) { } @@ -52,7 +52,7 @@ BT::KeyValueVector ConvertPoseStampedToTransformStamped::metadata() BT::NodeStatus ConvertPoseStampedToTransformStamped::tick() { - const auto ports = moveit_studio::behaviors::getRequiredInputs( + const auto ports = moveit_pro::behaviors::getRequiredInputs( getInput(kPortIDPoseStamped), getInput(kPortIDChildFrameId)); diff --git a/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp b/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp index f19bb6ef4..efafecec3 100644 --- a/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp +++ b/src/convert_pose_stamped_to_transform_stamped/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace convert_pose_stamped_to_transform_stamped { -class ConvertPoseStampedToTransformStampedBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class ConvertPoseStampedToTransformStampedBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "ConvertPoseStampedToTransformStamped", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "ConvertPoseStampedToTransformStamped", shared_resources); } }; } // namespace convert_pose_stamped_to_transform_stamped PLUGINLIB_EXPORT_CLASS(convert_pose_stamped_to_transform_stamped::ConvertPoseStampedToTransformStampedBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp b/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp index eb954b224..06556f40b 100644 --- a/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp +++ b/src/convert_pose_stamped_to_transform_stamped/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/convert_transform_stamped_to_odom/CMakeLists.txt b/src/convert_transform_stamped_to_odom/CMakeLists.txt index 2d9b8ff72..fe39c9ac9 100644 --- a/src/convert_transform_stamped_to_odom/CMakeLists.txt +++ b/src/convert_transform_stamped_to_odom/CMakeLists.txt @@ -4,7 +4,7 @@ project(convert_transform_stamped_to_odom CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface convert_transform_stamped_to_odom_plugin_description.xml) + moveit_pro_behavior_interface convert_transform_stamped_to_odom_plugin_description.xml) ament_export_targets(convert_transform_stamped_to_odomTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml b/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml index df6d107a9..554d72b30 100644 --- a/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml +++ b/src/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp b/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp index ca884859c..2c9a7112e 100644 --- a/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp +++ b/src/convert_transform_stamped_to_odom/include/convert_transform_stamped_to_odom/convert_transform_stamped_to_odom.hpp @@ -1,17 +1,17 @@ #pragma once #include -#include +#include // This header includes the SharedResourcesNode type -#include +#include namespace convert_transform_stamped_to_odom { /** * @brief Converts a geometry_msgs::msg::TransformStamped message into a nav_msgs::msg::Odometry message. */ -class ConvertTransformStampedToOdom : public moveit_studio::behaviors::SharedResourcesNode +class ConvertTransformStampedToOdom : public moveit_pro::behaviors::SharedResourcesNode { public: /** @@ -21,7 +21,7 @@ class ConvertTransformStampedToOdom : public moveit_studio::behaviors::SharedRes * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode. * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor. */ - ConvertTransformStampedToOdom(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + ConvertTransformStampedToOdom(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** * @brief Implementation of the required providedPorts() function for the convert_transform_stamped_to_odom Behavior. diff --git a/src/convert_transform_stamped_to_odom/package.xml b/src/convert_transform_stamped_to_odom/package.xml index 8d20bec79..f5f0a989e 100644 --- a/src/convert_transform_stamped_to_odom/package.xml +++ b/src/convert_transform_stamped_to_odom/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp b/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp index 52c1d29c1..b480055f7 100644 --- a/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp +++ b/src/convert_transform_stamped_to_odom/src/convert_transform_stamped_to_odom.cpp @@ -10,7 +10,7 @@ #include #include "spdlog/spdlog.h" -#include "moveit_studio_behavior_interface/get_required_ports.hpp" +#include "moveit_pro_behavior_interface/get_required_ports.hpp" #include #include @@ -26,8 +26,8 @@ namespace convert_transform_stamped_to_odom ConvertTransformStampedToOdom::ConvertTransformStampedToOdom( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) { } @@ -50,7 +50,7 @@ BT::KeyValueVector ConvertTransformStampedToOdom::metadata() BT::NodeStatus ConvertTransformStampedToOdom::tick() { - const auto ports = moveit_studio::behaviors::getRequiredInputs( + const auto ports = moveit_pro::behaviors::getRequiredInputs( getInput(kPortIDTransformStamped)); if (!ports.has_value()) diff --git a/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp b/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp index 254fcb376..5905333a8 100644 --- a/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp +++ b/src/convert_transform_stamped_to_odom/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,16 +8,16 @@ namespace convert_transform_stamped_to_odom { -class ConvertTransformStampedToOdomBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class ConvertTransformStampedToOdomBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "ConvertTransformStampedToOdom", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "ConvertTransformStampedToOdom", shared_resources); } }; } // namespace convert_transform_stamped_to_odom PLUGINLIB_EXPORT_CLASS(convert_transform_stamped_to_odom::ConvertTransformStampedToOdomBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); \ No newline at end of file + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); \ No newline at end of file diff --git a/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp b/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp index f971853d7..f52e45cea 100644 --- a/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp +++ b/src/convert_transform_stamped_to_odom/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/get_bool/CMakeLists.txt b/src/get_bool/CMakeLists.txt index 0e33c5604..83c007e68 100644 --- a/src/get_bool/CMakeLists.txt +++ b/src/get_bool/CMakeLists.txt @@ -4,7 +4,7 @@ project(get_bool CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface get_bool_plugin_description.xml) + moveit_pro_behavior_interface get_bool_plugin_description.xml) ament_export_targets(get_boolTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/get_bool/get_bool_plugin_description.xml b/src/get_bool/get_bool_plugin_description.xml index a3029807f..4c49e5ed9 100644 --- a/src/get_bool/get_bool_plugin_description.xml +++ b/src/get_bool/get_bool_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/get_bool/include/get_bool/get_bool.hpp b/src/get_bool/include/get_bool/get_bool.hpp index 58947e3ae..bdbc0c0ad 100644 --- a/src/get_bool/include/get_bool/get_bool.hpp +++ b/src/get_bool/include/get_bool/get_bool.hpp @@ -6,17 +6,17 @@ #include #include -#include +#include // This header includes the SharedResourcesNode type -#include +#include namespace get_bool { /** * @brief Subscribes to a std_msgs::msg::Bool topic and outputs the boolean value to the blackboard. */ -class GetBool : public moveit_studio::behaviors::SharedResourcesNode +class GetBool : public moveit_pro::behaviors::SharedResourcesNode { public: /** @@ -26,7 +26,7 @@ class GetBool : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); + GetBool(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** * @brief Implementation of the required providedPorts() function for the get_bool Behavior. diff --git a/src/get_bool/package.xml b/src/get_bool/package.xml index cbc657e60..dae4e7f85 100644 --- a/src/get_bool/package.xml +++ b/src/get_bool/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/get_bool/src/get_bool.cpp b/src/get_bool/src/get_bool.cpp index aa13659f8..049473ca5 100644 --- a/src/get_bool/src/get_bool.cpp +++ b/src/get_bool/src/get_bool.cpp @@ -8,8 +8,8 @@ #include "fmt/format.h" -#include "moveit_studio_behavior_interface/get_required_ports.hpp" -#include "moveit_studio_behavior_interface/metadata_fields.hpp" +#include "moveit_pro_behavior_interface/get_required_ports.hpp" +#include "moveit_pro_behavior_interface/metadata_fields.hpp" namespace get_bool { @@ -19,8 +19,8 @@ constexpr auto kPortIdBoolTopicName = "bool_topic_name"; constexpr auto kPortIdBoolValue = "subscribed_bool"; GetBool::GetBool(const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) { } @@ -34,14 +34,14 @@ BT::PortsList GetBool::providedPorts() BT::KeyValueVector GetBool::metadata() { - return { { moveit_studio::behaviors::kSubcategoryMetadataKey, "User Created Behaviors" }, - { moveit_studio::behaviors::kDescriptionMetadataKey, + return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "User Created Behaviors" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, "Subscribe to a bool message and store it on the blackboard." } }; } BT::NodeStatus GetBool::onStart() { - const auto ports = moveit_studio::behaviors::getRequiredInputs(getInput(kPortIdBoolTopicName)); + const auto ports = moveit_pro::behaviors::getRequiredInputs(getInput(kPortIdBoolTopicName)); if (!ports.has_value()) { diff --git a/src/get_bool/src/register_behaviors.cpp b/src/get_bool/src/register_behaviors.cpp index 849c1ab3c..e3ca0eda2 100644 --- a/src/get_bool/src/register_behaviors.cpp +++ b/src/get_bool/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace get_bool { -class GetBoolBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class GetBoolBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "GetBool", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "GetBool", shared_resources); } }; } // namespace get_bool PLUGINLIB_EXPORT_CLASS(get_bool::GetBoolBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/get_bool/test/test_behavior_plugins.cpp b/src/get_bool/test/test_behavior_plugins.cpp index aad71d513..f17f5ddea 100644 --- a/src/get_bool/test/test_behavior_plugins.cpp +++ b/src/get_bool/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/get_empty/CMakeLists.txt b/src/get_empty/CMakeLists.txt new file mode 100644 index 000000000..97ca6f40b --- /dev/null +++ b/src/get_empty/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(get_empty CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + get_empty + SHARED + src/get_empty.cpp + src/register_behaviors.cpp) +target_include_directories( + get_empty + PUBLIC $ + $) +ament_target_dependencies(get_empty + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS get_empty + EXPORT get_emptyTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(get_empty) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_pro_behavior_interface get_empty_plugin_description.xml) + +ament_export_targets(get_emptyTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/get_empty/behavior_plugin.yaml b/src/get_empty/behavior_plugin.yaml new file mode 100644 index 000000000..cc66ce11f --- /dev/null +++ b/src/get_empty/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + get_empty: + - "get_empty::GetEmptyBehaviorsLoader" diff --git a/src/get_empty/get_empty_plugin_description.xml b/src/get_empty/get_empty_plugin_description.xml new file mode 100644 index 000000000..e145592c2 --- /dev/null +++ b/src/get_empty/get_empty_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/get_empty/include/get_empty/get_empty.hpp b/src/get_empty/include/get_empty/get_empty.hpp new file mode 100644 index 000000000..ccd52521a --- /dev/null +++ b/src/get_empty/include/get_empty/get_empty.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include "behaviortree_cpp/action_node.h" +#include "moveit_pro_behavior_interface/shared_resources_node.hpp" + +namespace get_empty +{ + +class GetEmpty : public moveit_pro::behaviors::SharedResourcesNode +{ +public: + GetEmpty(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& 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> empty_subscriber_; + std::shared_mutex empty_mutex_; + uint64_t message_count_; +}; + +} // namespace get_empty \ No newline at end of file diff --git a/src/get_empty/package.xml b/src/get_empty/package.xml new file mode 100644 index 000000000..22dccdd9a --- /dev/null +++ b/src/get_empty/package.xml @@ -0,0 +1,30 @@ + + + get_empty + 0.0.0 + Wait for an empty message to be published on a specified ROS topic and copy it to an output data port. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_pro_behavior_interface + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/get_empty/src/get_empty.cpp b/src/get_empty/src/get_empty.cpp index 56b0791af..970b79774 100644 --- a/src/get_empty/src/get_empty.cpp +++ b/src/get_empty/src/get_empty.cpp @@ -7,8 +7,8 @@ namespace get_empty { GetEmpty::GetEmpty( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) , message_count_(0) { } diff --git a/src/get_empty/src/register_behaviors.cpp b/src/get_empty/src/register_behaviors.cpp new file mode 100644 index 000000000..35a849ae5 --- /dev/null +++ b/src/get_empty/src/register_behaviors.cpp @@ -0,0 +1,27 @@ +#include +#include +#include +#include + +#include + +#include + +// Explicitly instantiate the template before registration +template class moveit_pro::behaviors::GetMessageFromTopicBehaviorBase; + +namespace get_empty +{ +class GetEmptyBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "GetEmpty", shared_resources); + } +}; +} // namespace get_empty + +PLUGINLIB_EXPORT_CLASS(get_empty::GetEmptyBehaviorsLoader, + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); \ No newline at end of file diff --git a/src/get_empty/test/CMakeLists.txt b/src/get_empty/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/get_empty/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/get_empty/test/test_behavior_plugins.cpp b/src/get_empty/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..cff5178ff --- /dev/null +++ b/src/get_empty/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("get_empty::GetEmptyBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "GetEmpty", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/get_odom_instance/CMakeLists.txt b/src/get_odom_instance/CMakeLists.txt index 5ab88eaf3..54a80d667 100644 --- a/src/get_odom_instance/CMakeLists.txt +++ b/src/get_odom_instance/CMakeLists.txt @@ -4,7 +4,7 @@ project(get_odom_instance CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface get_odom_instance_plugin_description.xml) + moveit_pro_behavior_interface get_odom_instance_plugin_description.xml) ament_export_targets(get_odom_instanceTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/get_odom_instance/get_odom_instance_plugin_description.xml b/src/get_odom_instance/get_odom_instance_plugin_description.xml index 3e2bdb689..d9c130408 100644 --- a/src/get_odom_instance/get_odom_instance_plugin_description.xml +++ b/src/get_odom_instance/get_odom_instance_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp index f888ab2e9..651908e07 100644 --- a/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp +++ b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp @@ -7,8 +7,8 @@ #include #include -#include -#include +#include +#include namespace get_odom_instance { @@ -18,12 +18,12 @@ namespace get_odom_instance * The behavior exits successfully after the first message is received. */ class GetOdomInstance - : public moveit_studio::behaviors::SharedResourcesNode + : public moveit_pro::behaviors::SharedResourcesNode { public: GetOdomInstance(const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources); + const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); static BT::KeyValueVector metadata(); diff --git a/src/get_odom_instance/package.xml b/src/get_odom_instance/package.xml index 6809b7847..a1cdea8e1 100644 --- a/src/get_odom_instance/package.xml +++ b/src/get_odom_instance/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/get_odom_instance/src/get_odom_instance.cpp b/src/get_odom_instance/src/get_odom_instance.cpp index 84669e3f8..8c9ae01c3 100644 --- a/src/get_odom_instance/src/get_odom_instance.cpp +++ b/src/get_odom_instance/src/get_odom_instance.cpp @@ -4,8 +4,8 @@ #include "fmt/format.h" -#include "moveit_studio_behavior_interface/get_required_ports.hpp" -#include "moveit_studio_behavior_interface/metadata_fields.hpp" +#include "moveit_pro_behavior_interface/get_required_ports.hpp" +#include "moveit_pro_behavior_interface/metadata_fields.hpp" namespace get_odom_instance { @@ -16,8 +16,8 @@ constexpr auto kPortIdOdomValue = "subscribed_odom_instance"; GetOdomInstance::GetOdomInstance( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode( + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode( name, config, shared_resources) { odom_subscriber_ = @@ -48,9 +48,9 @@ BT::PortsList GetOdomInstance::providedPorts() BT::KeyValueVector GetOdomInstance::metadata() { return { - { moveit_studio::behaviors::kSubcategoryMetadataKey, + { moveit_pro::behaviors::kSubcategoryMetadataKey, "User Created Behaviors" }, - { moveit_studio::behaviors::kDescriptionMetadataKey, + { moveit_pro::behaviors::kDescriptionMetadataKey, "Subscribe to a single odometry message and store it on the blackboard." } }; } @@ -58,7 +58,7 @@ BT::KeyValueVector GetOdomInstance::metadata() BT::NodeStatus GetOdomInstance::onStart() { const auto ports = - moveit_studio::behaviors::getRequiredInputs( + moveit_pro::behaviors::getRequiredInputs( getInput(kPortIdOdomTopicName)); if (!ports) diff --git a/src/get_odom_instance/src/register_behaviors.cpp b/src/get_odom_instance/src/register_behaviors.cpp index 9efe7bf85..5a41105bc 100644 --- a/src/get_odom_instance/src/register_behaviors.cpp +++ b/src/get_odom_instance/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace get_odom_instance { -class GetOdomInstanceBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class GetOdomInstanceBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "GetOdomInstance", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "GetOdomInstance", shared_resources); } }; } // namespace get_odom_instance PLUGINLIB_EXPORT_CLASS(get_odom_instance::GetOdomInstanceBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/get_odom_instance/test/test_behavior_plugins.cpp b/src/get_odom_instance/test/test_behavior_plugins.cpp index 6b57e8026..5a639c88a 100644 --- a/src/get_odom_instance/test/test_behavior_plugins.cpp +++ b/src/get_odom_instance/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml new file mode 100644 index 000000000..4a4273707 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/publish_odom/CMakeLists.txt b/src/publish_odom/CMakeLists.txt index 0f4c2194c..fc99d245c 100644 --- a/src/publish_odom/CMakeLists.txt +++ b/src/publish_odom/CMakeLists.txt @@ -4,7 +4,7 @@ project(publish_odom CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface publish_odom_plugin_description.xml) + moveit_pro_behavior_interface publish_odom_plugin_description.xml) ament_export_targets(publish_odomTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/publish_odom/include/publish_odom/publish_odom.hpp b/src/publish_odom/include/publish_odom/publish_odom.hpp index cee24630f..c7e21dfb3 100644 --- a/src/publish_odom/include/publish_odom/publish_odom.hpp +++ b/src/publish_odom/include/publish_odom/publish_odom.hpp @@ -1,8 +1,8 @@ #pragma once #include -#include -#include +#include +#include #include #include @@ -21,7 +21,7 @@ namespace publish_odom * This Behavior is synchronous and must return quickly. */ class PublishOdom - : public moveit_studio::behaviors::SharedResourcesNode + : public moveit_pro::behaviors::SharedResourcesNode { public: /** @@ -34,7 +34,7 @@ class PublishOdom PublishOdom( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources); + const std::shared_ptr& shared_resources); /** * @brief Define the input ports for the PublishOdom Behavior. diff --git a/src/publish_odom/package.xml b/src/publish_odom/package.xml index 317e4e28f..2a5514833 100644 --- a/src/publish_odom/package.xml +++ b/src/publish_odom/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/publish_odom/publish_odom_plugin_description.xml b/src/publish_odom/publish_odom_plugin_description.xml index 0555b55e2..b8483fecd 100644 --- a/src/publish_odom/publish_odom_plugin_description.xml +++ b/src/publish_odom/publish_odom_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/publish_odom/src/publish_odom.cpp b/src/publish_odom/src/publish_odom.cpp index 586ab5409..e701ddc00 100644 --- a/src/publish_odom/src/publish_odom.cpp +++ b/src/publish_odom/src/publish_odom.cpp @@ -1,6 +1,6 @@ #include -#include +#include #include @@ -10,8 +10,8 @@ namespace publish_odom PublishOdom::PublishOdom( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode( + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode( name, config, shared_resources) { } @@ -37,8 +37,8 @@ BT::PortsList PublishOdom::providedPorts() BT::KeyValueVector PublishOdom::metadata() { return { - { moveit_studio::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, - { moveit_studio::behaviors::kDescriptionMetadataKey, + { moveit_pro::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, "Publish a nav_msgs::msg::Odometry message to a ROS 2 topic." } }; } diff --git a/src/publish_odom/src/register_behaviors.cpp b/src/publish_odom/src/register_behaviors.cpp index 42d82245f..848b6ecdb 100644 --- a/src/publish_odom/src/register_behaviors.cpp +++ b/src/publish_odom/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace publish_odom { -class PublishOdomBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class PublishOdomBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "PublishOdom", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "PublishOdom", shared_resources); } }; } // namespace publish_odom PLUGINLIB_EXPORT_CLASS(publish_odom::PublishOdomBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/publish_odom/test/test_behavior_plugins.cpp b/src/publish_odom/test/test_behavior_plugins.cpp index bf17bbb3d..0935ad5fe 100644 --- a/src/publish_odom/test/test_behavior_plugins.cpp +++ b/src/publish_odom/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/publish_pose/CMakeLists.txt b/src/publish_pose/CMakeLists.txt index e263261d4..388fd8fc9 100644 --- a/src/publish_pose/CMakeLists.txt +++ b/src/publish_pose/CMakeLists.txt @@ -4,7 +4,7 @@ project(publish_pose CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface publish_pose_plugin_description.xml) + moveit_pro_behavior_interface publish_pose_plugin_description.xml) ament_export_targets(publish_poseTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/publish_pose/include/publish_pose/publish_pose.hpp b/src/publish_pose/include/publish_pose/publish_pose.hpp index cc46e7fae..7fa08c912 100644 --- a/src/publish_pose/include/publish_pose/publish_pose.hpp +++ b/src/publish_pose/include/publish_pose/publish_pose.hpp @@ -1,10 +1,10 @@ #pragma once #include -#include +#include // This header includes the SharedResourcesNode type -#include +#include #include @@ -14,7 +14,7 @@ namespace publish_pose /** * @brief TODO(...) */ -class PublishPose : public moveit_studio::behaviors::SharedResourcesNode +class PublishPose : public moveit_pro::behaviors::SharedResourcesNode { public: /** @@ -26,7 +26,7 @@ class PublishPose : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); + const std::shared_ptr& shared_resources); /** diff --git a/src/publish_pose/package.xml b/src/publish_pose/package.xml index 126cf0c3b..a1217c02a 100644 --- a/src/publish_pose/package.xml +++ b/src/publish_pose/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/publish_pose/publish_pose_plugin_description.xml b/src/publish_pose/publish_pose_plugin_description.xml index 09204e1c1..2444b2ae8 100644 --- a/src/publish_pose/publish_pose_plugin_description.xml +++ b/src/publish_pose/publish_pose_plugin_description.xml @@ -2,6 +2,6 @@ diff --git a/src/publish_pose/src/publish_pose.cpp b/src/publish_pose/src/publish_pose.cpp index b9e8830b3..9f368d963 100644 --- a/src/publish_pose/src/publish_pose.cpp +++ b/src/publish_pose/src/publish_pose.cpp @@ -1,6 +1,6 @@ #include -#include +#include #include @@ -10,8 +10,8 @@ namespace publish_pose PublishPose::PublishPose( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode( + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode( name, config, shared_resources) { } @@ -37,8 +37,8 @@ BT::PortsList PublishPose::providedPorts() BT::KeyValueVector PublishPose::metadata() { return { - { moveit_studio::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, - { moveit_studio::behaviors::kDescriptionMetadataKey, + { moveit_pro::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, "Publish a geometry_msgs::msg::PoseStamped message to a ROS 2 topic." } }; } diff --git a/src/publish_pose/src/register_behaviors.cpp b/src/publish_pose/src/register_behaviors.cpp index 5dfb2897a..d6086d02f 100644 --- a/src/publish_pose/src/register_behaviors.cpp +++ b/src/publish_pose/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace publish_pose { -class PublishPoseBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class PublishPoseBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "PublishPose", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "PublishPose", shared_resources); } }; } // namespace publish_pose PLUGINLIB_EXPORT_CLASS(publish_pose::PublishPoseBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/publish_pose/test/test_behavior_plugins.cpp b/src/publish_pose/test/test_behavior_plugins.cpp index 9acdb67d0..198feb8ed 100644 --- a/src/publish_pose/test/test_behavior_plugins.cpp +++ b/src/publish_pose/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/transform_odom_with_pose/CMakeLists.txt b/src/transform_odom_with_pose/CMakeLists.txt index ee83dca62..7b15d6d9a 100644 --- a/src/transform_odom_with_pose/CMakeLists.txt +++ b/src/transform_odom_with_pose/CMakeLists.txt @@ -4,7 +4,7 @@ project(transform_odom_with_pose CXX) find_package(moveit_pro_package REQUIRED) moveit_pro_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -39,7 +39,7 @@ endif() # plugin loaders that load the behavior base class library from the # moveit_studio_behavior package. pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface transform_odom_with_pose_plugin_description.xml) + moveit_pro_behavior_interface transform_odom_with_pose_plugin_description.xml) ament_export_targets(transform_odom_with_poseTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp b/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp index 67cd5c73e..fdb630398 100644 --- a/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp +++ b/src/transform_odom_with_pose/include/transform_odom_with_pose/transform_odom_with_pose.hpp @@ -1,8 +1,8 @@ #pragma once #include -#include -#include +#include +#include #include #include @@ -23,7 +23,7 @@ namespace transform_odom_with_pose * frame transformations (e.g., logging, frame alignment, or data conditioning). */ class TransformOdomWithPose - : public moveit_studio::behaviors::SharedResourcesNode + : public moveit_pro::behaviors::SharedResourcesNode { public: /** @@ -40,7 +40,7 @@ class TransformOdomWithPose TransformOdomWithPose( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources); + const std::shared_ptr& shared_resources); /** * @brief Define the input and output ports for this Behavior. diff --git a/src/transform_odom_with_pose/package.xml b/src/transform_odom_with_pose/package.xml index 765a88b80..1833f7dc9 100644 --- a/src/transform_odom_with_pose/package.xml +++ b/src/transform_odom_with_pose/package.xml @@ -17,7 +17,7 @@ moveit_pro_package - moveit_studio_behavior_interface + moveit_pro_behavior_interface ament_cmake_ros ament_lint_auto diff --git a/src/transform_odom_with_pose/src/register_behaviors.cpp b/src/transform_odom_with_pose/src/register_behaviors.cpp index 98e6c9096..145014ce2 100644 --- a/src/transform_odom_with_pose/src/register_behaviors.cpp +++ b/src/transform_odom_with_pose/src/register_behaviors.cpp @@ -1,6 +1,6 @@ #include -#include -#include +#include +#include #include @@ -8,17 +8,17 @@ namespace transform_odom_with_pose { -class TransformOdomWithPoseBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +class TransformOdomWithPoseBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase { public: void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override + [[maybe_unused]] const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "TransformOdomWithPose", shared_resources); + moveit_pro::behaviors::registerBehavior(factory, "TransformOdomWithPose", shared_resources); } }; } // namespace transform_odom_with_pose PLUGINLIB_EXPORT_CLASS(transform_odom_with_pose::TransformOdomWithPoseBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp index d26355e7e..fc010ebcd 100644 --- a/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp +++ b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp @@ -15,8 +15,8 @@ namespace transform_odom_with_pose TransformOdomWithPose::TransformOdomWithPose( const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) { } diff --git a/src/transform_odom_with_pose/test/test_behavior_plugins.cpp b/src/transform_odom_with_pose/test/test_behavior_plugins.cpp index 67f607993..dc7c30374 100644 --- a/src/transform_odom_with_pose/test/test_behavior_plugins.cpp +++ b/src/transform_odom_with_pose/test/test_behavior_plugins.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include @@ -11,11 +11,11 @@ */ TEST(BehaviorTests, test_load_behavior_plugins) { - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); + auto shared_resources = std::make_shared(node); BT::BehaviorTreeFactory factory; { diff --git a/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml b/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml index 45c8e0f94..65af2a41f 100644 --- a/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml +++ b/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml @@ -2,6 +2,6 @@ From 76f4a46bb97a32d2f9a2a47b6c6cd00e8258949e Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Thu, 12 Feb 2026 09:49:26 -0700 Subject: [PATCH 22/27] made new objectives --- src/get_bool_instance/CMakeLists.txt | 46 +++++++ src/get_bool_instance/behavior_plugin.yaml | 4 + .../get_bool_instance_plugin_description.xml | 7 + .../get_bool_instance/get_bool_instance.hpp | 56 ++++++++ src/get_bool_instance/package.xml | 31 +++++ .../src/get_bool_instance.cpp | 120 ++++++++++++++++ .../src/register_behaviors.cpp | 24 ++++ src/get_bool_instance/test/CMakeLists.txt | 5 + .../test/test_behavior_plugins.cpp | 37 +++++ .../objectives/clutch-in_teleop_with_meta.xml | 126 +++++++++++++++++ ..._meta_via_pose_-_fix_frame_orientation.xml | 69 ++++++++++ .../teleop_with_meta_via_pose_old.xml | 129 ++++++++++++++++++ .../teleop_with_meta_via_pose_v6.xml | 126 +++++++---------- .../teleop_with_meta_via_pose_v7.xml | 123 +++++++++++++++++ .../teleop_with_meta_via_pose_v8.xml | 128 +++++++++++++++++ 15 files changed, 958 insertions(+), 73 deletions(-) create mode 100644 src/get_bool_instance/CMakeLists.txt create mode 100644 src/get_bool_instance/behavior_plugin.yaml create mode 100644 src/get_bool_instance/get_bool_instance_plugin_description.xml create mode 100644 src/get_bool_instance/include/get_bool_instance/get_bool_instance.hpp create mode 100644 src/get_bool_instance/package.xml create mode 100644 src/get_bool_instance/src/get_bool_instance.cpp create mode 100644 src/get_bool_instance/src/register_behaviors.cpp create mode 100644 src/get_bool_instance/test/CMakeLists.txt create mode 100644 src/get_bool_instance/test/test_behavior_plugins.cpp create mode 100644 src/lab_sim/objectives/clutch-in_teleop_with_meta.xml create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_-_fix_frame_orientation.xml create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_old.xml create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v7.xml create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v8.xml diff --git a/src/get_bool_instance/CMakeLists.txt b/src/get_bool_instance/CMakeLists.txt new file mode 100644 index 000000000..dffa9a6f4 --- /dev/null +++ b/src/get_bool_instance/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(get_bool_instance CXX) + +find_package(moveit_pro_package REQUIRED) +moveit_pro_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib std_msgs) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + get_bool_instance + SHARED + src/get_bool_instance.cpp + src/register_behaviors.cpp) +target_include_directories( + get_bool_instance + PUBLIC $ + $) +ament_target_dependencies(get_bool_instance + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS get_bool_instance + EXPORT get_bool_instanceTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(get_bool_instance) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_pro_behavior package. +pluginlib_export_plugin_description_file( + moveit_pro_behavior_interface get_bool_instance_plugin_description.xml) + +ament_export_targets(get_bool_instanceTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/get_bool_instance/behavior_plugin.yaml b/src/get_bool_instance/behavior_plugin.yaml new file mode 100644 index 000000000..6e4b3f5ff --- /dev/null +++ b/src/get_bool_instance/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + get_bool_instance: + - "get_bool_instance::GetBoolInstanceBehaviorsLoader" diff --git a/src/get_bool_instance/get_bool_instance_plugin_description.xml b/src/get_bool_instance/get_bool_instance_plugin_description.xml new file mode 100644 index 000000000..b0d0fde53 --- /dev/null +++ b/src/get_bool_instance/get_bool_instance_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/get_bool_instance/include/get_bool_instance/get_bool_instance.hpp b/src/get_bool_instance/include/get_bool_instance/get_bool_instance.hpp new file mode 100644 index 000000000..bc6dddae5 --- /dev/null +++ b/src/get_bool_instance/include/get_bool_instance/get_bool_instance.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace get_bool_instance +{ +/** + * @brief Subscribes to a single boolean message and publishes it to the blackboard. + * + * The behavior exits successfully after the first message is received. + */ +class GetBoolInstance + : public moveit_pro::behaviors::SharedResourcesNode +{ +public: + GetBoolInstance(const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + static BT::KeyValueVector metadata(); + +private: + /** @brief Called once when the behavior starts. Sets up the subscription. */ + BT::NodeStatus onStart() override; + + /** + * @brief Called while the behavior is running. + * @return RUNNING until a boolean message is received, then SUCCESS. + */ + BT::NodeStatus onRunning() override; + + /** @brief Called when the behavior is halted. Cleans up the subscription. */ + void onHalted() override; + + /** @brief Subscriber for the bool topic. */ + rclcpp::Subscription::SharedPtr bool_subscriber_; + + /** @brief Mutex for thread-safe access to boolean data. */ + std::shared_mutex bool_mutex_; + + /** @brief Latest received boolean message, if any. */ + std::optional current_bool_; + + std::string bool_topic_name_; +}; + +} // namespace get_bool_instance diff --git a/src/get_bool_instance/package.xml b/src/get_bool_instance/package.xml new file mode 100644 index 000000000..7145b48d7 --- /dev/null +++ b/src/get_bool_instance/package.xml @@ -0,0 +1,31 @@ + + + get_bool_instance + 0.0.0 + Subscribes to a single boolean message and publishes it to the blackboard. The behavior exits successfully after the first message on the topic is received. + + + MoveIt Pro User + + + MoveIt Pro User + + + TODO + + ament_cmake + + moveit_pro_package + + moveit_pro_behavior_interface + std_msgs + + ament_cmake_ros + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + python3-colcon-common-extensions + diff --git a/src/get_bool_instance/src/get_bool_instance.cpp b/src/get_bool_instance/src/get_bool_instance.cpp new file mode 100644 index 000000000..cc462ba93 --- /dev/null +++ b/src/get_bool_instance/src/get_bool_instance.cpp @@ -0,0 +1,120 @@ +#include + +#include + +#include "fmt/format.h" + +#include "moveit_pro_behavior_interface/get_required_ports.hpp" +#include "moveit_pro_behavior_interface/metadata_fields.hpp" + +namespace get_bool_instance +{ + +constexpr auto kPortIdBoolTopicName = "bool_topic_name"; +constexpr auto kPortIdBoolValue = "subscribed_bool_instance"; + +GetBoolInstance::GetBoolInstance( + const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode( + name, config, shared_resources) +{ + bool_subscriber_ = + shared_resources_->node->create_subscription( + "bool_topic", // temporary default; overridden in onStart if needed + rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::Bool::SharedPtr msg) + { + std::unique_lock lock(bool_mutex_); + current_bool_ = *msg; + }); +} + +BT::PortsList GetBoolInstance::providedPorts() +{ + return { + BT::InputPort( + kPortIdBoolTopicName, + "bool_topic", + "The name of the std_msgs::msg::Bool topic to subscribe to."), + BT::OutputPort( + kPortIdBoolValue, + "{subscribed_bool_instance}", + "Subscribed boolean message.") + }; +} + +BT::KeyValueVector GetBoolInstance::metadata() +{ + return { + { moveit_pro::behaviors::kSubcategoryMetadataKey, + "User Created Behaviors" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, + "Subscribe to a single boolean message and store it on the blackboard." } + }; +} + +BT::NodeStatus GetBoolInstance::onStart() +{ + const auto ports = + moveit_pro::behaviors::getRequiredInputs( + getInput(kPortIdBoolTopicName)); + + if (!ports) + { + shared_resources_->logger->publishFailureMessage( + name(), ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& [bool_topic_name] = ports.value(); + + // Recreate subscriber only if topic changed + if (!bool_subscriber_ || bool_topic_name != bool_topic_name_) + { + bool_topic_name_ = bool_topic_name; + bool_subscriber_ = + shared_resources_->node->create_subscription( + bool_topic_name_, + rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::Bool::SharedPtr msg) + { + std::unique_lock lock(bool_mutex_); + current_bool_ = *msg; + }); + } + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus GetBoolInstance::onRunning() +{ + std::optional bool_copy; + + { + std::shared_lock lock(bool_mutex_); + if (!current_bool_) + { + return BT::NodeStatus::RUNNING; + } + bool_copy = current_bool_; + } + + setOutput(kPortIdBoolValue, *bool_copy); + + // Stop listening — snapshot behavior + bool_subscriber_.reset(); + + return BT::NodeStatus::SUCCESS; +} + +void GetBoolInstance::onHalted() +{ + bool_subscriber_.reset(); + + std::unique_lock lock(bool_mutex_); + current_bool_.reset(); +} + +} // namespace get_bool_instance diff --git a/src/get_bool_instance/src/register_behaviors.cpp b/src/get_bool_instance/src/register_behaviors.cpp new file mode 100644 index 000000000..b5c592746 --- /dev/null +++ b/src/get_bool_instance/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace get_bool_instance +{ +class GetBoolInstanceBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "GetBoolInstance", shared_resources); + + } +}; +} // namespace get_bool_instance + +PLUGINLIB_EXPORT_CLASS(get_bool_instance::GetBoolInstanceBehaviorsLoader, + moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/get_bool_instance/test/CMakeLists.txt b/src/get_bool_instance/test/CMakeLists.txt new file mode 100644 index 000000000..ceb38dd7e --- /dev/null +++ b/src/get_bool_instance/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/get_bool_instance/test/test_behavior_plugins.cpp b/src/get_bool_instance/test/test_behavior_plugins.cpp new file mode 100644 index 000000000..62d711e4a --- /dev/null +++ b/src/get_bool_instance/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("get_bool_instance::GetBoolInstanceBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "GetBoolInstance", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/lab_sim/objectives/clutch-in_teleop_with_meta.xml b/src/lab_sim/objectives/clutch-in_teleop_with_meta.xml new file mode 100644 index 000000000..7ed25185b --- /dev/null +++ b/src/lab_sim/objectives/clutch-in_teleop_with_meta.xml @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_-_fix_frame_orientation.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_-_fix_frame_orientation.xml new file mode 100644 index 000000000..32221403e --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_-_fix_frame_orientation.xml @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_old.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_old.xml new file mode 100644 index 000000000..bf9bc8a91 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_old.xml @@ -0,0 +1,129 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml index 4a4273707..94a347c41 100644 --- a/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml @@ -1,105 +1,85 @@ - + - - + + + + - - + + + - + + - - - - - - - + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - + + + - - - - + + + + - - - - - - - - - - - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v7.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v7.xml new file mode 100644 index 000000000..76410212e --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v7.xml @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v8.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v8.xml new file mode 100644 index 000000000..b960de9b9 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v8.xml @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From db9ad8eca2941bf3dca33825574954b5635638f8 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Thu, 12 Feb 2026 14:06:58 -0700 Subject: [PATCH 23/27] adding in v7 and v8 --- .../teleop_with_meta_via_pose_v9.xml | 128 ++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml new file mode 100644 index 000000000..9ed45fd6e --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 165778f7093c924161bfd1e69a4be4edc08de9c0 Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Thu, 12 Feb 2026 21:24:18 -0700 Subject: [PATCH 24/27] Add new version of teleop which should handle demonstration indicator events, gripper toggling, and clutch based teleoperation. I have not tested this against the bag file or hw yet. --- .../convertposestampedtovectors.xml | 98 +++++++ .../teleop_with_meta_via_pose_v10.xml | 266 ++++++++++++++++++ .../teleop_with_meta_via_pose_v5.xml | 119 ++------ 3 files changed, 387 insertions(+), 96 deletions(-) create mode 100644 src/lab_sim/objectives/convertposestampedtovectors.xml create mode 100644 src/lab_sim/objectives/teleop_with_meta_via_pose_v10.xml diff --git a/src/lab_sim/objectives/convertposestampedtovectors.xml b/src/lab_sim/objectives/convertposestampedtovectors.xml new file mode 100644 index 000000000..15d2c6a5b --- /dev/null +++ b/src/lab_sim/objectives/convertposestampedtovectors.xml @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + Output vector of xyzw quaternion + + + Output vector of xyz positions + + + Input PoseStamped + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v10.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v10.xml new file mode 100644 index 000000000..cff0272f6 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v10.xml @@ -0,0 +1,266 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml index a9e5f56a1..e7eab233c 100644 --- a/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml @@ -6,19 +6,20 @@ _favorite="false" > - - - + + + - + @@ -91,76 +92,7 @@ ID="Sequence" name="I don't remember having to do this unpacking-repacking nonsense the first time I wrote this, but this seems to do the trick." > - - - - - - - - - - - - - - - + - + - - - + - - - - - - + From b216652dccaf817a34c077e2b435d2001f5d619e Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Fri, 13 Feb 2026 12:12:13 -0700 Subject: [PATCH 25/27] made version 9 and made a subtree for initial controller position --- .../objectives/rising_edge_detection.xml | 2 +- .../store_initial_controller_pose.xml | 30 +++++++++ .../teleop_with_meta_via_pose_v9.xml | 61 ++----------------- 3 files changed, 37 insertions(+), 56 deletions(-) create mode 100644 src/lab_sim/objectives/store_initial_controller_pose.xml diff --git a/src/lab_sim/objectives/rising_edge_detection.xml b/src/lab_sim/objectives/rising_edge_detection.xml index 721bffdc8..57844822c 100644 --- a/src/lab_sim/objectives/rising_edge_detection.xml +++ b/src/lab_sim/objectives/rising_edge_detection.xml @@ -18,7 +18,7 @@ - + diff --git a/src/lab_sim/objectives/store_initial_controller_pose.xml b/src/lab_sim/objectives/store_initial_controller_pose.xml new file mode 100644 index 000000000..d123aab77 --- /dev/null +++ b/src/lab_sim/objectives/store_initial_controller_pose.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml b/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml index 9ed45fd6e..ae09dceff 100644 --- a/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml @@ -2,31 +2,8 @@ - - - - - - - - - - - - - - - - - - - - - - - - - + + @@ -50,30 +27,9 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + @@ -118,11 +74,6 @@ - - - - - - + From e43b03b0a2d7bb5e80a34e0054842e3fbd6547f0 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 25 Feb 2026 13:28:03 -0700 Subject: [PATCH 26/27] I think the transformation bit is working? But going to try ignoring the initial rotation of the end effector --- .../objectives/transform_quest_frame.xml | 40 ++++++++++++++ .../objectives/visualize_quest_transform.xml | 54 +++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 src/lab_sim/objectives/transform_quest_frame.xml create mode 100644 src/lab_sim/objectives/visualize_quest_transform.xml diff --git a/src/lab_sim/objectives/transform_quest_frame.xml b/src/lab_sim/objectives/transform_quest_frame.xml new file mode 100644 index 000000000..e072134ea --- /dev/null +++ b/src/lab_sim/objectives/transform_quest_frame.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/visualize_quest_transform.xml b/src/lab_sim/objectives/visualize_quest_transform.xml new file mode 100644 index 000000000..44ab8ad10 --- /dev/null +++ b/src/lab_sim/objectives/visualize_quest_transform.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 629a91165716187a602f761d65930404f80c4ec6 Mon Sep 17 00:00:00 2001 From: Kristen Such Date: Wed, 25 Feb 2026 15:19:22 -0700 Subject: [PATCH 27/27] omg this is working on the generated bags --- .../objectives/visualize_quest_transform.xml | 55 ++++++++++++------- 1 file changed, 34 insertions(+), 21 deletions(-) diff --git a/src/lab_sim/objectives/visualize_quest_transform.xml b/src/lab_sim/objectives/visualize_quest_transform.xml index 44ab8ad10..08c000c7f 100644 --- a/src/lab_sim/objectives/visualize_quest_transform.xml +++ b/src/lab_sim/objectives/visualize_quest_transform.xml @@ -1,49 +1,62 @@ + - - - - - - + + + - + - + - + - - + + + + + - + - - + - + - - + + + + + + + + + + + + + - - + + + + - - + +