Merge pull request #3 from PickNikRobotics/registration

Moving registration behaviors

Author: MikeWrock

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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-         $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  PRIVATE ${PCL_INCLUDE_DIRS})
 ament_target_dependencies(example_behaviors
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
 

…icknik_registration/ndt_registration.hpp …e/example_behaviors/ndt_registration.hppsrc/picknik_registration/include/picknik_registration/ndt_registration.hpp renamed to src/example_behaviors/include/example_behaviors/ndt_registration.hpp src/picknik_registration/include/picknik_registration/ndt_registration.hpp renamed to src/example_behaviors/include/example_behaviors/ndt_registration.hpp

@@ -7,7 +7,7 @@
 #include <moveit_studio_behavior_interface/async_behavior_base.hpp>
 
 
-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<moveit_studio::behaviors::BehaviorContext>& 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<tl::expected<bool, std::string>> future_;
 };
-}  // namespace picknik_registration
+}  // namespace example_behaviors

…nik_registration/ransac_registration.hpp …xample_behaviors/ransac_registration.hppsrc/picknik_registration/include/picknik_registration/ransac_registration.hpp renamed to src/example_behaviors/include/example_behaviors/ransac_registration.hpp src/picknik_registration/include/picknik_registration/ransac_registration.hpp renamed to src/example_behaviors/include/example_behaviors/ransac_registration.hpp

@@ -7,7 +7,7 @@
 #include <moveit_studio_behavior_interface/async_behavior_base.hpp>
 
 
-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<moveit_studio::behaviors::BehaviorContext>& 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<tl::expected<bool, std::string>> future_;
 };
-}  // namespace picknik_registration
+}  // namespace example_behaviors

src/example_behaviors/package.xml

@@ -4,17 +4,21 @@
   <version>0.0.0</version>
   <description>Example behaviors for MoveIt Pro</description>
 
-  <maintainer email="john@doe.com">John Doe</maintainer>
-  <author email="john@doe.com">John Doe</author>
+  <maintainer email="support@picknik.ai">MoveIt Pro Maintainer</maintainer>
+  <author email="support@picknik.ai">MoveIt Pro Maintainer</author>
 
-  <license>TODO</license>
+  <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <build_depend>moveit_studio_common</build_depend>
 
   <depend>moveit_studio_behavior_interface</depend>
   <depend>example_interfaces</depend>
+  <depend>perception_pcl</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_studio_vision_msgs</depend>
+  <depend>moveit_studio_vision</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>

…ik_registration/src/ndt_registration.cpp …ample_behaviors/src/ndt_registration.cppsrc/picknik_registration/src/ndt_registration.cpp renamed to src/example_behaviors/src/ndt_registration.cpp src/picknik_registration/src/ndt_registration.cpp renamed to src/example_behaviors/src/ndt_registration.cpp

@@ -1,9 +1,8 @@
-#include <picknik_registration/ndt_registration.hpp>
+#include <example_behaviors/ndt_registration.hpp>
 #include <pcl/registration/ndt.h>
 #include <tl_expected/expected.hpp>
 #include <moveit_studio_behavior_interface/async_behavior_base.hpp>
 #include <moveit_studio_behavior_interface/check_for_error.hpp>
-// #include <moveit_studio_vision/geometry_types.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/header.hpp>
@@ -23,7 +22,7 @@
 #include <pcl/registration/sample_consensus_prerejective.h>
 #include <pcl/search/kdtree.h>
 
-namespace picknik_registration
+namespace example_behaviors
 {
 namespace
 {
@@ -145,4 +144,4 @@ tl::expected<bool, std::string> NDTRegistration::doWork()
 }
 
 
-}  // namespace picknik_registration
+}  // namespace example_behaviors

…registration/src/ransac_registration.cpp …le_behaviors/src/ransac_registration.cppsrc/picknik_registration/src/ransac_registration.cpp renamed to src/example_behaviors/src/ransac_registration.cpp src/picknik_registration/src/ransac_registration.cpp renamed to src/example_behaviors/src/ransac_registration.cpp

@@ -1,4 +1,4 @@
-#include <picknik_registration/ransac_registration.hpp>
+#include <example_behaviors/ransac_registration.hpp>
 #include <pcl/registration/ndt.h>
 #include <tl_expected/expected.hpp>
 #include <moveit_studio_behavior_interface/async_behavior_base.hpp>
@@ -22,7 +22,7 @@
 #include <pcl/registration/sample_consensus_prerejective.h>
 #include <pcl/search/kdtree.h>
 
-namespace picknik_registration
+namespace example_behaviors
 {
 namespace
 {
@@ -232,4 +232,4 @@ tl::expected<bool, std::string> RANSACRegistration::doWork()
 }
 
 
-}  // namespace picknik_registration
+}  // namespace example_behaviors

src/example_behaviors/src/register_behaviors.cpp

@@ -12,6 +12,8 @@
 #include <example_behaviors/publish_color_rgba.hpp>
 #include <example_behaviors/setup_mtc_pick_from_pose.hpp>
 #include <example_behaviors/setup_mtc_place_from_pose.hpp>
+#include <example_behaviors/ndt_registration.hpp>
+#include <example_behaviors/ransac_registration.hpp>
 
 #include <pluginlib/class_list_macros.hpp>
 
@@ -36,6 +38,8 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN
     moveit_studio::behaviors::registerBehavior<SetupMtcPickFromPose>(factory, "SetupMtcPickFromPose", shared_resources);
     moveit_studio::behaviors::registerBehavior<SetupMtcPlaceFromPose>(factory, "SetupMtcPlaceFromPose",
                                                                       shared_resources);
+    moveit_studio::behaviors::registerBehavior<NDTRegistration>(factory, "NDTRegistration", shared_resources);
+    moveit_studio::behaviors::registerBehavior<RANSACRegistration>(factory, "RANSACRegistration", shared_resources);
   }
 };
 }  // namespace example_behaviors

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)

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]

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: