diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt index ead16d95..21be9500 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 3957124b..433ce58d 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 71af241c..b3bd141c 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 0561a8f6..d68b3219 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 d9240fdb..12129edc 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 1ee39719..324f5a87 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 f0315f69..c3d26677 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 4fbf18fd..c6432ea5 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 11911421..538cc7d9 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 60a000bf..d02533be 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 a70ffaf9..00000000 --- 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 60885b30..00000000 --- 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 d3d820a0..00000000 --- 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 8e8fc829..00000000 --- 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 1f4774d1..00000000 --- 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 5b5afa9a..00000000 --- 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 8db41205..00000000 --- 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 788d332a..00000000 --- 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(); -}