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 new file mode 100644 index 000000000..4cbdd8ca4 --- /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_pro_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_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}) +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..8d3b8bc08 --- /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..722d71966 --- /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..b34243cdd --- /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_pro_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..3543843bb --- /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_pro::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..4f5750605 --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "ConvertOdomToPoseStamped"); + + } +}; +} // namespace convert_odom_to_pose_stamped + +PLUGINLIB_EXPORT_CLASS(convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader, + moveit_pro::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..51a8791be --- /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_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("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/convert_pose_stamped_to_odom/CMakeLists.txt b/src/convert_pose_stamped_to_odom/CMakeLists.txt new file mode 100644 index 000000000..5bb76d75d --- /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_pro_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_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}) +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..f1f5833cd --- /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..6b6453f11 --- /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..a496f4a90 --- /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_pro_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..d85b81165 --- /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_pro::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..73c3a80f5 --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "ConvertPoseStampedToOdom"); + + } +}; +} // namespace convert_pose_stamped_to_odom + +PLUGINLIB_EXPORT_CLASS(convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader, + moveit_pro::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..2d13fcbbe --- /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_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("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/convert_pose_stamped_to_transform_stamped/CMakeLists.txt b/src/convert_pose_stamped_to_transform_stamped/CMakeLists.txt new file mode 100644 index 000000000..7563c1275 --- /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_pro_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_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}) +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..0235c808e --- /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..d4a092e5c --- /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_pro::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..d04e5653c --- /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_pro_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..9254e78a5 --- /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_pro_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_pro::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_pro::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..efafecec3 --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + 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_pro::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..06556f40b --- /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_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("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..fe39c9ac9 --- /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_pro_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_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}) +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..554d72b30 --- /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..2c9a7112e --- /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_pro::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..f5f0a989e --- /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_pro_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..b480055f7 --- /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_pro_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_pro::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_pro::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..5905333a8 --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "ConvertTransformStampedToOdom", shared_resources); + } +}; +} // namespace convert_transform_stamped_to_odom + +PLUGINLIB_EXPORT_CLASS(convert_transform_stamped_to_odom::ConvertTransformStampedToOdomBehaviorsLoader, + moveit_pro::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..f52e45cea --- /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_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("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/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/factory_sim/objectives/teleop_with_meta.xml b/src/factory_sim/objectives/teleop_with_meta.xml new file mode 100644 index 000000000..b173ab0ba --- /dev/null +++ b/src/factory_sim/objectives/teleop_with_meta.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/factory_sim/objectives/transformquestframe.xml b/src/factory_sim/objectives/transformquestframe.xml new file mode 100644 index 000000000..3e5f28f78 --- /dev/null +++ b/src/factory_sim/objectives/transformquestframe.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/get_bool/CMakeLists.txt b/src/get_bool/CMakeLists.txt new file mode 100644 index 000000000..83c007e68 --- /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_pro_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_pro_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..4c49e5ed9 --- /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..bdbc0c0ad --- /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_pro::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..dae4e7f85 --- /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_pro_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..049473ca5 --- /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_pro_behavior_interface/get_required_ports.hpp" +#include "moveit_pro_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_pro::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_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_pro::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..e3ca0eda2 --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "GetBool", shared_resources); + + } +}; +} // namespace get_bool + +PLUGINLIB_EXPORT_CLASS(get_bool::GetBoolBehaviorsLoader, + moveit_pro::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..f17f5ddea --- /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_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::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(); +} 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/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 new file mode 100644 index 000000000..970b79774 --- /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_pro::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 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 new file mode 100644 index 000000000..54a80d667 --- /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_pro_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_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}) +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..d9c130408 --- /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..651908e07 --- /dev/null +++ b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp @@ -0,0 +1,56 @@ +#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_pro::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_; + + std::string odom_topic_name_; +}; + +} // 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..a1cdea8e1 --- /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_pro_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..8c9ae01c3 --- /dev/null +++ b/src/get_odom_instance/src/get_odom_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_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_pro::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() +{ + 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_pro::behaviors::kSubcategoryMetadataKey, + "User Created Behaviors" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, + "Subscribe to a single odometry message and store it on the blackboard." } + }; +} + +BT::NodeStatus GetOdomInstance::onStart() +{ + const auto ports = + moveit_pro::behaviors::getRequiredInputs( + getInput(kPortIdOdomTopicName)); + + if (!ports) + { + shared_resources_->logger->publishFailureMessage( + 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_) + { + 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; + }); + } + + 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); + + // Stop listening — snapshot behavior + odom_subscriber_.reset(); + + 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..5a41105bc --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "GetOdomInstance", shared_resources); + + } +}; +} // namespace get_odom_instance + +PLUGINLIB_EXPORT_CLASS(get_odom_instance::GetOdomInstanceBehaviorsLoader, + moveit_pro::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..5a639c88a --- /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_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_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/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/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/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/debug_transformation.xml b/src/lab_sim/objectives/debug_transformation.xml new file mode 100644 index 000000000..6ec85ed9d --- /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/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..57844822c --- /dev/null +++ b/src/lab_sim/objectives/rising_edge_detection.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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.xml b/src/lab_sim/objectives/teleop_with_meta.xml new file mode 100644 index 000000000..d2156a674 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta.xml @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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_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_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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..e7eab233c --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v5.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..94a347c41 --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v6.xml @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..ae09dceff --- /dev/null +++ b/src/lab_sim/objectives/teleop_with_meta_via_pose_v9.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + 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/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/transformquestframe.xml b/src/lab_sim/objectives/transformquestframe.xml new file mode 100644 index 000000000..0b3d29c55 --- /dev/null +++ b/src/lab_sim/objectives/transformquestframe.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..08c000c7f --- /dev/null +++ b/src/lab_sim/objectives/visualize_quest_transform.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/publish_odom/CMakeLists.txt b/src/publish_odom/CMakeLists.txt new file mode 100644 index 000000000..fc99d245c --- /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_pro_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_pro_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..c7e21dfb3 --- /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_pro::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..2a5514833 --- /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_pro_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..b8483fecd --- /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..e701ddc00 --- /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_pro::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_pro::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, + { moveit_pro::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..848b6ecdb --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "PublishOdom", shared_resources); + + } +}; +} // namespace publish_odom + +PLUGINLIB_EXPORT_CLASS(publish_odom::PublishOdomBehaviorsLoader, + moveit_pro::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..0935ad5fe --- /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_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("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/publish_pose/CMakeLists.txt b/src/publish_pose/CMakeLists.txt new file mode 100644 index 000000000..388fd8fc9 --- /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_pro_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_pro_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..7fa08c912 --- /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_pro::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..a1217c02a --- /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_pro_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..2444b2ae8 --- /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..9f368d963 --- /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_pro::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_pro::behaviors::kSubcategoryMetadataKey, "ROS Messaging" }, + { moveit_pro::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..d6086d02f --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "PublishPose", shared_resources); + + } +}; +} // namespace publish_pose + +PLUGINLIB_EXPORT_CLASS(publish_pose::PublishPoseBehaviorsLoader, + moveit_pro::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..198feb8ed --- /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_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("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/CMakeLists.txt b/src/transform_odom_with_pose/CMakeLists.txt new file mode 100644 index 000000000..7b15d6d9a --- /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_pro_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_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}) +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..fdb630398 --- /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_pro::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..1833f7dc9 --- /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_pro_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..145014ce2 --- /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_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior(factory, "TransformOdomWithPose", shared_resources); + + } +}; +} // namespace transform_odom_with_pose + +PLUGINLIB_EXPORT_CLASS(transform_odom_with_pose::TransformOdomWithPoseBehaviorsLoader, + 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 new file mode 100644 index 000000000..fc010ebcd --- /dev/null +++ b/src/transform_odom_with_pose/src/transform_odom_with_pose.cpp @@ -0,0 +1,132 @@ +#include + +#include +#include +#include + +#include "spdlog/spdlog.h" +#include + +#include +#include + +namespace transform_odom_with_pose +{ + +TransformOdomWithPose::TransformOdomWithPose( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_pro::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList TransformOdomWithPose::providedPorts() +{ + return { + BT::InputPort( + "odom_in", "Input odometry message"), + BT::InputPort( + "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") + }; +} + +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() +{ + auto odom_in_res = getInput("odom_in"); + 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 || !ee_pose_res || !controller_pose_res || !child_frame_res) + { + shared_resources_->logger->publishFailureMessage( + 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(); + + // --- 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); + + 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, + 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; + + 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; + + setOutput("odom_out", odom_out); + + shared_resources_->logger->publishInfoMessage( + name(), "Successfully transformed odometry using pose transform."); + + 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..dc7c30374 --- /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_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("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..65af2a41f --- /dev/null +++ b/src/transform_odom_with_pose/transform_odom_with_pose_plugin_description.xml @@ -0,0 +1,7 @@ + + + +