From 0b2604922e0cf6dc61533cce4590aa6009dbabc7 Mon Sep 17 00:00:00 2001 From: mewtwoshaurd Date: Wed, 11 Dec 2024 11:21:59 -0700 Subject: [PATCH 1/7] feat: mock behavior for pointclouds to poses --- src/graspnet_example_behavior/CMakeLists.txt | 48 ++++++++++ .../behavior_plugin.yaml | 4 + .../config/tree_nodes_model.xml | 6 ++ ...et_example_behavior_plugin_description.xml | 7 ++ .../graspnet_example_behavior.hpp | 49 ++++++++++ src/graspnet_example_behavior/package.xml | 29 ++++++ .../src/graspnet_example_behavior.cpp | 93 +++++++++++++++++++ .../src/register_behaviors.cpp | 24 +++++ .../test/CMakeLists.txt | 4 + .../test/test_behavior_plugins.cpp | 37 ++++++++ .../objectives/take_snapshots_of_object.xml | 20 ++++ 11 files changed, 321 insertions(+) create mode 100644 src/graspnet_example_behavior/CMakeLists.txt create mode 100644 src/graspnet_example_behavior/behavior_plugin.yaml create mode 100644 src/graspnet_example_behavior/config/tree_nodes_model.xml create mode 100644 src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml create mode 100644 src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp create mode 100644 src/graspnet_example_behavior/package.xml create mode 100644 src/graspnet_example_behavior/src/graspnet_example_behavior.cpp create mode 100644 src/graspnet_example_behavior/src/register_behaviors.cpp create mode 100644 src/graspnet_example_behavior/test/CMakeLists.txt create mode 100644 src/graspnet_example_behavior/test/test_behavior_plugins.cpp create mode 100644 src/lab_sim/objectives/take_snapshots_of_object.xml diff --git a/src/graspnet_example_behavior/CMakeLists.txt b/src/graspnet_example_behavior/CMakeLists.txt new file mode 100644 index 00000000..6edb107c --- /dev/null +++ b/src/graspnet_example_behavior/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.22) +project(graspnet_example_behavior CXX) + +find_package(moveit_studio_common REQUIRED) +moveit_studio_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + graspnet_example_behavior + SHARED + src/graspnet_example_behavior.cpp + src/register_behaviors.cpp) +target_include_directories( + graspnet_example_behavior + PUBLIC $ + $) +ament_target_dependencies(graspnet_example_behavior + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS graspnet_example_behavior + EXPORT graspnet_example_behaviorTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + moveit_pro_behavior_test(graspnet_example_behavior) +endif() + +# Export the behavior plugins defined in this package so they are available to +# plugin loaders that load the behavior base class library from the +# moveit_studio_behavior package. +pluginlib_export_plugin_description_file( + moveit_studio_behavior_interface graspnet_example_behavior_plugin_description.xml) + +ament_export_targets(graspnet_example_behaviorTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/graspnet_example_behavior/behavior_plugin.yaml b/src/graspnet_example_behavior/behavior_plugin.yaml new file mode 100644 index 00000000..921ac6b8 --- /dev/null +++ b/src/graspnet_example_behavior/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + graspnet_example_behavior: + - "graspnet_example_behavior::GraspnetExampleBehaviorBehaviorsLoader" diff --git a/src/graspnet_example_behavior/config/tree_nodes_model.xml b/src/graspnet_example_behavior/config/tree_nodes_model.xml new file mode 100644 index 00000000..f3844c22 --- /dev/null +++ b/src/graspnet_example_behavior/config/tree_nodes_model.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml b/src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml new file mode 100644 index 00000000..94a0e625 --- /dev/null +++ b/src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp b/src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp new file mode 100644 index 00000000..ea67df15 --- /dev/null +++ b/src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include + +// This header includes the SharedResourcesNode type +#include +#include +#include + +namespace graspnet_example_behavior +{ +/** + * @brief TODO(...) + */ +class GraspnetExampleBehavior : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the graspnet_example_behavior 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. + */ + GraspnetExampleBehavior(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); + + /** + * @brief Implementation of the required providedPorts() function for the graspnet_example_behavior 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 graspnet_example_behavior does not use expose any ports, so this function returns an empty list. + */ + static BT::PortsList providedPorts(); + + /** + * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and + * subcategory, in the MoveIt Studio Developer Tool. + * @return A BT::KeyValueVector containing the Behavior metadata. + */ + static BT::KeyValueVector metadata(); + + /** + * @brief Implementation of BT::SyncActionNode::tick() for GraspnetExampleBehavior. + * @details This function is where the Behavior performs its work when the behavior tree is being run. Since GraspnetExampleBehavior is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly. + */ + BT::NodeStatus tick() override; + +}; +} // namespace graspnet_example_behavior diff --git a/src/graspnet_example_behavior/package.xml b/src/graspnet_example_behavior/package.xml new file mode 100644 index 00000000..01553d07 --- /dev/null +++ b/src/graspnet_example_behavior/package.xml @@ -0,0 +1,29 @@ + + + graspnet_example_behavior + 0.0.0 + + Takes PointClouds of an object and generates a Grasp Pose from GraspNet then + executes the grasping. + + + John Doe + John Doe + + TODO + + ament_cmake + + moveit_studio_common + + moveit_studio_behavior_interface + + ament_lint_auto + ament_cmake_gtest + ament_clang_format + ament_clang_tidy + + + ament_cmake + + diff --git a/src/graspnet_example_behavior/src/graspnet_example_behavior.cpp b/src/graspnet_example_behavior/src/graspnet_example_behavior.cpp new file mode 100644 index 00000000..1056f7ef --- /dev/null +++ b/src/graspnet_example_behavior/src/graspnet_example_behavior.cpp @@ -0,0 +1,93 @@ +#include + +#include +#include +#include + +namespace +{ + inline constexpr auto kDescriptionGraspNetExampleBehavior = R"( +

+ To do +

+ )"; + constexpr auto kPortIDPointCloud = "point_cloud"; + constexpr auto kPortIDPCDTopicName = "pcd_topic"; + constexpr auto kPortIDStampedPose = "stamped_pose"; + constexpr auto kPortIDNumSamples = "num_samples"; +} //namespace + +namespace graspnet_example_behavior +{ +GraspnetExampleBehavior::GraspnetExampleBehavior( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + + +BT::PortsList GraspnetExampleBehavior::providedPorts() +{ + // TODO(...) + return { BT::InputPort(kPortIDPCDTopicName, "/pcd_pointcloud_captures", + "Topic the pcd formatted point cloud is published to."), + BT::InputPort(kPortIDPointCloud, "{point_cloud}", + "Point cloud in sensor_msgs::msg::PointCloud2 format."), + BT::InputPort(kPortIDNumSamples, "{num_samples}", "Number of samples (poses) to take from GraspNet."), + BT::OutputPort>(kPortIDStampedPose, "{pose_stamped_vector}", + "The geometry_msgs/PoseStamped message output.") }; +} + +BT::KeyValueVector GraspnetExampleBehavior::metadata() +{ + + return { { "subcategory", "Grasping"}, {"description", kDescriptionGraspNetExampleBehavior} }; +} + +BT::NodeStatus GraspnetExampleBehavior::tick() +{ + using namespace moveit_studio::behaviors; + // Return SUCCESS once the work has been completed. + const auto ports = getRequiredInputs(getInput(kPortIDPCDTopicName), + getInput(kPortIDPointCloud), + getInput(kPortIDNumSamples)); + + if (!ports.has_value()) + { + shared_resources_->logger->publishFailureMessage(name(), "Failed to get required value from input data port: " + + ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& [pc_topic, pointcloud, num_samples] = ports.value(); + std::vector pose_stamped_vector; + + std::random_device r; + std::mt19937 el(r()); + std::uniform_real_distribution uniform_dist(-0.75, 0.75); + + geometry_msgs::msg::PoseStamped to_edit; + for (int i=0; inode->now(); + + to_edit.pose.position.x = uniform_dist(el); + to_edit.pose.position.y = uniform_dist(el); + to_edit.pose.position.z = uniform_dist(el); + + to_edit.pose.orientation.x = 0.0; + to_edit.pose.orientation.y = 0.0; + to_edit.pose.orientation.z = 0.0; + to_edit.pose.orientation.w = 1.0; + + pose_stamped_vector.push_back(to_edit); + } + + setOutput(kPortIDStampedPose, pose_stamped_vector); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace graspnet_example_behavior diff --git a/src/graspnet_example_behavior/src/register_behaviors.cpp b/src/graspnet_example_behavior/src/register_behaviors.cpp new file mode 100644 index 00000000..a1e9fd20 --- /dev/null +++ b/src/graspnet_example_behavior/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace graspnet_example_behavior +{ +class GraspnetExampleBehaviorBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + [[maybe_unused]] const std::shared_ptr& shared_resources) override + { + moveit_studio::behaviors::registerBehavior(factory, "GraspnetExampleBehavior", shared_resources); + + } +}; +} // namespace graspnet_example_behavior + +PLUGINLIB_EXPORT_CLASS(graspnet_example_behavior::GraspnetExampleBehaviorBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/graspnet_example_behavior/test/CMakeLists.txt b/src/graspnet_example_behavior/test/CMakeLists.txt new file mode 100644 index 00000000..8db41205 --- /dev/null +++ b/src/graspnet_example_behavior/test/CMakeLists.txt @@ -0,0 +1,4 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/graspnet_example_behavior/test/test_behavior_plugins.cpp b/src/graspnet_example_behavior/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..6add9c68 --- /dev/null +++ b/src/graspnet_example_behavior/test/test_behavior_plugins.cpp @@ -0,0 +1,37 @@ +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("BehaviorTests"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("graspnet_example_behavior::GraspnetExampleBehaviorBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + + // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. + EXPECT_NO_THROW( + (void)factory.instantiateTreeNode("test_behavior_name", "GraspnetExampleBehavior", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/lab_sim/objectives/take_snapshots_of_object.xml b/src/lab_sim/objectives/take_snapshots_of_object.xml new file mode 100644 index 00000000..c2ab4778 --- /dev/null +++ b/src/lab_sim/objectives/take_snapshots_of_object.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + From 2108dae70b50447a964a8a2e331a5e68969ed5d4 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 11 Dec 2024 11:28:16 -0700 Subject: [PATCH 2/7] Move bottle to front of desk in lab_sim --- src/picknik_accessories | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/picknik_accessories b/src/picknik_accessories index 6ddf6570..474919a9 160000 --- a/src/picknik_accessories +++ b/src/picknik_accessories @@ -1 +1 @@ -Subproject commit 6ddf657050f678b9eac282ade0940d827a8ea166 +Subproject commit 474919a991bbff9c8c66e1b1ffb5bc782673cec8 From 60025a52d543babf3faebfca2921843ae18c3216 Mon Sep 17 00:00:00 2001 From: David Yackzan Date: Wed, 11 Dec 2024 12:00:18 -0700 Subject: [PATCH 3/7] Move bottle to front of desk in lab_sim try 2 --- src/lab_sim/description/scene.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lab_sim/description/scene.xml b/src/lab_sim/description/scene.xml index 9e7726e7..12e33db9 100644 --- a/src/lab_sim/description/scene.xml +++ b/src/lab_sim/description/scene.xml @@ -72,10 +72,11 @@ + + /> From 9efa80467961e6d72f1372d1790491987eba64c8 Mon Sep 17 00:00:00 2001 From: mewtwoshaurd Date: Wed, 11 Dec 2024 17:14:33 -0700 Subject: [PATCH 4/7] feat: integration testing taking snapshots and visualizing poses --- src/lab_sim/objectives/integration_test.xml | 31 +++++++++++++ .../take_wrist_camera_snapshot_output.xml | 31 +++++++++++++ .../objectives/vizualize_pose_vector.xml | 44 +++++++++++++++++++ 3 files changed, 106 insertions(+) create mode 100644 src/lab_sim/objectives/integration_test.xml create mode 100644 src/lab_sim/objectives/take_wrist_camera_snapshot_output.xml create mode 100644 src/lab_sim/objectives/vizualize_pose_vector.xml diff --git a/src/lab_sim/objectives/integration_test.xml b/src/lab_sim/objectives/integration_test.xml new file mode 100644 index 00000000..44feab58 --- /dev/null +++ b/src/lab_sim/objectives/integration_test.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/take_wrist_camera_snapshot_output.xml b/src/lab_sim/objectives/take_wrist_camera_snapshot_output.xml new file mode 100644 index 00000000..18dce65b --- /dev/null +++ b/src/lab_sim/objectives/take_wrist_camera_snapshot_output.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/vizualize_pose_vector.xml b/src/lab_sim/objectives/vizualize_pose_vector.xml new file mode 100644 index 00000000..530c0262 --- /dev/null +++ b/src/lab_sim/objectives/vizualize_pose_vector.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0deb0d97bf5699a3aec170e9c51cdf8f4e7312a6 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 11 Dec 2024 19:40:37 -0700 Subject: [PATCH 5/7] add model and behavior Signed-off-by: Paul Gesel --- src/example_behaviors/CMakeLists.txt | 4 +- src/example_behaviors/COLCON_IGNORE | 0 .../include/example_behaviors/l2g.hpp | 69 +++ src/example_behaviors/models/l2g.onnx | 3 + src/example_behaviors/src/l2g.cpp | 156 +++++++ .../src/register_behaviors.cpp | 4 + src/lab_sim/objectives/l2g.xml | 50 +++ src/lab_sim/objectives/run_sam2_onnx.xml | 3 +- src/lab_sim/waypoints/ur_waypoints.yaml | 401 ++++++++++-------- 9 files changed, 508 insertions(+), 182 deletions(-) delete mode 100644 src/example_behaviors/COLCON_IGNORE create mode 100644 src/example_behaviors/include/example_behaviors/l2g.hpp create mode 100644 src/example_behaviors/models/l2g.onnx create mode 100644 src/example_behaviors/src/l2g.cpp create mode 100644 src/lab_sim/objectives/l2g.xml diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt index 2c7cd6a0..ec37d6a3 100644 --- a/src/example_behaviors/CMakeLists.txt +++ b/src/example_behaviors/CMakeLists.txt @@ -33,6 +33,8 @@ add_library( src/example_ndt_registration.cpp src/example_ransac_registration.cpp src/example_sam2_segmentation.cpp + src/l2g.cpp + src/sam2_segmentation.cpp src/register_behaviors.cpp) target_include_directories( example_behaviors @@ -41,7 +43,7 @@ target_include_directories( PRIVATE ${PCL_INCLUDE_DIRS}) ament_target_dependencies(example_behaviors ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(example_behaviors onnx_sam2) +target_link_libraries(example_behaviors onnx_sam2 moveit_pro_ml) # Install Libraries install( diff --git a/src/example_behaviors/COLCON_IGNORE b/src/example_behaviors/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/src/example_behaviors/include/example_behaviors/l2g.hpp b/src/example_behaviors/include/example_behaviors/l2g.hpp new file mode 100644 index 00000000..0ed93191 --- /dev/null +++ b/src/example_behaviors/include/example_behaviors/l2g.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + + +namespace moveit_pro_ml +{ + class L2GModel; +} + +namespace example_behaviors +{ +/** + * @brief Segment an image using the SAM 2 model + */ +class L2GBehavior : public moveit_studio::behaviors::AsyncBehaviorBase +{ +public: +/** + * @brief Constructor for the L2GBehavior 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. + */ + L2GBehavior(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::shared_ptr l2g_; + + /** @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 sam2_segmentation diff --git a/src/example_behaviors/models/l2g.onnx b/src/example_behaviors/models/l2g.onnx new file mode 100644 index 00000000..e1eea70c --- /dev/null +++ b/src/example_behaviors/models/l2g.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ca6f4adf53759848b6697fe9daa81267b6a09cbb098965ccdd438818bf8654ac +size 22982271 diff --git a/src/example_behaviors/src/l2g.cpp b/src/example_behaviors/src/l2g.cpp new file mode 100644 index 00000000..3f36fc96 --- /dev/null +++ b/src/example_behaviors/src/l2g.cpp @@ -0,0 +1,156 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +#include "moveit_pro_ml/onnx_model.hpp" + +namespace moveit_pro_ml +{ + class L2GModel + { + public: + L2GModel(const std::filesystem::path& onnx_file) + : model{std::make_shared(onnx_file)} + { + } + + [[nodiscard]] std::vector predict(const std::vector>& points) const + { + // move image into tensor + int num_points = points.size(); + const auto point_data = model->create_dynamic_tensor(model->dynamic_inputs.at("point_cloud"), + {1, num_points, 3}); + std::copy_n(points.data()->data(), num_points * 3, point_data); + + auto pred = model->predict_base(model->inputs, model->dynamic_inputs); + + // copy mask out and return reference + auto shape = pred.at("predicted_grasps").onnx_shape; + const auto predicted_grasps_data = model->get_tensor_data(pred.at("predicted_grasps")); + // const auto grasp_scores_data = model->get_tensor_data(pred.at("grasp_scores")); + + return {predicted_grasps_data, predicted_grasps_data + get_size(shape)}; + } + + std::shared_ptr model; + }; +} // namespace moveit_pro_ml + + +namespace +{ + constexpr auto kPortPointCloud = "point_cloud"; + constexpr auto kPortPointCloudDefault = "{point_cloud}"; + constexpr auto kPortGrasps = "grasps"; + constexpr auto kPortGraspsDefault = "{grasps}"; +} // namespace + +namespace example_behaviors +{ + L2GBehavior::L2GBehavior(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 onnx_file = package_path / "models" / "l2g.onnx"; + l2g_ = std::make_unique(onnx_file); + } + + BT::PortsList L2GBehavior::providedPorts() + { + return { + BT::InputPort(kPortPointCloud, kPortPointCloudDefault, + "The Image to run segmentation on."), + BT::OutputPort>(kPortGrasps, kPortGraspsDefault, + "The masks contained in a vector of moveit_studio_vision_msgs::msg::Mask2D messages.") + }; + } + + tl::expected L2GBehavior::doWork() + { + const auto ports = moveit_studio::behaviors::getRequiredInputs( + getInput(kPortPointCloud)); + + // 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& [point_cloud_msg] = ports.value(); + + const auto cloud = std::make_shared>(); + pcl::fromROSMsg(point_cloud_msg, *cloud); + auto filtered_cloud = std::make_shared>(); + pcl::Indices index; + pcl::removeNaNFromPointCloud(*cloud, *filtered_cloud, index); + shared_resources_->logger->publishWarnMessage("Pointcloud size: " + filtered_cloud->size()); + double fraction = std::min(10000.0 / filtered_cloud->points.size(), 1.0); + auto downsampled_cloud = moveit_studio::point_cloud_tools::downsampleRandom(filtered_cloud, fraction); + shared_resources_->logger->publishWarnMessage("Downsampled pointcloud size: " + downsampled_cloud->size()); + + try + { + std::vector> points; + points.reserve(downsampled_cloud->points.size()); + for (auto& point : downsampled_cloud->points) + { + points.push_back({point.x, point.y, point.z}); + } + + const auto grasps = l2g_->predict(points); + std::vector grasps_pose; + for (size_t i = 0; i < grasps.size(); i += 7) + { + geometry_msgs::msg::PoseStamped pose; + pose.header = point_cloud_msg.header; + pose.header.stamp = shared_resources_->node->now(); + + pose.pose.position.x = (grasps[i + 0] + grasps[i + 3]) / 2.0; + pose.pose.position.y = (grasps[i + 1] + grasps[i + 4]) / 2.0; + pose.pose.position.z = (grasps[i + 2] + grasps[i + 5]) / 2.0; + grasps_pose.push_back(pose); + + if (i >= 70) + { + break; + } + } + + setOutput>(kPortGrasps, grasps_pose); + } + catch (const std::invalid_argument& e) + { + return tl::make_unexpected(fmt::format("Invalid argument: {}", e.what())); + } + + return true; + } + + BT::KeyValueVector L2GBehavior::metadata() + { + return { + { + "description", + "Generate a grasp from a point cloud and output as a geometry_msgs/PoseStamped message." + } + }; + } +} // namespace sam2_segmentation diff --git a/src/example_behaviors/src/register_behaviors.cpp b/src/example_behaviors/src/register_behaviors.cpp index e95a4c03..293ff396 100644 --- a/src/example_behaviors/src/register_behaviors.cpp +++ b/src/example_behaviors/src/register_behaviors.cpp @@ -15,6 +15,8 @@ #include #include #include +#include +#include #include @@ -42,6 +44,8 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN moveit_studio::behaviors::registerBehavior(factory, "ExampleNDTRegistration", shared_resources); moveit_studio::behaviors::registerBehavior(factory, "ExampleRANSACRegistration", shared_resources); moveit_studio::behaviors::registerBehavior(factory, "ExampleSAM2Segmentation", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "L2GBehavior", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "SAM2Segmentation", shared_resources); } }; } // namespace example_behaviors diff --git a/src/lab_sim/objectives/l2g.xml b/src/lab_sim/objectives/l2g.xml new file mode 100644 index 00000000..923fce15 --- /dev/null +++ b/src/lab_sim/objectives/l2g.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lab_sim/objectives/run_sam2_onnx.xml b/src/lab_sim/objectives/run_sam2_onnx.xml index 181bcd83..f18a549c 100644 --- a/src/lab_sim/objectives/run_sam2_onnx.xml +++ b/src/lab_sim/objectives/run_sam2_onnx.xml @@ -12,8 +12,7 @@ - Date: Thu, 12 Dec 2024 08:31:47 -0700 Subject: [PATCH 6/7] working demo Signed-off-by: Paul Gesel --- src/example_behaviors/CMakeLists.txt | 1 - src/example_behaviors/models/l2g.onnx | 4 +- src/example_behaviors/src/l2g.cpp | 112 ++++++++++------ .../src/register_behaviors.cpp | 4 +- src/lab_sim/description/scene.xml | 6 +- src/lab_sim/objectives/integration_test.xml | 5 +- src/lab_sim/objectives/l2g.xml | 121 ++++++++++++++---- .../objectives/segment_waypoint_cloud.xml | 45 +++++++ .../objectives/take_snapshots_of_object.xml | 4 +- src/lab_sim/waypoints/ur_waypoints.yaml | 102 +++++++++++++-- 10 files changed, 321 insertions(+), 83 deletions(-) create mode 100644 src/lab_sim/objectives/segment_waypoint_cloud.xml diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt index ec37d6a3..1a2649e6 100644 --- a/src/example_behaviors/CMakeLists.txt +++ b/src/example_behaviors/CMakeLists.txt @@ -34,7 +34,6 @@ add_library( src/example_ransac_registration.cpp src/example_sam2_segmentation.cpp src/l2g.cpp - src/sam2_segmentation.cpp src/register_behaviors.cpp) target_include_directories( example_behaviors diff --git a/src/example_behaviors/models/l2g.onnx b/src/example_behaviors/models/l2g.onnx index e1eea70c..b108aac6 100644 --- a/src/example_behaviors/models/l2g.onnx +++ b/src/example_behaviors/models/l2g.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ca6f4adf53759848b6697fe9daa81267b6a09cbb098965ccdd438818bf8654ac -size 22982271 +oid sha256:fc1fd4a7cf185225ba29940da16cf052cc9801a4883aecffd82845627e0965b5 +size 15618125 diff --git a/src/example_behaviors/src/l2g.cpp b/src/example_behaviors/src/l2g.cpp index 3f36fc96..edbeeef2 100644 --- a/src/example_behaviors/src/l2g.cpp +++ b/src/example_behaviors/src/l2g.cpp @@ -3,23 +3,20 @@ #include #include -#include #include #include +#include #include #include #include -#include -#include #include #include -#include -#include #include +#include +#include - -#include "moveit_pro_ml/onnx_model.hpp" +#include namespace moveit_pro_ml { @@ -34,18 +31,18 @@ namespace moveit_pro_ml [[nodiscard]] std::vector predict(const std::vector>& points) const { // move image into tensor - int num_points = points.size(); + const int num_points = points.size(); const auto point_data = model->create_dynamic_tensor(model->dynamic_inputs.at("point_cloud"), {1, num_points, 3}); std::copy_n(points.data()->data(), num_points * 3, point_data); auto pred = model->predict_base(model->inputs, model->dynamic_inputs); - // copy mask out and return reference - auto shape = pred.at("predicted_grasps").onnx_shape; + // copy out predicted_grasps data + const auto shape = pred.at("predicted_grasps").onnx_shape; const auto predicted_grasps_data = model->get_tensor_data(pred.at("predicted_grasps")); - // const auto grasp_scores_data = model->get_tensor_data(pred.at("grasp_scores")); + // output format [center_x, center_y, center_z, qw, qx, qy, qz, grasp_width] x N return {predicted_grasps_data, predicted_grasps_data + get_size(shape)}; } @@ -60,6 +57,8 @@ namespace constexpr auto kPortPointCloudDefault = "{point_cloud}"; constexpr auto kPortGrasps = "grasps"; constexpr auto kPortGraspsDefault = "{grasps}"; + constexpr auto kNumberOfPoints = 5000; + constexpr auto kMaxGrasps = 10; } // namespace namespace example_behaviors @@ -77,9 +76,9 @@ namespace example_behaviors { return { BT::InputPort(kPortPointCloud, kPortPointCloudDefault, - "The Image to run segmentation on."), + "The point cloud to grasp. The point cloud should be approximately fit in a 0.22 meter cube."), BT::OutputPort>(kPortGrasps, kPortGraspsDefault, - "The masks contained in a vector of moveit_studio_vision_msgs::msg::Mask2D messages.") + "The grasp poses in a vector of geometry_msgs::msg::PoseStamped messages.") }; } @@ -101,46 +100,75 @@ namespace example_behaviors auto filtered_cloud = std::make_shared>(); pcl::Indices index; pcl::removeNaNFromPointCloud(*cloud, *filtered_cloud, index); - shared_resources_->logger->publishWarnMessage("Pointcloud size: " + filtered_cloud->size()); - double fraction = std::min(10000.0 / filtered_cloud->points.size(), 1.0); + if (filtered_cloud->size() < kNumberOfPoints) + { + auto error_message = fmt::format("Point cloud must have at least {} points in it.", 5000); + return tl::make_unexpected(error_message); + } + double fraction = std::min(static_cast(kNumberOfPoints) / filtered_cloud->points.size(), 1.0); auto downsampled_cloud = moveit_studio::point_cloud_tools::downsampleRandom(filtered_cloud, fraction); - shared_resources_->logger->publishWarnMessage("Downsampled pointcloud size: " + downsampled_cloud->size()); - try + // Subtract centroid from the point cloud + pcl::PointXYZ centroid; + computeCentroid(*downsampled_cloud, centroid); + + std::vector> points_centered; + points_centered.reserve(downsampled_cloud->points.size()); + for (auto& point : downsampled_cloud->points) { - std::vector> points; - points.reserve(downsampled_cloud->points.size()); - for (auto& point : downsampled_cloud->points) - { - points.push_back({point.x, point.y, point.z}); - } + points_centered.push_back({point.x - centroid.x, point.y - centroid.y, point.z - centroid.z}); + } - const auto grasps = l2g_->predict(points); - std::vector grasps_pose; - for (size_t i = 0; i < grasps.size(); i += 7) + // Filter out points outside the bounds defines by L2G. + std::vector> points; + points.reserve(downsampled_cloud->points.size()); + for (auto& point : points_centered) + { + if (abs(point[0]) > .22 / 2.0 || abs(point[1]) > .22 / 2.0 || abs(point[2]) > .22) { - geometry_msgs::msg::PoseStamped pose; - pose.header = point_cloud_msg.header; - pose.header.stamp = shared_resources_->node->now(); - - pose.pose.position.x = (grasps[i + 0] + grasps[i + 3]) / 2.0; - pose.pose.position.y = (grasps[i + 1] + grasps[i + 4]) / 2.0; - pose.pose.position.z = (grasps[i + 2] + grasps[i + 5]) / 2.0; - grasps_pose.push_back(pose); - - if (i >= 70) - { - break; - } + continue; } + points.push_back({2.0f * point[0] / .22f, 2.0f * point[1] / .22f, point[2] / .22f}); + } - setOutput>(kPortGrasps, grasps_pose); + // Run the network + std::vector grasps; + try + { + grasps = l2g_->predict(points); } catch (const std::invalid_argument& e) { return tl::make_unexpected(fmt::format("Invalid argument: {}", e.what())); } + // Copy of grasps into the output vector + std::vector grasps_pose; + for (size_t i = 0; i < grasps.size(); i += 8) + { + geometry_msgs::msg::PoseStamped pose; + pose.header = point_cloud_msg.header; + pose.header.stamp = shared_resources_->node->now(); + + pose.pose.position.x = grasps[i + 0] + centroid.x; + pose.pose.position.y = grasps[i + 1] + centroid.y; + pose.pose.position.z = grasps[i + 2] + centroid.z; + + pose.pose.orientation.w = grasps[i + 3]; + pose.pose.orientation.x = grasps[i + 4]; + pose.pose.orientation.y = grasps[i + 5]; + pose.pose.orientation.z = grasps[i + 6]; + grasps_pose.push_back(pose); + + // break after kMaxGrasps grasps have been added + if (i >= 8*kMaxGrasps) + { + break; + } + } + + setOutput>(kPortGrasps, grasps_pose); + return true; } @@ -149,8 +177,8 @@ namespace example_behaviors return { { "description", - "Generate a grasp from a point cloud and output as a geometry_msgs/PoseStamped message." + "Generate a vector grasps from a point cloud using the L2G network." } }; } -} // namespace sam2_segmentation +} // namespace example_behaviors diff --git a/src/example_behaviors/src/register_behaviors.cpp b/src/example_behaviors/src/register_behaviors.cpp index 293ff396..20ca08ca 100644 --- a/src/example_behaviors/src/register_behaviors.cpp +++ b/src/example_behaviors/src/register_behaviors.cpp @@ -14,9 +14,8 @@ #include #include #include -#include #include -#include +#include #include @@ -45,7 +44,6 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN moveit_studio::behaviors::registerBehavior(factory, "ExampleRANSACRegistration", shared_resources); moveit_studio::behaviors::registerBehavior(factory, "ExampleSAM2Segmentation", shared_resources); moveit_studio::behaviors::registerBehavior(factory, "L2GBehavior", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "SAM2Segmentation", shared_resources); } }; } // namespace example_behaviors diff --git a/src/lab_sim/description/scene.xml b/src/lab_sim/description/scene.xml index 12e33db9..8820c8e2 100644 --- a/src/lab_sim/description/scene.xml +++ b/src/lab_sim/description/scene.xml @@ -4,6 +4,10 @@ + + + + diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml index d35e5c2e..4c492956 100644 --- a/src/lab_sim/waypoints/ur_waypoints.yaml +++ b/src/lab_sim/waypoints/ur_waypoints.yaml @@ -21,14 +21,14 @@ - wrist_2_joint - wrist_3_joint position: - - 0.005443531228038125 - - 0.16745667593896538 - - 0.38469500546329094 - - -0.9495951784993102 - - 1.4530580038656942 - - -2.3563673683102415 - - -1.6678073710000332 - - 0.3743635710277495 + - 0.0046894286514114905 + - 0.11915212690482295 + - 0.5464277879246356 + - -0.972651289391312 + - 1.7722867699172182 + - -2.3474280858161203 + - -2.1189675249981383 + - 0.6261953414682204 velocity: [] multi_dof_joint_state: header: @@ -256,6 +256,49 @@ twist: [] wrench: [] name: Wrist 2 Min +- description: '' + favorite: false + joint_group_names: + - gripper + - linear_actuator + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - robotiq_85_left_knuckle_joint + - linear_rail_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + position: + - 0.004692654238118904 + - -0.10730185663677162 + - 0.5694104049327926 + - -0.8274117204859058 + - 1.6886416613673396 + - -2.7156192222400812 + - -1.658675700213927 + - 0.6429500188652083 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: pills close - description: '' favorite: false joint_group_names: @@ -428,3 +471,46 @@ twist: [] wrench: [] name: Workspace Right +- description: '' + favorite: false + joint_group_names: + - gripper + - linear_actuator + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - robotiq_85_left_knuckle_joint + - linear_rail_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + position: + - 0.00468845173290927 + - -0.2644976294122572 + - 0.3748018822110223 + - -0.672537121582914 + - 1.182712430428366 + - -2.130689101859695 + - -1.0011130816973868 + - 0.19030279540117478 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: look cube back From bcc7b8815e26d03cfcd3766c310df33b4cb2af37 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 13 Dec 2024 11:18:56 -0700 Subject: [PATCH 7/7] remove graspnet_example_behavior Signed-off-by: Paul Gesel --- src/graspnet_example_behavior/CMakeLists.txt | 48 ---------- .../behavior_plugin.yaml | 4 - .../config/tree_nodes_model.xml | 6 -- ...et_example_behavior_plugin_description.xml | 7 -- .../graspnet_example_behavior.hpp | 49 ---------- src/graspnet_example_behavior/package.xml | 29 ------ .../src/graspnet_example_behavior.cpp | 93 ------------------- .../src/register_behaviors.cpp | 24 ----- .../test/CMakeLists.txt | 4 - .../test/test_behavior_plugins.cpp | 37 -------- 10 files changed, 301 deletions(-) delete mode 100644 src/graspnet_example_behavior/CMakeLists.txt delete mode 100644 src/graspnet_example_behavior/behavior_plugin.yaml delete mode 100644 src/graspnet_example_behavior/config/tree_nodes_model.xml delete mode 100644 src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml delete mode 100644 src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp delete mode 100644 src/graspnet_example_behavior/package.xml delete mode 100644 src/graspnet_example_behavior/src/graspnet_example_behavior.cpp delete mode 100644 src/graspnet_example_behavior/src/register_behaviors.cpp delete mode 100644 src/graspnet_example_behavior/test/CMakeLists.txt delete mode 100644 src/graspnet_example_behavior/test/test_behavior_plugins.cpp diff --git a/src/graspnet_example_behavior/CMakeLists.txt b/src/graspnet_example_behavior/CMakeLists.txt deleted file mode 100644 index 6edb107c..00000000 --- a/src/graspnet_example_behavior/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(graspnet_example_behavior CXX) - -find_package(moveit_studio_common REQUIRED) -moveit_studio_package() - -set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib) -foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${package} REQUIRED) -endforeach() - -add_library( - graspnet_example_behavior - SHARED - src/graspnet_example_behavior.cpp - src/register_behaviors.cpp) -target_include_directories( - graspnet_example_behavior - PUBLIC $ - $) -ament_target_dependencies(graspnet_example_behavior - ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Install Libraries -install( - TARGETS graspnet_example_behavior - EXPORT graspnet_example_behaviorTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES - DESTINATION include) - -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) - -if(BUILD_TESTING) - moveit_pro_behavior_test(graspnet_example_behavior) -endif() - -# Export the behavior plugins defined in this package so they are available to -# plugin loaders that load the behavior base class library from the -# moveit_studio_behavior package. -pluginlib_export_plugin_description_file( - moveit_studio_behavior_interface graspnet_example_behavior_plugin_description.xml) - -ament_export_targets(graspnet_example_behaviorTargets HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/src/graspnet_example_behavior/behavior_plugin.yaml b/src/graspnet_example_behavior/behavior_plugin.yaml deleted file mode 100644 index 921ac6b8..00000000 --- a/src/graspnet_example_behavior/behavior_plugin.yaml +++ /dev/null @@ -1,4 +0,0 @@ -objectives: - behavior_loader_plugins: - graspnet_example_behavior: - - "graspnet_example_behavior::GraspnetExampleBehaviorBehaviorsLoader" diff --git a/src/graspnet_example_behavior/config/tree_nodes_model.xml b/src/graspnet_example_behavior/config/tree_nodes_model.xml deleted file mode 100644 index f3844c22..00000000 --- a/src/graspnet_example_behavior/config/tree_nodes_model.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml b/src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml deleted file mode 100644 index 94a0e625..00000000 --- a/src/graspnet_example_behavior/graspnet_example_behavior_plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - diff --git a/src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp b/src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp deleted file mode 100644 index ea67df15..00000000 --- a/src/graspnet_example_behavior/include/graspnet_example_behavior/graspnet_example_behavior.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include - -// This header includes the SharedResourcesNode type -#include -#include -#include - -namespace graspnet_example_behavior -{ -/** - * @brief TODO(...) - */ -class GraspnetExampleBehavior : public moveit_studio::behaviors::SharedResourcesNode -{ -public: - /** - * @brief Constructor for the graspnet_example_behavior 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. - */ - GraspnetExampleBehavior(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); - - /** - * @brief Implementation of the required providedPorts() function for the graspnet_example_behavior 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 graspnet_example_behavior does not use expose any ports, so this function returns an empty list. - */ - static BT::PortsList providedPorts(); - - /** - * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and - * subcategory, in the MoveIt Studio Developer Tool. - * @return A BT::KeyValueVector containing the Behavior metadata. - */ - static BT::KeyValueVector metadata(); - - /** - * @brief Implementation of BT::SyncActionNode::tick() for GraspnetExampleBehavior. - * @details This function is where the Behavior performs its work when the behavior tree is being run. Since GraspnetExampleBehavior is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly. - */ - BT::NodeStatus tick() override; - -}; -} // namespace graspnet_example_behavior diff --git a/src/graspnet_example_behavior/package.xml b/src/graspnet_example_behavior/package.xml deleted file mode 100644 index 01553d07..00000000 --- a/src/graspnet_example_behavior/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - graspnet_example_behavior - 0.0.0 - - Takes PointClouds of an object and generates a Grasp Pose from GraspNet then - executes the grasping. - - - John Doe - John Doe - - TODO - - ament_cmake - - moveit_studio_common - - moveit_studio_behavior_interface - - ament_lint_auto - ament_cmake_gtest - ament_clang_format - ament_clang_tidy - - - ament_cmake - - diff --git a/src/graspnet_example_behavior/src/graspnet_example_behavior.cpp b/src/graspnet_example_behavior/src/graspnet_example_behavior.cpp deleted file mode 100644 index 1056f7ef..00000000 --- a/src/graspnet_example_behavior/src/graspnet_example_behavior.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include - -#include -#include -#include - -namespace -{ - inline constexpr auto kDescriptionGraspNetExampleBehavior = R"( -

