From 05e796e89388482db3633fbc59351b65650b7de8 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 11 Nov 2024 16:25:42 -0800 Subject: [PATCH] Moving registration behaviors --- src/example_behaviors/CMakeLists.txt | 13 +++- .../example_behaviors}/ndt_registration.hpp | 10 +-- .../ransac_registration.hpp | 10 +-- src/example_behaviors/package.xml | 10 ++- .../src/ndt_registration.cpp | 7 +-- .../src/ransac_registration.cpp | 6 +- .../src/register_behaviors.cpp | 4 ++ .../test/test_behavior_plugins.cpp | 6 +- src/lab_sim/config/config.yaml | 1 - .../picknik_ur_sim_config/config/config.yaml | 1 - src/picknik_registration/CMakeLists.txt | 61 ------------------- src/picknik_registration/behavior_plugin.yaml | 4 -- .../config/tree_nodes_model.xml | 5 -- src/picknik_registration/package.xml | 29 --------- ...icknik_registration_plugin_description.xml | 5 -- .../src/register_behaviors.cpp | 28 --------- src/picknik_registration/test/CMakeLists.txt | 4 -- .../test/test_behavior_plugins.cpp | 39 ------------ 18 files changed, 43 insertions(+), 200 deletions(-) rename src/{picknik_registration/include/picknik_registration => example_behaviors/include/example_behaviors}/ndt_registration.hpp (90%) rename src/{picknik_registration/include/picknik_registration => example_behaviors/include/example_behaviors}/ransac_registration.hpp (90%) rename src/{picknik_registration => example_behaviors}/src/ndt_registration.cpp (97%) rename src/{picknik_registration => example_behaviors}/src/ransac_registration.cpp (98%) delete mode 100644 src/picknik_registration/CMakeLists.txt delete mode 100644 src/picknik_registration/behavior_plugin.yaml delete mode 100644 src/picknik_registration/config/tree_nodes_model.xml delete mode 100644 src/picknik_registration/package.xml delete mode 100644 src/picknik_registration/picknik_registration_plugin_description.xml delete mode 100644 src/picknik_registration/src/register_behaviors.cpp delete mode 100644 src/picknik_registration/test/CMakeLists.txt delete mode 100644 src/picknik_registration/test/test_behavior_plugins.cpp diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt index ead16d9..21be950 100644 --- a/src/example_behaviors/CMakeLists.txt +++ b/src/example_behaviors/CMakeLists.txt @@ -5,7 +5,13 @@ find_package(moveit_studio_common REQUIRED) find_package(example_interfaces REQUIRED) moveit_studio_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior moveit_studio_behavior_interface pluginlib example_interfaces) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior moveit_studio_behavior_interface pluginlib +moveit_studio_vision +moveit_studio_vision_msgs +PCL +pcl_conversions +pcl_ros +example_interfaces) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -23,11 +29,14 @@ add_library( src/setup_mtc_pick_from_pose.cpp src/setup_mtc_place_from_pose.cpp src/setup_mtc_wave_hand.cpp + src/ndt_registration.cpp + src/ransac_registration.cpp src/register_behaviors.cpp) target_include_directories( example_behaviors PUBLIC $ - $) + $ + PRIVATE ${PCL_INCLUDE_DIRS}) ament_target_dependencies(example_behaviors ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/picknik_registration/include/picknik_registration/ndt_registration.hpp b/src/example_behaviors/include/example_behaviors/ndt_registration.hpp similarity index 90% rename from src/picknik_registration/include/picknik_registration/ndt_registration.hpp rename to src/example_behaviors/include/example_behaviors/ndt_registration.hpp index 3957124..433ce58 100644 --- a/src/picknik_registration/include/picknik_registration/ndt_registration.hpp +++ b/src/example_behaviors/include/example_behaviors/ndt_registration.hpp @@ -7,7 +7,7 @@ #include -namespace picknik_registration +namespace example_behaviors { /** * @brief TODO(...) @@ -16,7 +16,7 @@ class NDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase { public: /** - * @brief Constructor for the picknik_registration behavior. + * @brief Constructor for the 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. @@ -25,10 +25,10 @@ class NDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase NDTRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** - * @brief Implementation of the required providedPorts() function for the picknik_registration Behavior. + * @brief Implementation of the required providedPorts() function for the 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 picknik_registration does not use expose any ports, so this function returns an empty list. + * @return See ndt_registration.cpp for port list and description. */ static BT::PortsList providedPorts(); @@ -52,4 +52,4 @@ class NDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ std::shared_future> future_; }; -} // namespace picknik_registration +} // namespace example_behaviors diff --git a/src/picknik_registration/include/picknik_registration/ransac_registration.hpp b/src/example_behaviors/include/example_behaviors/ransac_registration.hpp similarity index 90% rename from src/picknik_registration/include/picknik_registration/ransac_registration.hpp rename to src/example_behaviors/include/example_behaviors/ransac_registration.hpp index 71af241..b3bd141 100644 --- a/src/picknik_registration/include/picknik_registration/ransac_registration.hpp +++ b/src/example_behaviors/include/example_behaviors/ransac_registration.hpp @@ -7,7 +7,7 @@ #include -namespace picknik_registration +namespace example_behaviors { /** * @brief TODO(...) @@ -16,7 +16,7 @@ class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase { public: /** - * @brief Constructor for the picknik_registration behavior. + * @brief Constructor for the 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. @@ -25,10 +25,10 @@ class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase RANSACRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** - * @brief Implementation of the required providedPorts() function for the picknik_registration Behavior. + * @brief Implementation of the required providedPorts() function for the 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 picknik_registration does not use expose any ports, so this function returns an empty list. + * @return See ransac_registration.cpp for port list and description. */ static BT::PortsList providedPorts(); @@ -52,4 +52,4 @@ class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ std::shared_future> future_; }; -} // namespace picknik_registration +} // namespace example_behaviors diff --git a/src/example_behaviors/package.xml b/src/example_behaviors/package.xml index 0561a8f..d68b321 100644 --- a/src/example_behaviors/package.xml +++ b/src/example_behaviors/package.xml @@ -4,10 +4,10 @@ 0.0.0 Example behaviors for MoveIt Pro - John Doe - John Doe + MoveIt Pro Maintainer + MoveIt Pro Maintainer - TODO + BSD-3-Clause ament_cmake @@ -15,6 +15,10 @@ moveit_studio_behavior_interface example_interfaces + perception_pcl + moveit_ros_planning_interface + moveit_studio_vision_msgs + moveit_studio_vision ament_lint_auto ament_cmake_gtest diff --git a/src/picknik_registration/src/ndt_registration.cpp b/src/example_behaviors/src/ndt_registration.cpp similarity index 97% rename from src/picknik_registration/src/ndt_registration.cpp rename to src/example_behaviors/src/ndt_registration.cpp index d9240fd..12129ed 100644 --- a/src/picknik_registration/src/ndt_registration.cpp +++ b/src/example_behaviors/src/ndt_registration.cpp @@ -1,9 +1,8 @@ -#include +#include #include #include #include #include -// #include #include #include #include @@ -23,7 +22,7 @@ #include #include -namespace picknik_registration +namespace example_behaviors { namespace { @@ -145,4 +144,4 @@ tl::expected NDTRegistration::doWork() } -} // namespace picknik_registration +} // namespace example_behaviors diff --git a/src/picknik_registration/src/ransac_registration.cpp b/src/example_behaviors/src/ransac_registration.cpp similarity index 98% rename from src/picknik_registration/src/ransac_registration.cpp rename to src/example_behaviors/src/ransac_registration.cpp index 1ee3971..324f5a8 100644 --- a/src/picknik_registration/src/ransac_registration.cpp +++ b/src/example_behaviors/src/ransac_registration.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -22,7 +22,7 @@ #include #include -namespace picknik_registration +namespace example_behaviors { namespace { @@ -232,4 +232,4 @@ tl::expected RANSACRegistration::doWork() } -} // namespace picknik_registration +} // namespace example_behaviors diff --git a/src/example_behaviors/src/register_behaviors.cpp b/src/example_behaviors/src/register_behaviors.cpp index f0315f6..c3d2667 100644 --- a/src/example_behaviors/src/register_behaviors.cpp +++ b/src/example_behaviors/src/register_behaviors.cpp @@ -12,6 +12,8 @@ #include #include #include +#include +#include #include @@ -36,6 +38,8 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN moveit_studio::behaviors::registerBehavior(factory, "SetupMtcPickFromPose", shared_resources); moveit_studio::behaviors::registerBehavior(factory, "SetupMtcPlaceFromPose", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "NDTRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "RANSACRegistration", shared_resources); } }; } // namespace example_behaviors diff --git a/src/example_behaviors/test/test_behavior_plugins.cpp b/src/example_behaviors/test/test_behavior_plugins.cpp index 4fbf18f..c6432ea 100644 --- a/src/example_behaviors/test/test_behavior_plugins.cpp +++ b/src/example_behaviors/test/test_behavior_plugins.cpp @@ -36,7 +36,11 @@ TEST(BehaviorTests, test_load_behavior_plugins) (void)factory.instantiateTreeNode("test_behavior_name", "SetupMtcPickFromPose", BT::NodeConfiguration())); EXPECT_NO_THROW( (void)factory.instantiateTreeNode("test_behavior_name", "SetupMtcPlaceFromPose", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "SetupMTCWaveHand", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "SetupMTCWaveHand", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "NDTRegistration", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "RANSACRegistration", BT::NodeConfiguration())); } int main(int argc, char** argv) diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml index 1191142..538cc7d 100644 --- a/src/lab_sim/config/config.yaml +++ b/src/lab_sim/config/config.yaml @@ -42,7 +42,6 @@ objectives: - "moveit_studio::behaviors::MTCCoreBehaviorsLoader" - "moveit_studio::behaviors::ServoBehaviorsLoader" - "moveit_studio::behaviors::VisionBehaviorsLoader" - - "picknik_registration::PicknikRegistrationBehaviorsLoader" - "example_behaviors::ExampleBehaviorsLoader" # Specify source folder for objectives # [Required] diff --git a/src/moveit_pro_ur_configs/picknik_ur_sim_config/config/config.yaml b/src/moveit_pro_ur_configs/picknik_ur_sim_config/config/config.yaml index 60a000b..d02533b 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_sim_config/config/config.yaml +++ b/src/moveit_pro_ur_configs/picknik_ur_sim_config/config/config.yaml @@ -42,7 +42,6 @@ objectives: - "moveit_studio::behaviors::MTCCoreBehaviorsLoader" - "moveit_studio::behaviors::ServoBehaviorsLoader" - "moveit_studio::behaviors::VisionBehaviorsLoader" - - "picknik_registration::PicknikRegistrationBehaviorsLoader" # Specify source folder for objectives # [Required] objective_library_paths: diff --git a/src/picknik_registration/CMakeLists.txt b/src/picknik_registration/CMakeLists.txt deleted file mode 100644 index a70ffaf..0000000 --- a/src/picknik_registration/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(picknik_registration CXX) - -find_package(moveit_studio_common REQUIRED) -moveit_studio_package() - -find_package(moveit_studio_behavior_interface REQUIRED) -find_package(pluginlib REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib -moveit_studio_vision -moveit_studio_vision_msgs -PCL -pcl_conversions -pcl_ros -moveit_ros_planning_interface -geometric_shapes -shape_msgs) -foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${package} REQUIRED) -endforeach() - -add_library( - picknik_registration - SHARED - src/ndt_registration.cpp - src/ransac_registration.cpp - src/register_behaviors.cpp) -target_include_directories( - picknik_registration - PUBLIC $ - $ - PRIVATE ${PCL_INCLUDE_DIRS}) -ament_target_dependencies(picknik_registration - ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Install Libraries -install( - TARGETS picknik_registration - EXPORT picknik_registrationTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES - DESTINATION include) - -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) - -if(BUILD_TESTING) - moveit_pro_behavior_test(picknik_registration) -endif() - -# Export the behavior plugins defined in this package so they are available to -# plugin loaders that load the behavior base class library from the -# moveit_studio_behavior package. -pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface picknik_registration_plugin_description.xml) - -ament_export_targets(picknik_registrationTargets HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/src/picknik_registration/behavior_plugin.yaml b/src/picknik_registration/behavior_plugin.yaml deleted file mode 100644 index 60885b3..0000000 --- a/src/picknik_registration/behavior_plugin.yaml +++ /dev/null @@ -1,4 +0,0 @@ -objectives: - behavior_loader_plugins: - picknik_registration: - - "picknik_registration::PicknikRegistrationBehaviorsLoader" diff --git a/src/picknik_registration/config/tree_nodes_model.xml b/src/picknik_registration/config/tree_nodes_model.xml deleted file mode 100644 index d3d820a..0000000 --- a/src/picknik_registration/config/tree_nodes_model.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/picknik_registration/package.xml b/src/picknik_registration/package.xml deleted file mode 100644 index 8e8fc82..0000000 --- a/src/picknik_registration/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - picknik_registration - 0.0.0 - Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algoritm - - MoveIt Pro Maintainer - - BSD-3-Clause - - ament_cmake - - moveit_studio_common - - moveit_studio_behavior_interface - perception_pcl - moveit_ros_planning_interface - moveit_studio_vision_msgs - moveit_studio_vision - - ament_lint_auto - ament_cmake_gtest - ament_clang_format - ament_clang_tidy - - - ament_cmake - - diff --git a/src/picknik_registration/picknik_registration_plugin_description.xml b/src/picknik_registration/picknik_registration_plugin_description.xml deleted file mode 100644 index 1f4774d..0000000 --- a/src/picknik_registration/picknik_registration_plugin_description.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/src/picknik_registration/src/register_behaviors.cpp b/src/picknik_registration/src/register_behaviors.cpp deleted file mode 100644 index 5b5afa9..0000000 --- a/src/picknik_registration/src/register_behaviors.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include -#include - -#include -#include - -#include - -#include - -namespace picknik_registration -{ -class PicknikRegistrationBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase -{ -public: - void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override - { - moveit_studio::behaviors::registerBehavior(factory, "NDTRegistration", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "RANSACRegistration", shared_resources); - - } -}; -} // namespace picknik_registration - -PLUGINLIB_EXPORT_CLASS(picknik_registration::PicknikRegistrationBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/picknik_registration/test/CMakeLists.txt b/src/picknik_registration/test/CMakeLists.txt deleted file mode 100644 index 8db4120..0000000 --- a/src/picknik_registration/test/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -find_package(ament_cmake_gtest REQUIRED) - -ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp) -ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/picknik_registration/test/test_behavior_plugins.cpp b/src/picknik_registration/test/test_behavior_plugins.cpp deleted file mode 100644 index 788d332..0000000 --- a/src/picknik_registration/test/test_behavior_plugins.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include - -#include -#include -#include -#include - -/** - * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and - * instantiated by the behavior tree factory. - */ -TEST(BehaviorTests, test_load_behavior_plugins) -{ - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); - - auto node = std::make_shared("test_node"); - auto shared_resources = std::make_shared(node); - - BT::BehaviorTreeFactory factory; - { - auto plugin_instance = class_loader.createUniqueInstance("picknik_registration::PicknikRegistrationBehaviorsLoader"); - 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", "NDTRegistration", BT::NodeConfiguration())); - EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "RANSACRegistration", BT::NodeConfiguration())); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}