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..300cc8a48 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp @@ -0,0 +1,52 @@ +#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..6c7987d46 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/package.xml @@ -0,0 +1,29 @@ + + + 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..0daa5ac27 --- /dev/null +++ b/src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp @@ -0,0 +1,59 @@ +#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..78d56063b --- /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..449bd162a --- /dev/null +++ b/src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp @@ -0,0 +1,38 @@ +#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_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/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..263419321 --- /dev/null +++ b/src/get_odom_instance/include/get_odom_instance/get_odom_instance.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace get_odom_instance +{ +/** + * @brief Subscribes to a single odometry message and publishes it to the blackboard. + * + * The behavior exits successfully after the first message is received. + */ +class GetOdomInstance : public moveit_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..1cc96fcc6 --- /dev/null +++ b/src/get_odom_instance/package.xml @@ -0,0 +1,28 @@ + + + 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..af06c9314 --- /dev/null +++ b/src/get_odom_instance/src/get_odom_instance.cpp @@ -0,0 +1,98 @@ +#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..0100cab07 --- /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..1d2039a18 --- /dev/null +++ b/src/get_odom_instance/test/test_behavior_plugins.cpp @@ -0,0 +1,36 @@ +#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/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/transform_quest_frame.xml b/src/lab_sim/objectives/transform_quest_frame.xml new file mode 100644 index 000000000..7602af859 --- /dev/null +++ b/src/lab_sim/objectives/transform_quest_frame.xml @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..f295801e4 --- /dev/null +++ b/src/lab_sim/objectives/visualize_quest_transform.xml @@ -0,0 +1,196 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +