From 2bc1b8e4704b2ed161bbbc91b6d178b26c28a4a5 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Thu, 5 Dec 2024 23:19:25 -0800 Subject: [PATCH] Squashed 'src/example_behaviors/' changes from d1451e3..b360dc0 b360dc0 Merge commit '7e8c6066177e2fd16ea46cfb16d8c207e17fd929' a71ffeb fixed build and test for example behaviors (#39) 72ad640 Sam naming (#38) 46a17ce Sync 6.5 to main (#37) d41f38d Add example SAM 2 behavior and objective (#23) f6ac548 Sync V6.5 to main (#29) e932e45 Merge branch 'main' into pr-update-to-make-ports-explicit 6a14c44 Fix depend of moveit_studio_behavior 1416547 version bump 5f6056a formatting a06f504 pre-commit formatting 5bfeaa5 Moving registration behaviors 93dd16f Added mtc to jtc converter git-subtree-dir: src/example_behaviors git-subtree-split: b360dc0970f6dbe4b633cb33edb9ef75042f00d7 --- CMakeLists.txt | 37 ++- COLCON_IGNORE | 0 behavior_plugin.yaml | 2 +- example_behaviors_plugin_description.xml | 2 +- ...nvert_mtc_solution_to_joint_trajectory.hpp | 49 ++++ ...> example_add_two_ints_service_client.hpp} | 4 +- ...nvert_mtc_solution_to_joint_trajectory.hpp | 49 ++++ ...essage.hpp => example_delayed_message.hpp} | 6 +- ...pp => example_fibonacci_action_client.hpp} | 4 +- ....hpp => example_get_string_from_topic.hpp} | 4 +- ...ello_world.hpp => example_hello_world.hpp} | 10 +- .../example_ndt_registration.hpp | 55 ++++ ...gba.hpp => example_publish_color_rgba.hpp} | 4 +- .../example_ransac_registration.hpp | 55 ++++ .../example_sam2_segmentation.hpp | 67 +++++ ...p => example_setup_mtc_pick_from_pose.hpp} | 4 +- ... => example_setup_mtc_place_from_pose.hpp} | 4 +- ...nd.hpp => example_setup_mtc_wave_hand.hpp} | 14 +- .../example_behaviors/ndt_registration.hpp | 55 ++++ .../example_behaviors/ransac_registration.hpp | 55 ++++ models/decoder.onnx | 3 + models/sam2_hiera_large_encoder.onnx | 3 + package.xml | 13 +- ...nvert_mtc_solution_to_joint_trajectory.cpp | 142 +++++++++++ ...> example_add_two_ints_service_client.cpp} | 16 +- ...nvert_mtc_solution_to_joint_trajectory.cpp | 142 +++++++++++ ...essage.cpp => example_delayed_message.cpp} | 16 +- ...pp => example_fibonacci_action_client.cpp} | 18 +- ....cpp => example_get_string_from_topic.cpp} | 10 +- ...ello_world.cpp => example_hello_world.cpp} | 14 +- src/example_ndt_registration.cpp | 147 +++++++++++ ...gba.cpp => example_publish_color_rgba.cpp} | 12 +- src/example_ransac_registration.cpp | 235 ++++++++++++++++++ src/example_sam2_segmentation.cpp | 142 +++++++++++ ...p => example_setup_mtc_pick_from_pose.cpp} | 14 +- ... => example_setup_mtc_place_from_pose.cpp} | 14 +- ...nd.cpp => example_setup_mtc_wave_hand.cpp} | 14 +- src/ndt_registration.cpp | 147 +++++++++++ src/ransac_registration.cpp | 235 ++++++++++++++++++ src/register_behaviors.cpp | 44 ++-- test/test_behavior_plugins.cpp | 24 +- 41 files changed, 1749 insertions(+), 136 deletions(-) create mode 100644 COLCON_IGNORE create mode 100644 include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp rename include/example_behaviors/{add_two_ints_service_client.hpp => example_add_two_ints_service_client.hpp} (91%) create mode 100644 include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp rename include/example_behaviors/{delayed_message.hpp => example_delayed_message.hpp} (90%) rename include/example_behaviors/{fibonacci_action_client.hpp => example_fibonacci_action_client.hpp} (92%) rename include/example_behaviors/{get_string_from_topic.hpp => example_get_string_from_topic.hpp} (86%) rename include/example_behaviors/{hello_world.hpp => example_hello_world.hpp} (84%) create mode 100644 include/example_behaviors/example_ndt_registration.hpp rename include/example_behaviors/{publish_color_rgba.hpp => example_publish_color_rgba.hpp} (78%) create mode 100644 include/example_behaviors/example_ransac_registration.hpp create mode 100644 include/example_behaviors/example_sam2_segmentation.hpp rename include/example_behaviors/{setup_mtc_pick_from_pose.hpp => example_setup_mtc_pick_from_pose.hpp} (83%) rename include/example_behaviors/{setup_mtc_place_from_pose.hpp => example_setup_mtc_place_from_pose.hpp} (83%) rename include/example_behaviors/{setup_mtc_wave_hand.hpp => example_setup_mtc_wave_hand.hpp} (86%) create mode 100644 include/example_behaviors/ndt_registration.hpp create mode 100644 include/example_behaviors/ransac_registration.hpp create mode 100644 models/decoder.onnx create mode 100644 models/sam2_hiera_large_encoder.onnx create mode 100644 src/convert_mtc_solution_to_joint_trajectory.cpp rename src/{add_two_ints_service_client.cpp => example_add_two_ints_service_client.cpp} (74%) create mode 100644 src/example_convert_mtc_solution_to_joint_trajectory.cpp rename src/{delayed_message.cpp => example_delayed_message.cpp} (84%) rename src/{fibonacci_action_client.cpp => example_fibonacci_action_client.cpp} (78%) rename src/{get_string_from_topic.cpp => example_get_string_from_topic.cpp} (74%) rename src/{hello_world.cpp => example_hello_world.cpp} (55%) create mode 100644 src/example_ndt_registration.cpp rename src/{publish_color_rgba.cpp => example_publish_color_rgba.cpp} (66%) create mode 100644 src/example_ransac_registration.cpp create mode 100644 src/example_sam2_segmentation.cpp rename src/{setup_mtc_pick_from_pose.cpp => example_setup_mtc_pick_from_pose.cpp} (96%) rename src/{setup_mtc_place_from_pose.cpp => example_setup_mtc_place_from_pose.cpp} (95%) rename src/{setup_mtc_wave_hand.cpp => example_setup_mtc_wave_hand.cpp} (90%) create mode 100644 src/ndt_registration.cpp create mode 100644 src/ransac_registration.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e8b9eddb..2c7cd6a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,30 +5,43 @@ find_package(moveit_studio_common REQUIRED) find_package(example_interfaces REQUIRED) moveit_studio_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS 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() +find_package(moveit_pro_ml REQUIRED) add_library( example_behaviors SHARED - src/add_two_ints_service_client.cpp - src/delayed_message.cpp - src/get_string_from_topic.cpp - src/fibonacci_action_client.cpp - src/hello_world.cpp - src/publish_color_rgba.cpp - src/setup_mtc_pick_from_pose.cpp - src/setup_mtc_place_from_pose.cpp - src/setup_mtc_wave_hand.cpp + src/example_add_two_ints_service_client.cpp + src/example_convert_mtc_solution_to_joint_trajectory.cpp + src/example_delayed_message.cpp + src/example_get_string_from_topic.cpp + src/example_fibonacci_action_client.cpp + src/example_hello_world.cpp + src/example_publish_color_rgba.cpp + src/example_setup_mtc_pick_from_pose.cpp + src/example_setup_mtc_place_from_pose.cpp + src/example_setup_mtc_wave_hand.cpp + src/example_ndt_registration.cpp + src/example_ransac_registration.cpp + src/example_sam2_segmentation.cpp src/register_behaviors.cpp) target_include_directories( example_behaviors PUBLIC $ - $) + $ + PRIVATE ${PCL_INCLUDE_DIRS}) ament_target_dependencies(example_behaviors ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(example_behaviors onnx_sam2) # Install Libraries install( @@ -40,7 +53,7 @@ install( INCLUDES DESTINATION include) -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config models DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) moveit_pro_behavior_test(example_behaviors) diff --git a/COLCON_IGNORE b/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/behavior_plugin.yaml b/behavior_plugin.yaml index 570905cb..a5f10861 100644 --- a/behavior_plugin.yaml +++ b/behavior_plugin.yaml @@ -1,4 +1,4 @@ objectives: behavior_loader_plugins: example_behaviors: - - "example_behaviors::ExampleBehaviorsBehaviorsLoader" + - "example_behaviors::ExampleBehaviorsLoader" diff --git a/example_behaviors_plugin_description.xml b/example_behaviors_plugin_description.xml index 3174df85..347594f9 100644 --- a/example_behaviors_plugin_description.xml +++ b/example_behaviors_plugin_description.xml @@ -1,7 +1,7 @@ diff --git a/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp b/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp new file mode 100644 index 00000000..1c0a95c7 --- /dev/null +++ b/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace example_behaviors +{ +/** + * @brief Converts a MoveIt Task Constructor Solution into a JointTrajectory. + * + * @details + * | Data Port Name | Port Type | Object Type | + * | ----------------- |---------------|---------------------------------------------------------| + * | solution | Input | moveit_task_constructor_msgs::msg::Solution | + * | joint_group | Input | std::string | + * | velocity_scaling_factor | Input | double | + * | acceleration_scaling_factor | Input | double | + * | sampling_rate | Input | int | + * | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory | + */ +class ConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + ConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; + +private: + std::unique_ptr robot_model_loader_; + + const std::vector& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution); +}; +} // namespace example_behaviors diff --git a/include/example_behaviors/add_two_ints_service_client.hpp b/include/example_behaviors/example_add_two_ints_service_client.hpp similarity index 91% rename from include/example_behaviors/add_two_ints_service_client.hpp rename to include/example_behaviors/example_add_two_ints_service_client.hpp index a79f76e8..9840ee58 100644 --- a/include/example_behaviors/add_two_ints_service_client.hpp +++ b/include/example_behaviors/example_add_two_ints_service_client.hpp @@ -9,10 +9,10 @@ using AddTwoInts = example_interfaces::srv::AddTwoInts; namespace example_behaviors { -class AddTwoIntsServiceClient final : public ServiceClientBehaviorBase +class ExampleAddTwoIntsServiceClient final : public ServiceClientBehaviorBase { public: - AddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config, + ExampleAddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */ diff --git a/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp b/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp new file mode 100644 index 00000000..c8712d5a --- /dev/null +++ b/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace example_behaviors +{ +/** + * @brief Converts a MoveIt Task Constructor Solution into a JointTrajectory. + * + * @details + * | Data Port Name | Port Type | Object Type | + * | ----------------- |---------------|---------------------------------------------------------| + * | solution | Input | moveit_task_constructor_msgs::msg::Solution | + * | joint_group | Input | std::string | + * | velocity_scaling_factor | Input | double | + * | acceleration_scaling_factor | Input | double | + * | sampling_rate | Input | int | + * | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory | + */ +class ExampleConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + ExampleConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::PortsList providedPorts(); + + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; + +private: + std::unique_ptr robot_model_loader_; + + const std::vector& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution); +}; +} // namespace example_behaviors diff --git a/include/example_behaviors/delayed_message.hpp b/include/example_behaviors/example_delayed_message.hpp similarity index 90% rename from include/example_behaviors/delayed_message.hpp rename to include/example_behaviors/example_delayed_message.hpp index 6e95c173..b6648f93 100644 --- a/include/example_behaviors/delayed_message.hpp +++ b/include/example_behaviors/example_delayed_message.hpp @@ -12,9 +12,9 @@ namespace example_behaviors { /** - * @brief DelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port + * @brief ExampleDelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port */ -class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode +class ExampleDelayedMessage : public moveit_studio::behaviors::SharedResourcesNode { private: std::chrono::time_point start_time_; @@ -33,7 +33,7 @@ class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); /** diff --git a/include/example_behaviors/fibonacci_action_client.hpp b/include/example_behaviors/example_fibonacci_action_client.hpp similarity index 92% rename from include/example_behaviors/fibonacci_action_client.hpp rename to include/example_behaviors/example_fibonacci_action_client.hpp index 06f70b12..19e4e2f9 100644 --- a/include/example_behaviors/fibonacci_action_client.hpp +++ b/include/example_behaviors/example_fibonacci_action_client.hpp @@ -9,10 +9,10 @@ using Fibonacci = example_interfaces::action::Fibonacci; namespace example_behaviors { -class FibonacciActionClient final : public ActionClientBehaviorBase +class ExampleFibonacciActionClient final : public ActionClientBehaviorBase { public: - FibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config, + ExampleFibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */ diff --git a/include/example_behaviors/get_string_from_topic.hpp b/include/example_behaviors/example_get_string_from_topic.hpp similarity index 86% rename from include/example_behaviors/get_string_from_topic.hpp rename to include/example_behaviors/example_get_string_from_topic.hpp index 7c6e6662..5ea2d711 100644 --- a/include/example_behaviors/get_string_from_topic.hpp +++ b/include/example_behaviors/example_get_string_from_topic.hpp @@ -8,10 +8,10 @@ using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase; namespace example_behaviors { -class GetStringFromTopic final : public GetMessageFromTopicBehaviorBase +class ExampleGetStringFromTopic final : public GetMessageFromTopicBehaviorBase { public: - GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, + ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */ diff --git a/include/example_behaviors/hello_world.hpp b/include/example_behaviors/example_hello_world.hpp similarity index 84% rename from include/example_behaviors/hello_world.hpp rename to include/example_behaviors/example_hello_world.hpp index 02886219..524f650c 100644 --- a/include/example_behaviors/hello_world.hpp +++ b/include/example_behaviors/example_hello_world.hpp @@ -8,9 +8,9 @@ namespace example_behaviors { /** - * @brief The HelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS + * @brief The ExampleHelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS */ -class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode +class ExampleHelloWorld : public moveit_studio::behaviors::SharedResourcesNode { public: /** @@ -25,7 +25,7 @@ class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode& shared_resources); /** @@ -46,9 +46,9 @@ class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode + +// This header includes the SharedResourcesNode type +#include +#include + + +namespace example_behaviors +{ +/** + * @brief TODO(...) + */ +class ExampleNDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase +{ +public: + /** + * @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. + * @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. + */ + ExampleNDTRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @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 See ndt_registration.cpp for port list and description. + */ + 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(); + +protected: + tl::expected doWork() override; + +private: + /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ + std::shared_future>& getFuture() override + { + return future_; + } + + /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ + std::shared_future> future_; +}; +} // namespace example_behaviors diff --git a/include/example_behaviors/publish_color_rgba.hpp b/include/example_behaviors/example_publish_color_rgba.hpp similarity index 78% rename from include/example_behaviors/publish_color_rgba.hpp rename to include/example_behaviors/example_publish_color_rgba.hpp index 38531347..11c08809 100644 --- a/include/example_behaviors/publish_color_rgba.hpp +++ b/include/example_behaviors/example_publish_color_rgba.hpp @@ -12,10 +12,10 @@ namespace example_behaviors { -class PublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode +class ExamplePublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode { public: - PublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, + ExamplePublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/include/example_behaviors/example_ransac_registration.hpp b/include/example_behaviors/example_ransac_registration.hpp new file mode 100644 index 00000000..a786b733 --- /dev/null +++ b/include/example_behaviors/example_ransac_registration.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +// This header includes the SharedResourcesNode type +#include +#include + + +namespace example_behaviors +{ +/** + * @brief TODO(...) + */ +class ExampleRANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase +{ +public: + /** + * @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. + * @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. + */ + ExampleRANSACRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @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 See ransac_registration.cpp for port list and description. + */ + 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(); + +protected: + tl::expected doWork() override; + +private: + /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ + std::shared_future>& getFuture() override + { + return future_; + } + + /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ + std::shared_future> future_; +}; +} // namespace example_behaviors diff --git a/include/example_behaviors/example_sam2_segmentation.hpp b/include/example_behaviors/example_sam2_segmentation.hpp new file mode 100644 index 00000000..dcbe8878 --- /dev/null +++ b/include/example_behaviors/example_sam2_segmentation.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + + +namespace example_behaviors +{ +/** + * @brief Segment an image using the SAM 2 model + */ +class ExampleSAM2Segmentation : public moveit_studio::behaviors::AsyncBehaviorBase +{ +public: +/** + * @brief Constructor for the ExampleSAM2Segmentation 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. + */ + ExampleSAM2Segmentation(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + /** + * @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 List of ports for the behavior. + */ + 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(); + +protected: + tl::expected doWork() override; + + +private: + std::unique_ptr sam2_; + moveit_pro_ml::ONNXImage onnx_image_; + sensor_msgs::msg::Image mask_image_msg_; + moveit_studio_vision_msgs::msg::Mask2D mask_msg_; + + /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ + std::shared_future>& getFuture() override + { + return future_; + } + + /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ + std::shared_future> future_; + +}; +} // namespace example_behaviors diff --git a/include/example_behaviors/setup_mtc_pick_from_pose.hpp b/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp similarity index 83% rename from include/example_behaviors/setup_mtc_pick_from_pose.hpp rename to include/example_behaviors/example_setup_mtc_pick_from_pose.hpp index e271bd23..3ce38834 100644 --- a/include/example_behaviors/setup_mtc_pick_from_pose.hpp +++ b/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp @@ -17,10 +17,10 @@ namespace example_behaviors * | task | Bidirectional | std::shared_ptr | * | grasp_pose | Input | geometry_msgs::msg::PoseStamped | */ -class SetupMtcPickFromPose final : public moveit_studio::behaviors::SharedResourcesNode +class ExampleSetupMtcPickFromPose final : public moveit_studio::behaviors::SharedResourcesNode { public: - SetupMtcPickFromPose(const std::string& name, const BT::NodeConfiguration& config, + ExampleSetupMtcPickFromPose(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/include/example_behaviors/setup_mtc_place_from_pose.hpp b/include/example_behaviors/example_setup_mtc_place_from_pose.hpp similarity index 83% rename from include/example_behaviors/setup_mtc_place_from_pose.hpp rename to include/example_behaviors/example_setup_mtc_place_from_pose.hpp index c06431cb..9f81ae9a 100644 --- a/include/example_behaviors/setup_mtc_place_from_pose.hpp +++ b/include/example_behaviors/example_setup_mtc_place_from_pose.hpp @@ -17,10 +17,10 @@ namespace example_behaviors * | task | Bidirectional | std::shared_ptr | * | grasp_pose | Input | geometry_msgs::msg::PoseStamped | */ -class SetupMtcPlaceFromPose final : public moveit_studio::behaviors::SharedResourcesNode +class ExampleSetupMtcPlaceFromPose final : public moveit_studio::behaviors::SharedResourcesNode { public: - SetupMtcPlaceFromPose(const std::string& name, const BT::NodeConfiguration& config, + ExampleSetupMtcPlaceFromPose(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/include/example_behaviors/setup_mtc_wave_hand.hpp b/include/example_behaviors/example_setup_mtc_wave_hand.hpp similarity index 86% rename from include/example_behaviors/setup_mtc_wave_hand.hpp rename to include/example_behaviors/example_setup_mtc_wave_hand.hpp index 4d635b08..756b3555 100644 --- a/include/example_behaviors/setup_mtc_wave_hand.hpp +++ b/include/example_behaviors/example_setup_mtc_wave_hand.hpp @@ -11,7 +11,7 @@ namespace example_behaviors { /** - * @brief The SetupMTCWaveHand behavior makes any robot move the end of its arm back and forth. + * @brief The ExampleSetupMTCWaveHand behavior makes any robot move the end of its arm back and forth. * @details This is an example of a behavior that uses MoveIt Task Constructor to configure an MTC task, * which can be planned and executed by MoveIt Pro's core PlanMTCTask and ExecuteMTCTask Behaviors. * It is derived from AsyncBehaviorBase, an extension of the templated SharedResourcesNode type, @@ -24,11 +24,11 @@ namespace example_behaviors * if the process completed successfully or was canceled, * or an error message if the process failed unexpectedly. */ -class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBase +class ExampleSetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBase { public: /** - * @brief Constructor for the SetupMTCWaveHand behavior. + * @brief Constructor for the ExampleSetupMTCWaveHand 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 @@ -38,17 +38,17 @@ class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBas * 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. */ - SetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, + ExampleSetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); /** - * @brief Implementation of the required providedPorts() function for the SetupMTCWaveHand Behavior. + * @brief Implementation of the required providedPorts() function for the ExampleSetupMTCWaveHand 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 SetupMTCWaveHand has one data port: a bidirectional port named "task", which is a shared_ptr + * @return ExampleSetupMTCWaveHand has one data port: a bidirectional port named "task", which is a shared_ptr * to an MTC task object. This function returns a BT::PortsList that declares this single port. */ static BT::PortsList providedPorts(); @@ -62,7 +62,7 @@ class SetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBas private: /** - * @brief Async thread for SetupMTCWaveHand. Adds MTC stages to an MTC task provided on a data port. + * @brief Async thread for ExampleSetupMTCWaveHand. Adds MTC stages to an MTC task provided on a data port. * @details This function is where the Behavior performs its work asynchronously while the behavior tree ticks. * It is very important that behaviors return from being ticked very quickly because if it blocks before returning * it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors diff --git a/include/example_behaviors/ndt_registration.hpp b/include/example_behaviors/ndt_registration.hpp new file mode 100644 index 00000000..433ce58d --- /dev/null +++ b/include/example_behaviors/ndt_registration.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +// This header includes the SharedResourcesNode type +#include +#include + + +namespace example_behaviors +{ +/** + * @brief TODO(...) + */ +class NDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase +{ +public: + /** + * @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. + * @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. + */ + NDTRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @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 See ndt_registration.cpp for port list and description. + */ + 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(); + +protected: + tl::expected doWork() override; + +private: + /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ + std::shared_future>& getFuture() override + { + return future_; + } + + /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ + std::shared_future> future_; +}; +} // namespace example_behaviors diff --git a/include/example_behaviors/ransac_registration.hpp b/include/example_behaviors/ransac_registration.hpp new file mode 100644 index 00000000..b3bd141c --- /dev/null +++ b/include/example_behaviors/ransac_registration.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +// This header includes the SharedResourcesNode type +#include +#include + + +namespace example_behaviors +{ +/** + * @brief TODO(...) + */ +class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase +{ +public: + /** + * @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. + * @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. + */ + RANSACRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @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 See ransac_registration.cpp for port list and description. + */ + 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(); + +protected: + tl::expected doWork() override; + +private: + /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ + std::shared_future>& getFuture() override + { + return future_; + } + + /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ + std::shared_future> future_; +}; +} // namespace example_behaviors diff --git a/models/decoder.onnx b/models/decoder.onnx new file mode 100644 index 00000000..fc4bc327 --- /dev/null +++ b/models/decoder.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1f448cdb479e6ec14e61c4756138eb4081ce7f8a11ca43a0a24856d5e8b61b6f +size 20665365 diff --git a/models/sam2_hiera_large_encoder.onnx b/models/sam2_hiera_large_encoder.onnx new file mode 100644 index 00000000..cace9759 --- /dev/null +++ b/models/sam2_hiera_large_encoder.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c99ab89a38385753aff7ea9155f0808ad5535bc55ea2a49320254e39e4011630 +size 889364590 diff --git a/package.xml b/package.xml index 0561a8f6..0ac3ce5d 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,13 @@ example_behaviors - 0.0.0 + 6.5.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,11 @@ moveit_studio_behavior_interface example_interfaces + perception_pcl + moveit_ros_planning_interface + moveit_studio_vision_msgs + moveit_studio_vision + moveit_studio_behavior ament_lint_auto ament_cmake_gtest diff --git a/src/convert_mtc_solution_to_joint_trajectory.cpp b/src/convert_mtc_solution_to_joint_trajectory.cpp new file mode 100644 index 00000000..6cb5cd28 --- /dev/null +++ b/src/convert_mtc_solution_to_joint_trajectory.cpp @@ -0,0 +1,142 @@ +#include + +namespace +{ +const auto kLogger = rclcpp::get_logger("ConvertMtcSolutionToJointTrajectory"); + +// Port names for input and output ports. +constexpr auto kPortIDSolution = "solution"; +constexpr auto kPortIDJointGroup = "joint_group"; +constexpr auto kPortIDJointTrajectory = "joint_trajectory"; +constexpr auto kPortIDVelocityScalingFactor = "velocity_scaling_factor"; +constexpr auto kPortIDAccelerationScalingFactor = "acceleration_scaling_factor"; +constexpr auto kPortIDSamplingRate = "sampling_rate"; +} // namespace + +namespace example_behaviors +{ +ConvertMtcSolutionToJointTrajectory::ConvertMtcSolutionToJointTrajectory( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList ConvertMtcSolutionToJointTrajectory::providedPorts() +{ + return { + BT::InputPort(kPortIDSolution, "{mtc_solution}", + "MoveIt Task Constructor solution."), + BT::InputPort(kPortIDJointGroup, "manipulator", + "Joint group name used in the MTC solution."), + BT::InputPort(kPortIDVelocityScalingFactor, 1.0, + "Velocity scaling factor for trajectory generation."), + BT::InputPort(kPortIDAccelerationScalingFactor, 1.0, + "Acceleration scaling factor for trajectory generation."), + BT::InputPort(kPortIDSamplingRate, 100, + "Sampling rate for trajectory generation."), + BT::OutputPort(kPortIDJointTrajectory, "{joint_trajectory_msg}", + "Resulting joint trajectory."), + }; +} + +BT::KeyValueVector ConvertMtcSolutionToJointTrajectory::metadata() +{ + return { { "subcategory", "Motion Planning" }, + { "description", "Extracts joint space trajectories from an MTC solution and adds time parameterization using TOTG." } }; +} + +BT::NodeStatus ConvertMtcSolutionToJointTrajectory::tick() +{ + using namespace moveit_studio::behaviors; + + // Load data from the behavior input ports. + const auto solution = getInput(kPortIDSolution); + const auto joint_group_name = getInput(kPortIDJointGroup); + const auto velocity_scaling_factor = getInput(kPortIDVelocityScalingFactor); + const auto acceleration_scaling_factor = getInput(kPortIDAccelerationScalingFactor); + const auto sampling_rate = getInput(kPortIDSamplingRate); + + // Check that the required input data port was set + if (const auto error = maybe_error(solution, joint_group_name, velocity_scaling_factor, acceleration_scaling_factor, sampling_rate); error) + { + spdlog::error("Failed to get required value from input data port: {}", error.value()); + return BT::NodeStatus::FAILURE; + } + // Initialize the robot model loader if this is the first time this Behavior has been run. + if (robot_model_loader_ == nullptr) + { + robot_model_loader_ = std::make_unique(shared_resources_->node); + } + + // Get the robot model from the robot model loader. + const auto robot_model = robot_model_loader_->getModel(); + if (robot_model == nullptr) + { + // Return as a failure case if the robot model loader has no robot model. + spdlog::error("Failed to load robot model."); + return BT::NodeStatus::FAILURE; + } + + // Get the JointModelGroup using the joint group name from the input port + auto joint_model_group = robot_model->getJointModelGroup(joint_group_name.value()); + if (!joint_model_group) + { + spdlog::error("Failed to get JointModelGroup '{}'.", joint_group_name.value()); + return BT::NodeStatus::FAILURE; + } + + // Extract joint positions from the MTC solution + const auto& waypoints = extractJointPositions(solution.value()); + + // Use trajectory_utils.hpp to create a trajectory + auto trajectory_result = createTrajectoryFromWaypoints( + *joint_model_group, waypoints, velocity_scaling_factor.value(), acceleration_scaling_factor.value(), sampling_rate.value()); + + if (!trajectory_result) + { + spdlog::error("Trajectory generation failed: {}", trajectory_result.error()); + return BT::NodeStatus::FAILURE; + } + + // Set the output port with the resulting joint trajectory + setOutput(kPortIDJointTrajectory, trajectory_result.value()); + + spdlog::info("Successfully converted MTC Solution to JointTrajectory with {} points.", + trajectory_result.value().points.size()); + + return BT::NodeStatus::SUCCESS; +} + +const std::vector& ConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) +{ + static std::vector waypoints; + waypoints.clear(); + for (size_t sub_traj_idx = 0; sub_traj_idx < solution.sub_trajectory.size(); ++sub_traj_idx) + { + const auto& sub_traj = solution.sub_trajectory[sub_traj_idx]; + const auto& joint_traj = sub_traj.trajectory.joint_trajectory; + + if (joint_traj.points.empty()) + { + spdlog::warn("Sub-trajectory {} has no points.", sub_traj_idx); + continue; + } + + spdlog::info("Processing sub-trajectory {} with {} points.", + sub_traj_idx, joint_traj.points.size()); + + for (const auto& point : joint_traj.points) + { + Eigen::VectorXd positions(point.positions.size()); + for (size_t i = 0; i < point.positions.size(); ++i) + { + positions[i] = point.positions[i]; + } + waypoints.push_back(positions); + } + } + return waypoints; +} + +} // namespace example_behaviors diff --git a/src/add_two_ints_service_client.cpp b/src/example_add_two_ints_service_client.cpp similarity index 74% rename from src/add_two_ints_service_client.cpp rename to src/example_add_two_ints_service_client.cpp index 9ade021b..5e90fa05 100644 --- a/src/add_two_ints_service_client.cpp +++ b/src/example_add_two_ints_service_client.cpp @@ -1,17 +1,17 @@ -#include +#include // Include the template implementation for GetMessageFromTopicBehaviorBase. #include namespace example_behaviors { -AddTwoIntsServiceClient::AddTwoIntsServiceClient( +ExampleAddTwoIntsServiceClient::ExampleAddTwoIntsServiceClient( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : ServiceClientBehaviorBase(name, config, shared_resources) { } -BT::PortsList AddTwoIntsServiceClient::providedPorts() +BT::PortsList ExampleAddTwoIntsServiceClient::providedPorts() { // This node has three input ports and one output port return BT::PortsList({ @@ -22,13 +22,13 @@ BT::PortsList AddTwoIntsServiceClient::providedPorts() }); } -BT::KeyValueVector AddTwoIntsServiceClient::metadata() +BT::KeyValueVector ExampleAddTwoIntsServiceClient::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Calls a service to add two integers and makes the result available on an output port." } }; } -tl::expected AddTwoIntsServiceClient::getServiceName() +tl::expected ExampleAddTwoIntsServiceClient::getServiceName() { const auto service_name = getInput("service_name"); if (const auto error = moveit_studio::behaviors::maybe_error(service_name)) @@ -38,7 +38,7 @@ tl::expected AddTwoIntsServiceClient::getServiceName() return service_name.value(); } -tl::expected AddTwoIntsServiceClient::createRequest() +tl::expected ExampleAddTwoIntsServiceClient::createRequest() { const auto a = getInput("addend1"); const auto b = getInput("addend2"); @@ -49,7 +49,7 @@ tl::expected AddTwoIntsServiceClient::createRe return example_interfaces::build().a(a.value()).b(b.value()); } -tl::expected AddTwoIntsServiceClient::processResponse(const AddTwoInts::Response& response) +tl::expected ExampleAddTwoIntsServiceClient::processResponse(const AddTwoInts::Response& response) { setOutput("result", response.sum); return { true }; diff --git a/src/example_convert_mtc_solution_to_joint_trajectory.cpp b/src/example_convert_mtc_solution_to_joint_trajectory.cpp new file mode 100644 index 00000000..51dd0077 --- /dev/null +++ b/src/example_convert_mtc_solution_to_joint_trajectory.cpp @@ -0,0 +1,142 @@ +#include + +namespace +{ +const auto kLogger = rclcpp::get_logger("ExampleConvertMtcSolutionToJointTrajectory"); + +// Port names for input and output ports. +constexpr auto kPortIDSolution = "solution"; +constexpr auto kPortIDJointGroup = "joint_group"; +constexpr auto kPortIDJointTrajectory = "joint_trajectory"; +constexpr auto kPortIDVelocityScalingFactor = "velocity_scaling_factor"; +constexpr auto kPortIDAccelerationScalingFactor = "acceleration_scaling_factor"; +constexpr auto kPortIDSamplingRate = "sampling_rate"; +} // namespace + +namespace example_behaviors +{ +ExampleConvertMtcSolutionToJointTrajectory::ExampleConvertMtcSolutionToJointTrajectory( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList ExampleConvertMtcSolutionToJointTrajectory::providedPorts() +{ + return { + BT::InputPort(kPortIDSolution, "{mtc_solution}", + "MoveIt Task Constructor solution."), + BT::InputPort(kPortIDJointGroup, "manipulator", + "Joint group name used in the MTC solution."), + BT::InputPort(kPortIDVelocityScalingFactor, 1.0, + "Velocity scaling factor for trajectory generation."), + BT::InputPort(kPortIDAccelerationScalingFactor, 1.0, + "Acceleration scaling factor for trajectory generation."), + BT::InputPort(kPortIDSamplingRate, 100, + "Sampling rate for trajectory generation."), + BT::OutputPort(kPortIDJointTrajectory, "{joint_trajectory_msg}", + "Resulting joint trajectory."), + }; +} + +BT::KeyValueVector ExampleConvertMtcSolutionToJointTrajectory::metadata() +{ + return { { "subcategory", "Motion Planning" }, + { "description", "Extracts joint space trajectories from an MTC solution and adds time parameterization using TOTG." } }; +} + +BT::NodeStatus ExampleConvertMtcSolutionToJointTrajectory::tick() +{ + using namespace moveit_studio::behaviors; + + // Load data from the behavior input ports. + const auto solution = getInput(kPortIDSolution); + const auto joint_group_name = getInput(kPortIDJointGroup); + const auto velocity_scaling_factor = getInput(kPortIDVelocityScalingFactor); + const auto acceleration_scaling_factor = getInput(kPortIDAccelerationScalingFactor); + const auto sampling_rate = getInput(kPortIDSamplingRate); + + // Check that the required input data port was set + if (const auto error = maybe_error(solution, joint_group_name, velocity_scaling_factor, acceleration_scaling_factor, sampling_rate); error) + { + spdlog::error("Failed to get required value from input data port: {}", error.value()); + return BT::NodeStatus::FAILURE; + } + // Initialize the robot model loader if this is the first time this Behavior has been run. + if (robot_model_loader_ == nullptr) + { + robot_model_loader_ = std::make_unique(shared_resources_->node); + } + + // Get the robot model from the robot model loader. + const auto robot_model = robot_model_loader_->getModel(); + if (robot_model == nullptr) + { + // Return as a failure case if the robot model loader has no robot model. + spdlog::error("Failed to load robot model."); + return BT::NodeStatus::FAILURE; + } + + // Get the JointModelGroup using the joint group name from the input port + auto joint_model_group = robot_model->getJointModelGroup(joint_group_name.value()); + if (!joint_model_group) + { + spdlog::error("Failed to get JointModelGroup '{}'.", joint_group_name.value()); + return BT::NodeStatus::FAILURE; + } + + // Extract joint positions from the MTC solution + const auto& waypoints = extractJointPositions(solution.value()); + + // Use trajectory_utils.hpp to create a trajectory + auto trajectory_result = createTrajectoryFromWaypoints( + *joint_model_group, waypoints, velocity_scaling_factor.value(), acceleration_scaling_factor.value(), sampling_rate.value()); + + if (!trajectory_result) + { + spdlog::error("Trajectory generation failed: {}", trajectory_result.error()); + return BT::NodeStatus::FAILURE; + } + + // Set the output port with the resulting joint trajectory + setOutput(kPortIDJointTrajectory, trajectory_result.value()); + + spdlog::info("Successfully converted MTC Solution to JointTrajectory with {} points.", + trajectory_result.value().points.size()); + + return BT::NodeStatus::SUCCESS; +} + +const std::vector& ExampleConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) +{ + static std::vector waypoints; + waypoints.clear(); + for (size_t sub_traj_idx = 0; sub_traj_idx < solution.sub_trajectory.size(); ++sub_traj_idx) + { + const auto& sub_traj = solution.sub_trajectory[sub_traj_idx]; + const auto& joint_traj = sub_traj.trajectory.joint_trajectory; + + if (joint_traj.points.empty()) + { + spdlog::warn("Sub-trajectory {} has no points.", sub_traj_idx); + continue; + } + + spdlog::info("Processing sub-trajectory {} with {} points.", + sub_traj_idx, joint_traj.points.size()); + + for (const auto& point : joint_traj.points) + { + Eigen::VectorXd positions(point.positions.size()); + for (size_t i = 0; i < point.positions.size(); ++i) + { + positions[i] = point.positions[i]; + } + waypoints.push_back(positions); + } + } + return waypoints; +} + +} // namespace example_behaviors diff --git a/src/delayed_message.cpp b/src/example_delayed_message.cpp similarity index 84% rename from src/delayed_message.cpp rename to src/example_delayed_message.cpp index 87c4c826..34818fae 100644 --- a/src/delayed_message.cpp +++ b/src/example_delayed_message.cpp @@ -1,27 +1,27 @@ -#include +#include namespace example_behaviors { -DelayedMessage::DelayedMessage(const std::string& name, const BT::NodeConfiguration& config, +ExampleDelayedMessage::ExampleDelayedMessage(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList DelayedMessage::providedPorts() +BT::PortsList ExampleDelayedMessage::providedPorts() { // delay_duration: Number of seconds to wait before logging a message return BT::PortsList( { BT::InputPort("delay_duration", "5", "The duration, in seconds, to wait before logging a message.") }); } -BT::KeyValueVector DelayedMessage::metadata() +BT::KeyValueVector ExampleDelayedMessage::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "After some time, log a message that says \"Hello, world!\"." } }; } -BT::NodeStatus DelayedMessage::onStart() +BT::NodeStatus ExampleDelayedMessage::onStart() { // Store the time at which this node was first ticked start_time_ = std::chrono::steady_clock::now(); @@ -53,7 +53,7 @@ BT::NodeStatus DelayedMessage::onStart() return BT::NodeStatus::RUNNING; } -BT::NodeStatus DelayedMessage::onRunning() +BT::NodeStatus ExampleDelayedMessage::onRunning() { // If the delay duration has not elapsed since this node was started, return RUNNING if (std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time_).count() < @@ -70,7 +70,7 @@ BT::NodeStatus DelayedMessage::onRunning() } } -void DelayedMessage::onHalted() +void ExampleDelayedMessage::onHalted() { } diff --git a/src/fibonacci_action_client.cpp b/src/example_fibonacci_action_client.cpp similarity index 78% rename from src/fibonacci_action_client.cpp rename to src/example_fibonacci_action_client.cpp index cf3255e9..e8c0bdd1 100644 --- a/src/fibonacci_action_client.cpp +++ b/src/example_fibonacci_action_client.cpp @@ -1,18 +1,18 @@ -#include +#include // Include the template implementation for ActionClientBehaviorBase. #include namespace example_behaviors { -FibonacciActionClient::FibonacciActionClient( +ExampleFibonacciActionClient::ExampleFibonacciActionClient( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : ActionClientBehaviorBase(name, config, shared_resources) { } -BT::PortsList FibonacciActionClient::providedPorts() +BT::PortsList ExampleFibonacciActionClient::providedPorts() { // This node has two input ports and two output ports return BT::PortsList({ @@ -26,14 +26,14 @@ BT::PortsList FibonacciActionClient::providedPorts() }); } -BT::KeyValueVector FibonacciActionClient::metadata() +BT::KeyValueVector ExampleFibonacciActionClient::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Calls an action to get a Fibonacci sequence and makes the result available on an output port." } }; } -tl::expected FibonacciActionClient::getActionName() +tl::expected ExampleFibonacciActionClient::getActionName() { const auto action_name = getInput("action_name"); if (const auto error = moveit_studio::behaviors::maybe_error(action_name)) @@ -43,7 +43,7 @@ tl::expected FibonacciActionClient::getActionName() return action_name.value(); } -tl::expected FibonacciActionClient::createGoal() +tl::expected ExampleFibonacciActionClient::createGoal() { const auto order = getInput("order"); @@ -55,7 +55,7 @@ tl::expected FibonacciActionClient::createGoal() return example_interfaces::build().order(order.value()); } -tl::expected FibonacciActionClient::processResult(const std::shared_ptr result) +tl::expected ExampleFibonacciActionClient::processResult(const std::shared_ptr result) { std::stringstream stream; for (const auto& value : result->sequence) @@ -72,7 +72,7 @@ tl::expected FibonacciActionClient::processResult(const std:: return { true }; } -void FibonacciActionClient::processFeedback(const std::shared_ptr feedback) +void ExampleFibonacciActionClient::processFeedback(const std::shared_ptr feedback) { std::stringstream stream; for (const auto& value : feedback->sequence) diff --git a/src/get_string_from_topic.cpp b/src/example_get_string_from_topic.cpp similarity index 74% rename from src/get_string_from_topic.cpp rename to src/example_get_string_from_topic.cpp index 1ee63726..48495ed3 100644 --- a/src/get_string_from_topic.cpp +++ b/src/example_get_string_from_topic.cpp @@ -1,17 +1,17 @@ -#include +#include // Include the template implementation for GetMessageFromTopicBehaviorBase. #include namespace example_behaviors { -GetStringFromTopic::GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, +ExampleGetStringFromTopic::ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : GetMessageFromTopicBehaviorBase(name, config, shared_resources) { } -BT::PortsList GetStringFromTopic::providedPorts() +BT::PortsList ExampleGetStringFromTopic::providedPorts() { // This node has one input port and one output port return BT::PortsList({ @@ -20,9 +20,9 @@ BT::PortsList GetStringFromTopic::providedPorts() }); } -BT::KeyValueVector GetStringFromTopic::metadata() +BT::KeyValueVector ExampleGetStringFromTopic::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Captures a string message and makes it available on an output port." } }; } diff --git a/src/hello_world.cpp b/src/example_hello_world.cpp similarity index 55% rename from src/hello_world.cpp rename to src/example_hello_world.cpp index 596d6b95..c214c389 100644 --- a/src/hello_world.cpp +++ b/src/example_hello_world.cpp @@ -1,27 +1,27 @@ -#include +#include namespace example_behaviors { -HelloWorld::HelloWorld(const std::string& name, const BT::NodeConfiguration& config, +ExampleHelloWorld::ExampleHelloWorld(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList HelloWorld::providedPorts() +BT::PortsList ExampleHelloWorld::providedPorts() { // This node has no input or output ports return BT::PortsList({}); } -BT::KeyValueVector HelloWorld::metadata() +BT::KeyValueVector ExampleHelloWorld::metadata() { - return { { "subcategory", "Example" }, { "description", "Log a message that says \"Hello, world!\"." } }; + return { { "subcategory", "Example Behaviors" }, { "description", "Log a message that says \"Hello, world!\"." } }; } -BT::NodeStatus HelloWorld::tick() +BT::NodeStatus ExampleHelloWorld::tick() { - // Do HelloWorld's useful work. + // Do ExampleHelloWorld's useful work. // Setting the third argument to false ensures the message will be shown immediately shared_resources_->logger->publishInfoMessage(name(), "Hello, world!"); diff --git a/src/example_ndt_registration.cpp b/src/example_ndt_registration.cpp new file mode 100644 index 00000000..f8e538b5 --- /dev/null +++ b/src/example_ndt_registration.cpp @@ -0,0 +1,147 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace example_behaviors +{ +namespace +{ + +std::shared_ptr> +getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) +{ + const auto cloud = std::make_shared>(); + pcl::fromROSMsg(cloud_msg, *cloud); + + const pcl::PointIndices valid_indices = moveit_studio::selectPointIndices(*cloud, moveit_studio::point_cloud_tools::getAllIndices(cloud), + moveit_studio::NeitherNanNorNearZeroPointValidator); + auto filtered_cloud = std::make_shared>(); + pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud); + + return filtered_cloud; +} + +} // namespace + +ExampleNDTRegistration::ExampleNDTRegistration( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) +{ +} + + +BT::PortsList ExampleNDTRegistration::providedPorts() +{ + return { BT::InputPort("base_point_cloud", "{point_cloud}", + "Point cloud to align with the target point cloud."), + BT::InputPort("target_point_cloud", "{target_point_cloud}", + "Point cloud to which align the base point cloud."), + BT::InputPort("max_iterations", 30, + "Maximum number of attempts to find the transform. Setting a higher number of iterations " + "will allow the solver to converge even if the initial estimate of the transform was far " + "from the actual transform, but it may take longer to complete."), + BT::InputPort("transformation_epsilon", 0.001, + "Minimum transformation difference for termination condition "), + BT::InputPort("step_size", 0.1, + "Maximum step size for More-Thuente line search "), + BT::InputPort("resolution", 1.0, + "Resolution of NDT grid structure (VoxelGridCovariance) "), + BT::InputPort("inlier_threshold", 1.0, + "Resolution of NDT grid structure (VoxelGridCovariance) "), + BT::OutputPort("target_pose_in_base_frame", "{target_pose}", + "The pose of the target point cloud relative to the frame " + "of the base point cloud.") }; +} +BT::KeyValueVector ExampleNDTRegistration::metadata() +{ +return { {"subcategory", "Perception - 3D Point Cloud"}, {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} }; +} + + +tl::expected ExampleNDTRegistration::doWork() +{ + const auto base_point_cloud_msg = getInput("base_point_cloud"); + const auto target_point_cloud_msg = getInput("target_point_cloud"); + const auto max_iterations = getInput("max_iterations"); + const auto transformation_epsilon = getInput("transformation_epsilon"); + const auto step_size = getInput("step_size"); + const auto resolution = getInput("resolution"); + + if (const auto error = + moveit_studio::behaviors::maybe_error(base_point_cloud_msg, target_point_cloud_msg, max_iterations, transformation_epsilon, step_size, resolution); + error) + { + return tl::make_unexpected("Failed to get required value from input data port: " + error.value()); + } + + const auto base_cloud = getFilteredPointCloudFromMessage(base_point_cloud_msg.value()); + if (base_cloud->empty()) + { + return tl::make_unexpected("Base point cloud has no valid points"); + } + const auto target_cloud = getFilteredPointCloudFromMessage(target_point_cloud_msg.value()); + if (target_cloud->empty()) + { + return tl::make_unexpected("Target point cloud has no valid points"); + } + // Initializing Normal Distributions Transform (NDT). + pcl::NormalDistributionsTransform ndt; + + // Setting scale dependent NDT parameters + // Setting minimum transformation difference for termination condition. + ndt.setTransformationEpsilon (transformation_epsilon.value()); + // Setting maximum step size for More-Thuente line search. + ndt.setStepSize (step_size.value()); + //Setting Resolution of NDT grid structure (VoxelGridCovariance). + ndt.setResolution (resolution.value()); + + // Setting max number of registration iterations. + ndt.setMaximumIterations (max_iterations.value()); + + // Setting point cloud to be aligned. + ndt.setInputSource (base_cloud); + // Setting point cloud to be aligned to. + ndt.setInputTarget (target_cloud); + + + // Calculating required rigid transform to align the input cloud to the target cloud. + pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); + ndt.align (*output_cloud); + + if (!ndt.hasConverged()) + { + return tl::make_unexpected("NDT could not converge to a transform that aligns both point clouds"); + } + + geometry_msgs::msg::PoseStamped out; + out.pose = tf2::toMsg(Eigen::Isometry3d{ ndt.getFinalTransformation().cast() }); + out.header.frame_id = base_point_cloud_msg.value().header.frame_id; + out.header.stamp = base_point_cloud_msg.value().header.stamp; + setOutput("target_pose_in_base_frame", out); + + return true; +} + + +} // namespace example_behaviors diff --git a/src/publish_color_rgba.cpp b/src/example_publish_color_rgba.cpp similarity index 66% rename from src/publish_color_rgba.cpp rename to src/example_publish_color_rgba.cpp index b227746f..5a6e68ed 100644 --- a/src/publish_color_rgba.cpp +++ b/src/example_publish_color_rgba.cpp @@ -1,26 +1,26 @@ -#include +#include namespace example_behaviors { -PublishColorRGBA::PublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, +ExamplePublishColorRGBA::ExamplePublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) , publisher_{ shared_resources_->node->create_publisher("/my_topic", rclcpp::QoS(1)) } { } -BT::PortsList PublishColorRGBA::providedPorts() +BT::PortsList ExamplePublishColorRGBA::providedPorts() { return {}; } -BT::KeyValueVector PublishColorRGBA::metadata() +BT::KeyValueVector ExamplePublishColorRGBA::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Publishes a fixed std_msgs::msg::ColorRGBA message to a topic named \"/my_topic\"" } }; } -BT::NodeStatus PublishColorRGBA::tick() +BT::NodeStatus ExamplePublishColorRGBA::tick() { std_msgs::msg::ColorRGBA color_msg; color_msg.r = 0.5; diff --git a/src/example_ransac_registration.cpp b/src/example_ransac_registration.cpp new file mode 100644 index 00000000..b85d69c0 --- /dev/null +++ b/src/example_ransac_registration.cpp @@ -0,0 +1,235 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace example_behaviors +{ +namespace +{ + +std::shared_ptr> +getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) +{ + const auto cloud = std::make_shared>(); + pcl::fromROSMsg(cloud_msg, *cloud); + + const pcl::PointIndices valid_indices = moveit_studio::selectPointIndices(*cloud, moveit_studio::point_cloud_tools::getAllIndices(cloud), + moveit_studio::NeitherNanNorNearZeroPointValidator); + auto filtered_cloud = std::make_shared>(); + pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud); + + return filtered_cloud; +} + +} // namespace + +ExampleRANSACRegistration::ExampleRANSACRegistration( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) +{ +} + + +BT::PortsList ExampleRANSACRegistration::providedPorts() +{ + return { BT::InputPort("base_point_cloud", "{point_cloud}", + "Point cloud to align with the target point cloud."), + BT::InputPort("target_point_cloud", "{target_point_cloud}", + "Point cloud to which align the base point cloud."), + BT::InputPort("max_iterations", 30, + "Maximum number of attempts to find the transform. Setting a higher number of iterations " + "will allow the solver to converge even if the initial estimate of the transform was far " + "from the actual transform, but it may take longer to complete."), + BT::InputPort("transformation_epsilon", 0.001, + "Minimum transformation difference for termination condition "), + BT::InputPort("step_size", 0.1, + "Maximum step size for More-Thuente line search "), + BT::InputPort("resolution", 1.0, + "Resolution of NDT grid structure (VoxelGridCovariance) "), + BT::InputPort("inlier_threshold", 1.0, + "Inlier threshold for RANSAC "), + BT::InputPort("uniform_sampling_radius", 0.02, + "Radius for uniform sampling of keypoints "), + BT::InputPort("k_search", 10, + "Number of nearest neighbors to use for normal estimation "), + BT::InputPort("feature_radius", 0.05, + "Radius for feature estimation "), + BT::OutputPort("target_pose_in_base_frame", "{target_pose}", + "The pose of the target point cloud relative to the frame " + "of the base point cloud.") }; +} + +BT::KeyValueVector ExampleRANSACRegistration::metadata() +{ + return { {"subcategory", "Perception - 3D Point Cloud"}, {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} }; +} + + +tl::expected ExampleRANSACRegistration::doWork() +{ + + const auto base_point_cloud_msg = getInput("base_point_cloud"); + const auto target_point_cloud_msg = getInput("target_point_cloud"); + const auto max_iterations = getInput("max_iterations"); + const auto transformation_epsilon = getInput("transformation_epsilon"); + const auto inlier_threshold = getInput("inlier_threshold"); + const auto uniform_sampling_radius = getInput("uniform_sampling_radius"); + const auto k_search = getInput("k_search"); + const auto feature_radius = getInput("feature_radius"); + + if (const auto error = moveit_studio::behaviors::maybe_error(base_point_cloud_msg, target_point_cloud_msg, max_iterations, transformation_epsilon, inlier_threshold, uniform_sampling_radius, k_search, feature_radius); error) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Failed to get required value from input data port: %s", error.value().c_str()); + return tl::make_unexpected("Failed to get required value from input data port: " + error.value()); + } + + const auto base_cloud = getFilteredPointCloudFromMessage(base_point_cloud_msg.value()); + if (base_cloud->empty()) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Base point cloud has no valid points"); + return tl::make_unexpected("Base point cloud has no valid points"); + } + const auto target_cloud = getFilteredPointCloudFromMessage(target_point_cloud_msg.value()); + if (target_cloud->empty()) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Target point cloud has no valid points"); + return tl::make_unexpected("Target point cloud has no valid points"); + } + + // Estimating surface normals + pcl::PointCloud::Ptr base_normals(new pcl::PointCloud); + pcl::PointCloud::Ptr target_normals(new pcl::PointCloud); + + pcl::NormalEstimation normal_est; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + normal_est.setSearchMethod(tree); + + normal_est.setInputCloud(base_cloud); + normal_est.setKSearch(k_search.value()); + normal_est.compute(*base_normals); + + normal_est.setInputCloud(target_cloud); + normal_est.setKSearch(k_search.value()); + normal_est.compute(*target_normals); + + // Logging normals + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Computed %ld base normals and %ld target normals", base_normals->size(), target_normals->size()); + + // Keypoint selection + pcl::PointCloud::Ptr base_keypoints(new pcl::PointCloud); + pcl::PointCloud::Ptr target_keypoints(new pcl::PointCloud); + + pcl::UniformSampling uniform_sampling; + uniform_sampling.setInputCloud(base_cloud); + uniform_sampling.setRadiusSearch(uniform_sampling_radius.value()); + uniform_sampling.filter(*base_keypoints); + + uniform_sampling.setInputCloud(target_cloud); + uniform_sampling.setRadiusSearch(uniform_sampling_radius.value()); + uniform_sampling.filter(*target_keypoints); + + // Logging keypoints + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Selected %ld base keypoints and %ld target keypoints", base_keypoints->size(), target_keypoints->size()); + + // Estimating normals for keypoints + pcl::PointCloud::Ptr base_keypoint_normals(new pcl::PointCloud); + pcl::PointCloud::Ptr target_keypoint_normals(new pcl::PointCloud); + + pcl::PointIndices::Ptr base_keypoint_indices(new pcl::PointIndices); + pcl::PointIndices::Ptr target_keypoint_indices(new pcl::PointIndices); + + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(base_cloud); + for (size_t i = 0; i < base_keypoints->points.size(); ++i) + { + std::vector indices(1); + std::vector distances(1); + kdtree.nearestKSearch(base_keypoints->points[i], 1, indices, distances); + base_keypoint_indices->indices.push_back(indices[0]); + } + pcl::copyPointCloud(*base_normals, *base_keypoint_indices, *base_keypoint_normals); + + kdtree.setInputCloud(target_cloud); + for (size_t i = 0; i < target_keypoints->points.size(); ++i) + { + std::vector indices(1); + std::vector distances(1); + kdtree.nearestKSearch(target_keypoints->points[i], 1, indices, distances); + target_keypoint_indices->indices.push_back(indices[0]); + } + pcl::copyPointCloud(*target_normals, *target_keypoint_indices, *target_keypoint_normals); + + // Feature estimation + pcl::FPFHEstimation fpfh_est; + pcl::PointCloud::Ptr base_features(new pcl::PointCloud); + pcl::PointCloud::Ptr target_features(new pcl::PointCloud); + + fpfh_est.setSearchMethod(tree); + + fpfh_est.setInputCloud(base_keypoints); + fpfh_est.setInputNormals(base_keypoint_normals); + fpfh_est.setRadiusSearch(feature_radius.value()); + fpfh_est.compute(*base_features); + + fpfh_est.setInputCloud(target_keypoints); + fpfh_est.setInputNormals(target_keypoint_normals); + fpfh_est.setRadiusSearch(feature_radius.value()); + fpfh_est.compute(*target_features); + + // Logging features + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Computed %ld base features and %ld target features", base_features->size(), target_features->size()); + + // RANSAC alignment + pcl::SampleConsensusPrerejective align; + align.setInputSource(base_keypoints); + align.setSourceFeatures(base_features); + align.setInputTarget(target_keypoints); + align.setTargetFeatures(target_features); + align.setMaximumIterations(max_iterations.value()); + align.setNumberOfSamples(3); + align.setCorrespondenceRandomness(5); + align.setSimilarityThreshold(0.9f); + align.setMaxCorrespondenceDistance(inlier_threshold.value()); + align.setInlierFraction(0.25f); + + pcl::PointCloud output_cloud; + align.align(output_cloud); + + if (!align.hasConverged()) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "RANSAC could not converge to a transform that aligns both point clouds"); + return tl::make_unexpected("RANSAC could not converge to a transform that aligns both point clouds"); + } + + geometry_msgs::msg::PoseStamped out; + out.pose = tf2::toMsg(Eigen::Isometry3d{ align.getFinalTransformation().cast() }); + out.header.frame_id = base_point_cloud_msg.value().header.frame_id; + out.header.stamp = base_point_cloud_msg.value().header.stamp; + setOutput("target_pose_in_base_frame", out); + + return true; +} + + +} // namespace example_behaviors diff --git a/src/example_sam2_segmentation.cpp b/src/example_sam2_segmentation.cpp new file mode 100644 index 00000000..afa97777 --- /dev/null +++ b/src/example_sam2_segmentation.cpp @@ -0,0 +1,142 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + constexpr auto kPortImage = "image"; + constexpr auto kPortImageDefault = "{image}"; + constexpr auto kPortPoint = "pixel_coords"; + constexpr auto kPortPointDefault = "{pixel_coords}"; + constexpr auto kPortMasks = "masks2d"; + constexpr auto kPortMasksDefault = "{masks2d}"; + + constexpr auto kImageInferenceWidth = 1024; + constexpr auto kImageInferenceHeight = 1024; +} // namespace + +namespace example_behaviors +{ + // Convert a ROS image message to the ONNX image format used by the SAM 2 model + void set_onnx_image_from_ros_image(const sensor_msgs::msg::Image& image_msg, + moveit_pro_ml::ONNXImage& onnx_image) + { + onnx_image.shape = {1, image_msg.height, image_msg.width, 3}; + onnx_image.data.resize(image_msg.height * image_msg.width * 3); + const int stride = image_msg.encoding != "rgb8" ? 3: 4; + for (size_t i = 0; i < onnx_image.data.size(); i+=stride) + { + onnx_image.data[i] = static_cast(image_msg.data[i]) / 255.0f; + onnx_image.data[i+1] = static_cast(image_msg.data[i+1]) / 255.0f; + onnx_image.data[i+2] = static_cast(image_msg.data[i+2]) / 255.0f; + } + } + + // Converts a single channel ONNX image mask to a ROS mask message. + void set_ros_mask_from_onnx_mask(const moveit_pro_ml::ONNXImage& onnx_image, sensor_msgs::msg::Image& mask_image_msg, moveit_studio_vision_msgs::msg::Mask2D& mask_msg) + { + mask_image_msg.height = static_cast(onnx_image.shape[0]); + mask_image_msg.width = static_cast(onnx_image.shape[1]); + mask_image_msg.encoding = "mono8"; + mask_image_msg.data.resize(mask_image_msg.height * mask_image_msg.width); + mask_image_msg.step = mask_image_msg.width; + for (size_t i = 0; i < onnx_image.data.size(); ++i) + { + mask_image_msg.data[i] = onnx_image.data[i] > 0.5 ? 255: 0; + } + mask_msg.pixels = mask_image_msg; + mask_msg.x = 0; + mask_msg.y = 0; + } + + ExampleSAM2Segmentation::ExampleSAM2Segmentation(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) + { + + const std::filesystem::path package_path = ament_index_cpp::get_package_share_directory("example_behaviors"); + const std::filesystem::path encoder_onnx_file = package_path / "models" / "sam2_hiera_large_encoder.onnx"; + const std::filesystem::path decoder_onnx_file = package_path / "models" / "decoder.onnx"; + sam2_ = std::make_unique(encoder_onnx_file, decoder_onnx_file); + } + + BT::PortsList ExampleSAM2Segmentation::providedPorts() + { + return { + BT::InputPort(kPortImage, kPortImageDefault, + "The Image to run segmentation on."), + BT::InputPort>(kPortPoint, kPortPointDefault, + "The input points, as a vector of geometry_msgs/PointStamped messages to be used for segmentation."), + + BT::OutputPort>(kPortMasks, kPortMasksDefault, + "The masks contained in a vector of moveit_studio_vision_msgs::msg::Mask2D messages.") + }; + } + + tl::expected ExampleSAM2Segmentation::doWork() + { + const auto ports = moveit_studio::behaviors::getRequiredInputs(getInput(kPortImage), + getInput>(kPortPoint)); + + // Check that all required input data ports were set. + if (!ports.has_value()) + { + auto error_message = fmt::format("Failed to get required values from input data ports:\n{}", ports.error()); + return tl::make_unexpected(error_message); + } + const auto& [image_msg, points_2d] = ports.value(); + + if (image_msg.encoding != "rgb8" && image_msg.encoding != "rgba8") + { + auto error_message = fmt::format("Invalid image message format. Expected `(rgb8, rgba8)` got :\n{}", image_msg.encoding); + return tl::make_unexpected(error_message); + } + + // Create ONNX formatted image tensor from ROS image + set_onnx_image_from_ros_image(image_msg, onnx_image_); + + std::vector point_prompts; + for (auto const& point : points_2d) + { + // Assume all points are the same label + point_prompts.push_back({{kImageInferenceWidth*static_cast(point.point.x), kImageInferenceHeight*static_cast(point.point.y)}, {1.0f}}); + } + + try + { + const auto masks = sam2_->predict(onnx_image_, point_prompts); + + mask_image_msg_.header = image_msg.header; + set_ros_mask_from_onnx_mask(masks, mask_image_msg_, mask_msg_); + + setOutput>(kPortMasks, {mask_msg_}); + } + catch (const std::invalid_argument& e) + { + return tl::make_unexpected(fmt::format("Invalid argument: {}", e.what())); + } + + return true; + } + + BT::KeyValueVector ExampleSAM2Segmentation::metadata() + { + return { + { + "description", + "Segments a ROS image message using the provided points represented as a vector of geometry_msgs/PointStamped messages." + } + }; + } +} // namespace example_behaviors diff --git a/src/setup_mtc_pick_from_pose.cpp b/src/example_setup_mtc_pick_from_pose.cpp similarity index 96% rename from src/setup_mtc_pick_from_pose.cpp rename to src/example_setup_mtc_pick_from_pose.cpp index f83267f9..67f56007 100644 --- a/src/setup_mtc_pick_from_pose.cpp +++ b/src/example_setup_mtc_pick_from_pose.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -14,7 +14,7 @@ namespace { -const auto kLogger = rclcpp::get_logger("SetupMTCPickFromPose"); +const auto kLogger = rclcpp::get_logger("ExampleSetupMtcPickFromPose"); using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes; // Port names for input and output ports. @@ -37,14 +37,14 @@ constexpr auto kSceneObjectNameOctomap = ""; namespace example_behaviors { -SetupMtcPickFromPose::SetupMtcPickFromPose( +ExampleSetupMtcPickFromPose::ExampleSetupMtcPickFromPose( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList SetupMtcPickFromPose::providedPorts() +BT::PortsList ExampleSetupMtcPickFromPose::providedPorts() { return { BT::BidirectionalPort(kPortIDTask, "{mtc_task}", "MoveIt Task Constructor task."), @@ -53,13 +53,13 @@ BT::PortsList SetupMtcPickFromPose::providedPorts() }; } -BT::KeyValueVector SetupMtcPickFromPose::metadata() +BT::KeyValueVector ExampleSetupMtcPickFromPose::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Adds the stages to describe a pick motion to the MTC task." } }; } -BT::NodeStatus SetupMtcPickFromPose::tick() +BT::NodeStatus ExampleSetupMtcPickFromPose::tick() { using namespace moveit_studio::behaviors; diff --git a/src/setup_mtc_place_from_pose.cpp b/src/example_setup_mtc_place_from_pose.cpp similarity index 95% rename from src/setup_mtc_place_from_pose.cpp rename to src/example_setup_mtc_place_from_pose.cpp index dd24d5ba..3f5e44ae 100644 --- a/src/setup_mtc_place_from_pose.cpp +++ b/src/example_setup_mtc_place_from_pose.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -14,7 +14,7 @@ namespace { -const auto kLogger = rclcpp::get_logger("SetupMTCPickFromPose"); +const auto kLogger = rclcpp::get_logger("ExampleSetupMtcPickFromPose"); using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes; // Port names for input and output ports. @@ -38,14 +38,14 @@ constexpr auto kSceneObjectNameOctomap = ""; namespace example_behaviors { -SetupMtcPlaceFromPose::SetupMtcPlaceFromPose( +ExampleSetupMtcPlaceFromPose::ExampleSetupMtcPlaceFromPose( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) { } -BT::PortsList SetupMtcPlaceFromPose::providedPorts() +BT::PortsList ExampleSetupMtcPlaceFromPose::providedPorts() { return { BT::BidirectionalPort(kPortIDTask, "{mtc_task}", "MoveIt Task Constructor task."), @@ -54,13 +54,13 @@ BT::PortsList SetupMtcPlaceFromPose::providedPorts() }; } -BT::KeyValueVector SetupMtcPlaceFromPose::metadata() +BT::KeyValueVector ExampleSetupMtcPlaceFromPose::metadata() { - return { { "subcategory", "Example" }, + return { { "subcategory", "Example Behaviors" }, { "description", "Adds the stages to describe a place motion to the MTC task." } }; } -BT::NodeStatus SetupMtcPlaceFromPose::tick() +BT::NodeStatus ExampleSetupMtcPlaceFromPose::tick() { using namespace moveit_studio::behaviors; diff --git a/src/setup_mtc_wave_hand.cpp b/src/example_setup_mtc_wave_hand.cpp similarity index 90% rename from src/setup_mtc_wave_hand.cpp rename to src/example_setup_mtc_wave_hand.cpp index 3f00581c..5867b1b0 100644 --- a/src/setup_mtc_wave_hand.cpp +++ b/src/example_setup_mtc_wave_hand.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -9,7 +9,7 @@ namespace // Define a constant for the ID of the Behavior's data port. constexpr auto kPortIDTask = "task"; -// Define constants for the names of the behavior parameters used by the SetupMTCWaveHand behavior. +// Define constants for the names of the behavior parameters used by the ExampleSetupMTCWaveHand behavior. // These defaults are set to match the UR5e robot defined in the moveit_studio_custom_site_config_example package. constexpr auto kPrimaryGroupName = "manipulator"; constexpr auto kHandFrameName = "grasp_link"; @@ -20,13 +20,13 @@ const rclcpp::Logger kLogger = rclcpp::get_logger("WaveHello"); namespace example_behaviors { -SetupMTCWaveHand::SetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, +ExampleSetupMTCWaveHand::ExampleSetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) { } -BT::PortsList SetupMTCWaveHand::providedPorts() +BT::PortsList ExampleSetupMTCWaveHand::providedPorts() { return { BT::BidirectionalPort>(kPortIDTask, "{mtc_task}", @@ -34,12 +34,12 @@ BT::PortsList SetupMTCWaveHand::providedPorts() }; } -BT::KeyValueVector SetupMTCWaveHand::metadata() +BT::KeyValueVector ExampleSetupMTCWaveHand::metadata() { - return { { "subcategory", "Example" }, { "description", "Wave hello with the end effector." } }; + return { { "subcategory", "Example Behaviors" }, { "description", "Wave hello with the end effector." } }; } -tl::expected SetupMTCWaveHand::doWork() +tl::expected ExampleSetupMTCWaveHand::doWork() { // Retrieve the "task" data port const auto task = getInput(kPortIDTask); diff --git a/src/ndt_registration.cpp b/src/ndt_registration.cpp new file mode 100644 index 00000000..3b8c6e63 --- /dev/null +++ b/src/ndt_registration.cpp @@ -0,0 +1,147 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace example_behaviors +{ +namespace +{ + +std::shared_ptr> +getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) +{ + const auto cloud = std::make_shared>(); + pcl::fromROSMsg(cloud_msg, *cloud); + + const pcl::PointIndices valid_indices = moveit_studio::selectPointIndices(*cloud, moveit_studio::point_cloud_tools::getAllIndices(cloud), + moveit_studio::NeitherNanNorNearZeroPointValidator); + auto filtered_cloud = std::make_shared>(); + pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud); + + return filtered_cloud; +} + +} // namespace + +NDTRegistration::NDTRegistration( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) +{ +} + + +BT::PortsList NDTRegistration::providedPorts() +{ + return { BT::InputPort("base_point_cloud", "{point_cloud}", + "Point cloud to align with the target point cloud."), + BT::InputPort("target_point_cloud", "{target_point_cloud}", + "Point cloud to which align the base point cloud."), + BT::InputPort("max_iterations", 30, + "Maximum number of attempts to find the transform. Setting a higher number of iterations " + "will allow the solver to converge even if the initial estimate of the transform was far " + "from the actual transform, but it may take longer to complete."), + BT::InputPort("transformation_epsilon", 0.001, + "Minimum transformation difference for termination condition "), + BT::InputPort("step_size", 0.1, + "Maximum step size for More-Thuente line search "), + BT::InputPort("resolution", 1.0, + "Resolution of NDT grid structure (VoxelGridCovariance) "), + BT::InputPort("inlier_threshold", 1.0, + "Resolution of NDT grid structure (VoxelGridCovariance) "), + BT::OutputPort("target_pose_in_base_frame", "{target_pose}", + "The pose of the target point cloud relative to the frame " + "of the base point cloud.") }; +} +BT::KeyValueVector NDTRegistration::metadata() +{ +return { {"subcategory", "Perception - 3D Point Cloud"}, {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} }; +} + + +tl::expected NDTRegistration::doWork() +{ + const auto base_point_cloud_msg = getInput("base_point_cloud"); + const auto target_point_cloud_msg = getInput("target_point_cloud"); + const auto max_iterations = getInput("max_iterations"); + const auto transformation_epsilon = getInput("transformation_epsilon"); + const auto step_size = getInput("step_size"); + const auto resolution = getInput("resolution"); + + if (const auto error = + moveit_studio::behaviors::maybe_error(base_point_cloud_msg, target_point_cloud_msg, max_iterations, transformation_epsilon, step_size, resolution); + error) + { + return tl::make_unexpected("Failed to get required value from input data port: " + error.value()); + } + + const auto base_cloud = getFilteredPointCloudFromMessage(base_point_cloud_msg.value()); + if (base_cloud->empty()) + { + return tl::make_unexpected("Base point cloud has no valid points"); + } + const auto target_cloud = getFilteredPointCloudFromMessage(target_point_cloud_msg.value()); + if (target_cloud->empty()) + { + return tl::make_unexpected("Target point cloud has no valid points"); + } + // Initializing Normal Distributions Transform (NDT). + pcl::NormalDistributionsTransform ndt; + + // Setting scale dependent NDT parameters + // Setting minimum transformation difference for termination condition. + ndt.setTransformationEpsilon (transformation_epsilon.value()); + // Setting maximum step size for More-Thuente line search. + ndt.setStepSize (step_size.value()); + //Setting Resolution of NDT grid structure (VoxelGridCovariance). + ndt.setResolution (resolution.value()); + + // Setting max number of registration iterations. + ndt.setMaximumIterations (max_iterations.value()); + + // Setting point cloud to be aligned. + ndt.setInputSource (base_cloud); + // Setting point cloud to be aligned to. + ndt.setInputTarget (target_cloud); + + + // Calculating required rigid transform to align the input cloud to the target cloud. + pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); + ndt.align (*output_cloud); + + if (!ndt.hasConverged()) + { + return tl::make_unexpected("NDT could not converge to a transform that aligns both point clouds"); + } + + geometry_msgs::msg::PoseStamped out; + out.pose = tf2::toMsg(Eigen::Isometry3d{ ndt.getFinalTransformation().cast() }); + out.header.frame_id = base_point_cloud_msg.value().header.frame_id; + out.header.stamp = base_point_cloud_msg.value().header.stamp; + setOutput("target_pose_in_base_frame", out); + + return true; +} + + +} // namespace example_behaviors diff --git a/src/ransac_registration.cpp b/src/ransac_registration.cpp new file mode 100644 index 00000000..c25f81ea --- /dev/null +++ b/src/ransac_registration.cpp @@ -0,0 +1,235 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace example_behaviors +{ +namespace +{ + +std::shared_ptr> +getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) +{ + const auto cloud = std::make_shared>(); + pcl::fromROSMsg(cloud_msg, *cloud); + + const pcl::PointIndices valid_indices = moveit_studio::selectPointIndices(*cloud, moveit_studio::point_cloud_tools::getAllIndices(cloud), + moveit_studio::NeitherNanNorNearZeroPointValidator); + auto filtered_cloud = std::make_shared>(); + pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud); + + return filtered_cloud; +} + +} // namespace + +RANSACRegistration::RANSACRegistration( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) +{ +} + + +BT::PortsList RANSACRegistration::providedPorts() +{ + return { BT::InputPort("base_point_cloud", "{point_cloud}", + "Point cloud to align with the target point cloud."), + BT::InputPort("target_point_cloud", "{target_point_cloud}", + "Point cloud to which align the base point cloud."), + BT::InputPort("max_iterations", 30, + "Maximum number of attempts to find the transform. Setting a higher number of iterations " + "will allow the solver to converge even if the initial estimate of the transform was far " + "from the actual transform, but it may take longer to complete."), + BT::InputPort("transformation_epsilon", 0.001, + "Minimum transformation difference for termination condition "), + BT::InputPort("step_size", 0.1, + "Maximum step size for More-Thuente line search "), + BT::InputPort("resolution", 1.0, + "Resolution of NDT grid structure (VoxelGridCovariance) "), + BT::InputPort("inlier_threshold", 1.0, + "Inlier threshold for RANSAC "), + BT::InputPort("uniform_sampling_radius", 0.02, + "Radius for uniform sampling of keypoints "), + BT::InputPort("k_search", 10, + "Number of nearest neighbors to use for normal estimation "), + BT::InputPort("feature_radius", 0.05, + "Radius for feature estimation "), + BT::OutputPort("target_pose_in_base_frame", "{target_pose}", + "The pose of the target point cloud relative to the frame " + "of the base point cloud.") }; +} + +BT::KeyValueVector RANSACRegistration::metadata() +{ + return { {"subcategory", "Perception - 3D Point Cloud"}, {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} }; +} + + +tl::expected RANSACRegistration::doWork() +{ + + const auto base_point_cloud_msg = getInput("base_point_cloud"); + const auto target_point_cloud_msg = getInput("target_point_cloud"); + const auto max_iterations = getInput("max_iterations"); + const auto transformation_epsilon = getInput("transformation_epsilon"); + const auto inlier_threshold = getInput("inlier_threshold"); + const auto uniform_sampling_radius = getInput("uniform_sampling_radius"); + const auto k_search = getInput("k_search"); + const auto feature_radius = getInput("feature_radius"); + + if (const auto error = moveit_studio::behaviors::maybe_error(base_point_cloud_msg, target_point_cloud_msg, max_iterations, transformation_epsilon, inlier_threshold, uniform_sampling_radius, k_search, feature_radius); error) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Failed to get required value from input data port: %s", error.value().c_str()); + return tl::make_unexpected("Failed to get required value from input data port: " + error.value()); + } + + const auto base_cloud = getFilteredPointCloudFromMessage(base_point_cloud_msg.value()); + if (base_cloud->empty()) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Base point cloud has no valid points"); + return tl::make_unexpected("Base point cloud has no valid points"); + } + const auto target_cloud = getFilteredPointCloudFromMessage(target_point_cloud_msg.value()); + if (target_cloud->empty()) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Target point cloud has no valid points"); + return tl::make_unexpected("Target point cloud has no valid points"); + } + + // Estimating surface normals + pcl::PointCloud::Ptr base_normals(new pcl::PointCloud); + pcl::PointCloud::Ptr target_normals(new pcl::PointCloud); + + pcl::NormalEstimation normal_est; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + normal_est.setSearchMethod(tree); + + normal_est.setInputCloud(base_cloud); + normal_est.setKSearch(k_search.value()); + normal_est.compute(*base_normals); + + normal_est.setInputCloud(target_cloud); + normal_est.setKSearch(k_search.value()); + normal_est.compute(*target_normals); + + // Logging normals + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Computed %ld base normals and %ld target normals", base_normals->size(), target_normals->size()); + + // Keypoint selection + pcl::PointCloud::Ptr base_keypoints(new pcl::PointCloud); + pcl::PointCloud::Ptr target_keypoints(new pcl::PointCloud); + + pcl::UniformSampling uniform_sampling; + uniform_sampling.setInputCloud(base_cloud); + uniform_sampling.setRadiusSearch(uniform_sampling_radius.value()); + uniform_sampling.filter(*base_keypoints); + + uniform_sampling.setInputCloud(target_cloud); + uniform_sampling.setRadiusSearch(uniform_sampling_radius.value()); + uniform_sampling.filter(*target_keypoints); + + // Logging keypoints + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Selected %ld base keypoints and %ld target keypoints", base_keypoints->size(), target_keypoints->size()); + + // Estimating normals for keypoints + pcl::PointCloud::Ptr base_keypoint_normals(new pcl::PointCloud); + pcl::PointCloud::Ptr target_keypoint_normals(new pcl::PointCloud); + + pcl::PointIndices::Ptr base_keypoint_indices(new pcl::PointIndices); + pcl::PointIndices::Ptr target_keypoint_indices(new pcl::PointIndices); + + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(base_cloud); + for (size_t i = 0; i < base_keypoints->points.size(); ++i) + { + std::vector indices(1); + std::vector distances(1); + kdtree.nearestKSearch(base_keypoints->points[i], 1, indices, distances); + base_keypoint_indices->indices.push_back(indices[0]); + } + pcl::copyPointCloud(*base_normals, *base_keypoint_indices, *base_keypoint_normals); + + kdtree.setInputCloud(target_cloud); + for (size_t i = 0; i < target_keypoints->points.size(); ++i) + { + std::vector indices(1); + std::vector distances(1); + kdtree.nearestKSearch(target_keypoints->points[i], 1, indices, distances); + target_keypoint_indices->indices.push_back(indices[0]); + } + pcl::copyPointCloud(*target_normals, *target_keypoint_indices, *target_keypoint_normals); + + // Feature estimation + pcl::FPFHEstimation fpfh_est; + pcl::PointCloud::Ptr base_features(new pcl::PointCloud); + pcl::PointCloud::Ptr target_features(new pcl::PointCloud); + + fpfh_est.setSearchMethod(tree); + + fpfh_est.setInputCloud(base_keypoints); + fpfh_est.setInputNormals(base_keypoint_normals); + fpfh_est.setRadiusSearch(feature_radius.value()); + fpfh_est.compute(*base_features); + + fpfh_est.setInputCloud(target_keypoints); + fpfh_est.setInputNormals(target_keypoint_normals); + fpfh_est.setRadiusSearch(feature_radius.value()); + fpfh_est.compute(*target_features); + + // Logging features + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "Computed %ld base features and %ld target features", base_features->size(), target_features->size()); + + // RANSAC alignment + pcl::SampleConsensusPrerejective align; + align.setInputSource(base_keypoints); + align.setSourceFeatures(base_features); + align.setInputTarget(target_keypoints); + align.setTargetFeatures(target_features); + align.setMaximumIterations(max_iterations.value()); + align.setNumberOfSamples(3); + align.setCorrespondenceRandomness(5); + align.setSimilarityThreshold(0.9f); + align.setMaxCorrespondenceDistance(inlier_threshold.value()); + align.setInlierFraction(0.25f); + + pcl::PointCloud output_cloud; + align.align(output_cloud); + + if (!align.hasConverged()) + { + RCLCPP_ERROR(rclcpp::get_logger("Logger"), "RANSAC could not converge to a transform that aligns both point clouds"); + return tl::make_unexpected("RANSAC could not converge to a transform that aligns both point clouds"); + } + + geometry_msgs::msg::PoseStamped out; + out.pose = tf2::toMsg(Eigen::Isometry3d{ align.getFinalTransformation().cast() }); + out.header.frame_id = base_point_cloud_msg.value().header.frame_id; + out.header.stamp = base_point_cloud_msg.value().header.stamp; + setOutput("target_pose_in_base_frame", out); + + return true; +} + + +} // namespace example_behaviors diff --git a/src/register_behaviors.cpp b/src/register_behaviors.cpp index d2aa6076..e95a4c03 100644 --- a/src/register_behaviors.cpp +++ b/src/register_behaviors.cpp @@ -2,15 +2,19 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -22,18 +26,22 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN void registerBehaviors(BT::BehaviorTreeFactory& factory, const std::shared_ptr& shared_resources) override { - moveit_studio::behaviors::registerBehavior(factory, "HelloWorld", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "DelayedMessage", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SetupMTCWaveHand", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "GetStringFromTopic", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "AddTwoIntsServiceClient", + moveit_studio::behaviors::registerBehavior(factory, "ExampleHelloWorld", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleConvertMtcSolutionToJointTrajectory", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleDelayedMessage", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSetupMTCWaveHand", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleGetStringFromTopic", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleAddTwoIntsServiceClient", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "FibonacciActionClient", + moveit_studio::behaviors::registerBehavior(factory, "ExampleFibonacciActionClient", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "PublishColorRGBA", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SetupMtcPickFromPose", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SetupMtcPlaceFromPose", + moveit_studio::behaviors::registerBehavior(factory, "ExamplePublishColorRGBA", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSetupMtcPickFromPose", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSetupMtcPlaceFromPose", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleNDTRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleRANSACRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleSAM2Segmentation", shared_resources); } }; } // namespace example_behaviors diff --git a/test/test_behavior_plugins.cpp b/test/test_behavior_plugins.cpp index 4fbf18fd..d23a5b4e 100644 --- a/test/test_behavior_plugins.cpp +++ b/test/test_behavior_plugins.cpp @@ -24,19 +24,25 @@ TEST(BehaviorTests, test_load_behavior_plugins) } // 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", "AddTwoIntsServiceClient", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "DelayedMessage", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleAddTwoIntsServiceClient", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExampleDelayedMessage", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "FibonacciActionClient", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleFibonacciActionClient", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "GetStringFromTopic", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "HelloWorld", BT::NodeConfiguration())); - EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "PublishColorRGBA", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleGetStringFromTopic", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExampleHelloWorld", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExamplePublishColorRGBA", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "SetupMtcPickFromPose", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleSetupMtcPickFromPose", 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())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleSetupMtcPlaceFromPose", BT::NodeConfiguration())); + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ExampleSetupMTCWaveHand", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleNDTRegistration", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleRANSACRegistration", BT::NodeConfiguration())); + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleSAM2Segmentation", BT::NodeConfiguration())); } int main(int argc, char** argv)