- To do -

- )"; - constexpr auto kPortIDPointCloud = "point_cloud"; - constexpr auto kPortIDPCDTopicName = "pcd_topic"; - constexpr auto kPortIDStampedPose = "stamped_pose"; - constexpr auto kPortIDNumSamples = "num_samples"; -} //namespace - -namespace graspnet_example_behavior -{ -GraspnetExampleBehavior::GraspnetExampleBehavior( - const std::string& name, const BT::NodeConfiguration& config, - const std::shared_ptr& shared_resources) - : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) -{ -} - - -BT::PortsList GraspnetExampleBehavior::providedPorts() -{ - // TODO(...) - return { BT::InputPort(kPortIDPCDTopicName, "/pcd_pointcloud_captures", - "Topic the pcd formatted point cloud is published to."), - BT::InputPort(kPortIDPointCloud, "{point_cloud}", - "Point cloud in sensor_msgs::msg::PointCloud2 format."), - BT::InputPort(kPortIDNumSamples, "{num_samples}", "Number of samples (poses) to take from GraspNet."), - BT::OutputPort>(kPortIDStampedPose, "{pose_stamped_vector}", - "The geometry_msgs/PoseStamped message output.") }; -} - -BT::KeyValueVector GraspnetExampleBehavior::metadata() -{ - - return { { "subcategory", "Grasping"}, {"description", kDescriptionGraspNetExampleBehavior} }; -} - -BT::NodeStatus GraspnetExampleBehavior::tick() -{ - using namespace moveit_studio::behaviors; - // Return SUCCESS once the work has been completed. - const auto ports = getRequiredInputs(getInput(kPortIDPCDTopicName), - getInput(kPortIDPointCloud), - getInput(kPortIDNumSamples)); - - if (!ports.has_value()) - { - shared_resources_->logger->publishFailureMessage(name(), "Failed to get required value from input data port: " + - ports.error()); - return BT::NodeStatus::FAILURE; - } - - const auto& [pc_topic, pointcloud, num_samples] = ports.value(); - std::vector pose_stamped_vector; - - std::random_device r; - std::mt19937 el(r()); - std::uniform_real_distribution uniform_dist(-0.75, 0.75); - - geometry_msgs::msg::PoseStamped to_edit; - for (int i=0; inode->now(); - - to_edit.pose.position.x = uniform_dist(el); - to_edit.pose.position.y = uniform_dist(el); - to_edit.pose.position.z = uniform_dist(el); - - to_edit.pose.orientation.x = 0.0; - to_edit.pose.orientation.y = 0.0; - to_edit.pose.orientation.z = 0.0; - to_edit.pose.orientation.w = 1.0; - - pose_stamped_vector.push_back(to_edit); - } - - setOutput(kPortIDStampedPose, pose_stamped_vector); - - return BT::NodeStatus::SUCCESS; -} - -} // namespace graspnet_example_behavior diff --git a/src/graspnet_example_behavior/src/register_behaviors.cpp b/src/graspnet_example_behavior/src/register_behaviors.cpp deleted file mode 100644 index a1e9fd20..00000000 --- a/src/graspnet_example_behavior/src/register_behaviors.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include -#include -#include - -#include - -#include - -namespace graspnet_example_behavior -{ -class GraspnetExampleBehaviorBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase -{ -public: - void registerBehaviors(BT::BehaviorTreeFactory& factory, - [[maybe_unused]] const std::shared_ptr& shared_resources) override - { - moveit_studio::behaviors::registerBehavior(factory, "GraspnetExampleBehavior", shared_resources); - - } -}; -} // namespace graspnet_example_behavior - -PLUGINLIB_EXPORT_CLASS(graspnet_example_behavior::GraspnetExampleBehaviorBehaviorsLoader, - moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/graspnet_example_behavior/test/CMakeLists.txt b/src/graspnet_example_behavior/test/CMakeLists.txt deleted file mode 100644 index 8db41205..00000000 --- a/src/graspnet_example_behavior/test/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -find_package(ament_cmake_gtest REQUIRED) - -ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp) -ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/graspnet_example_behavior/test/test_behavior_plugins.cpp b/src/graspnet_example_behavior/test/test_behavior_plugins.cpp deleted file mode 100644 index 6add9c68..00000000 --- a/src/graspnet_example_behavior/test/test_behavior_plugins.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include - -#include -#include -#include -#include - -/** - * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and - * instantiated by the behavior tree factory. - */ -TEST(BehaviorTests, test_load_behavior_plugins) -{ - pluginlib::ClassLoader class_loader( - "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"); - - auto node = std::make_shared("BehaviorTests"); - auto shared_resources = std::make_shared(node); - - BT::BehaviorTreeFactory factory; - { - auto plugin_instance = class_loader.createUniqueInstance("graspnet_example_behavior::GraspnetExampleBehaviorBehaviorsLoader"); - ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); - } - - // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. - EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "GraspnetExampleBehavior", BT::NodeConfiguration())); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}