diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt index 98129a10..e5944160 100644 --- a/src/example_behaviors/CMakeLists.txt +++ b/src/example_behaviors/CMakeLists.txt @@ -20,19 +20,18 @@ find_package(moveit_pro_ml REQUIRED) add_library( example_behaviors SHARED - src/add_two_ints_service_client.cpp - src/convert_mtc_solution_to_joint_trajectory.cpp - src/delayed_message.cpp - src/fibonacci_action_client.cpp - src/get_string_from_topic.cpp - src/hello_world.cpp - src/ndt_registration.cpp - src/publish_color_rgba.cpp - src/ransac_registration.cpp - src/sam2_segmentation.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/register_behaviors.cpp) target_include_directories( example_behaviors diff --git a/src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp b/src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp similarity index 91% rename from src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp rename to src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp index a79f76e8..9840ee58 100644 --- a/src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp b/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp similarity index 89% rename from src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp rename to src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp index 1c0a95c7..c8712d5a 100644 --- a/src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp +++ b/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp @@ -29,10 +29,10 @@ namespace example_behaviors * | sampling_rate | Input | int | * | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory | */ -class ConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode +class ExampleConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode { public: - ConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, + ExampleConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources); static BT::PortsList providedPorts(); diff --git a/src/example_behaviors/include/example_behaviors/delayed_message.hpp b/src/example_behaviors/include/example_behaviors/example_delayed_message.hpp similarity index 90% rename from src/example_behaviors/include/example_behaviors/delayed_message.hpp rename to src/example_behaviors/include/example_behaviors/example_delayed_message.hpp index 6e95c173..b6648f93 100644 --- a/src/example_behaviors/include/example_behaviors/delayed_message.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp b/src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp similarity index 92% rename from src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp rename to src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp index 06f70b12..19e4e2f9 100644 --- a/src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/get_string_from_topic.hpp b/src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp similarity index 86% rename from src/example_behaviors/include/example_behaviors/get_string_from_topic.hpp rename to src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp index 7c6e6662..5ea2d711 100644 --- a/src/example_behaviors/include/example_behaviors/get_string_from_topic.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/hello_world.hpp b/src/example_behaviors/include/example_behaviors/example_hello_world.hpp similarity index 84% rename from src/example_behaviors/include/example_behaviors/hello_world.hpp rename to src/example_behaviors/include/example_behaviors/example_hello_world.hpp index 02886219..524f650c 100644 --- a/src/example_behaviors/include/example_behaviors/hello_world.hpp +++ b/src/example_behaviors/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& shared_resources); + 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. diff --git a/src/example_behaviors/include/example_behaviors/publish_color_rgba.hpp b/src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp similarity index 78% rename from src/example_behaviors/include/example_behaviors/publish_color_rgba.hpp rename to src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp index 38531347..11c08809 100644 --- a/src/example_behaviors/include/example_behaviors/publish_color_rgba.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/ransac_registration.hpp b/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp similarity index 91% rename from src/example_behaviors/include/example_behaviors/ransac_registration.hpp rename to src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp index b3bd141c..a786b733 100644 --- a/src/example_behaviors/include/example_behaviors/ransac_registration.hpp +++ b/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp @@ -12,7 +12,7 @@ namespace example_behaviors /** * @brief TODO(...) */ -class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase +class ExampleRANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase { public: /** @@ -22,7 +22,7 @@ class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase * @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); + 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. diff --git a/src/example_behaviors/include/example_behaviors/setup_mtc_pick_from_pose.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp similarity index 83% rename from src/example_behaviors/include/example_behaviors/setup_mtc_pick_from_pose.hpp rename to src/example_behaviors/include/example_behaviors/example_setup_mtc_pick_from_pose.hpp index e271bd23..3ce38834 100644 --- a/src/example_behaviors/include/example_behaviors/setup_mtc_pick_from_pose.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/setup_mtc_place_from_pose.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_place_from_pose.hpp similarity index 83% rename from src/example_behaviors/include/example_behaviors/setup_mtc_place_from_pose.hpp rename to src/example_behaviors/include/example_behaviors/example_setup_mtc_place_from_pose.hpp index c06431cb..9f81ae9a 100644 --- a/src/example_behaviors/include/example_behaviors/setup_mtc_place_from_pose.hpp +++ b/src/example_behaviors/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/src/example_behaviors/include/example_behaviors/setup_mtc_wave_hand.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp similarity index 86% rename from src/example_behaviors/include/example_behaviors/setup_mtc_wave_hand.hpp rename to src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp index 4d635b08..756b3555 100644 --- a/src/example_behaviors/include/example_behaviors/setup_mtc_wave_hand.hpp +++ b/src/example_behaviors/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/src/example_behaviors/src/add_two_ints_service_client.cpp b/src/example_behaviors/src/example_add_two_ints_service_client.cpp similarity index 74% rename from src/example_behaviors/src/add_two_ints_service_client.cpp rename to src/example_behaviors/src/example_add_two_ints_service_client.cpp index 9ade021b..5e90fa05 100644 --- a/src/example_behaviors/src/add_two_ints_service_client.cpp +++ b/src/example_behaviors/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_behaviors/src/convert_mtc_solution_to_joint_trajectory.cpp b/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp similarity index 89% rename from src/example_behaviors/src/convert_mtc_solution_to_joint_trajectory.cpp rename to src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp index 6cb5cd28..51dd0077 100644 --- a/src/example_behaviors/src/convert_mtc_solution_to_joint_trajectory.cpp +++ b/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp @@ -1,8 +1,8 @@ -#include +#include namespace { -const auto kLogger = rclcpp::get_logger("ConvertMtcSolutionToJointTrajectory"); +const auto kLogger = rclcpp::get_logger("ExampleConvertMtcSolutionToJointTrajectory"); // Port names for input and output ports. constexpr auto kPortIDSolution = "solution"; @@ -15,14 +15,14 @@ constexpr auto kPortIDSamplingRate = "sampling_rate"; namespace example_behaviors { -ConvertMtcSolutionToJointTrajectory::ConvertMtcSolutionToJointTrajectory( +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 ConvertMtcSolutionToJointTrajectory::providedPorts() +BT::PortsList ExampleConvertMtcSolutionToJointTrajectory::providedPorts() { return { BT::InputPort(kPortIDSolution, "{mtc_solution}", @@ -40,13 +40,13 @@ BT::PortsList ConvertMtcSolutionToJointTrajectory::providedPorts() }; } -BT::KeyValueVector ConvertMtcSolutionToJointTrajectory::metadata() +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 ConvertMtcSolutionToJointTrajectory::tick() +BT::NodeStatus ExampleConvertMtcSolutionToJointTrajectory::tick() { using namespace moveit_studio::behaviors; @@ -108,7 +108,7 @@ BT::NodeStatus ConvertMtcSolutionToJointTrajectory::tick() return BT::NodeStatus::SUCCESS; } -const std::vector& ConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) +const std::vector& ExampleConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) { static std::vector waypoints; waypoints.clear(); diff --git a/src/example_behaviors/src/delayed_message.cpp b/src/example_behaviors/src/example_delayed_message.cpp similarity index 84% rename from src/example_behaviors/src/delayed_message.cpp rename to src/example_behaviors/src/example_delayed_message.cpp index 87c4c826..34818fae 100644 --- a/src/example_behaviors/src/delayed_message.cpp +++ b/src/example_behaviors/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/example_behaviors/src/fibonacci_action_client.cpp b/src/example_behaviors/src/example_fibonacci_action_client.cpp similarity index 78% rename from src/example_behaviors/src/fibonacci_action_client.cpp rename to src/example_behaviors/src/example_fibonacci_action_client.cpp index cf3255e9..e8c0bdd1 100644 --- a/src/example_behaviors/src/fibonacci_action_client.cpp +++ b/src/example_behaviors/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/example_behaviors/src/get_string_from_topic.cpp b/src/example_behaviors/src/example_get_string_from_topic.cpp similarity index 74% rename from src/example_behaviors/src/get_string_from_topic.cpp rename to src/example_behaviors/src/example_get_string_from_topic.cpp index 1ee63726..48495ed3 100644 --- a/src/example_behaviors/src/get_string_from_topic.cpp +++ b/src/example_behaviors/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/example_behaviors/src/hello_world.cpp b/src/example_behaviors/src/example_hello_world.cpp similarity index 55% rename from src/example_behaviors/src/hello_world.cpp rename to src/example_behaviors/src/example_hello_world.cpp index 596d6b95..c214c389 100644 --- a/src/example_behaviors/src/hello_world.cpp +++ b/src/example_behaviors/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_behaviors/src/ndt_registration.cpp b/src/example_behaviors/src/example_ndt_registration.cpp similarity index 95% rename from src/example_behaviors/src/ndt_registration.cpp rename to src/example_behaviors/src/example_ndt_registration.cpp index 3b8c6e63..f8e538b5 100644 --- a/src/example_behaviors/src/ndt_registration.cpp +++ b/src/example_behaviors/src/example_ndt_registration.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -43,7 +43,7 @@ getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) } // namespace -NDTRegistration::NDTRegistration( +ExampleNDTRegistration::ExampleNDTRegistration( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) @@ -51,7 +51,7 @@ NDTRegistration::NDTRegistration( } -BT::PortsList NDTRegistration::providedPorts() +BT::PortsList ExampleNDTRegistration::providedPorts() { return { BT::InputPort("base_point_cloud", "{point_cloud}", "Point cloud to align with the target point cloud."), @@ -73,13 +73,13 @@ BT::PortsList NDTRegistration::providedPorts() "The pose of the target point cloud relative to the frame " "of the base point cloud.") }; } -BT::KeyValueVector NDTRegistration::metadata() +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 NDTRegistration::doWork() +tl::expected ExampleNDTRegistration::doWork() { const auto base_point_cloud_msg = getInput("base_point_cloud"); const auto target_point_cloud_msg = getInput("target_point_cloud"); diff --git a/src/example_behaviors/src/publish_color_rgba.cpp b/src/example_behaviors/src/example_publish_color_rgba.cpp similarity index 66% rename from src/example_behaviors/src/publish_color_rgba.cpp rename to src/example_behaviors/src/example_publish_color_rgba.cpp index b227746f..5a6e68ed 100644 --- a/src/example_behaviors/src/publish_color_rgba.cpp +++ b/src/example_behaviors/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_behaviors/src/ransac_registration.cpp b/src/example_behaviors/src/example_ransac_registration.cpp similarity index 97% rename from src/example_behaviors/src/ransac_registration.cpp rename to src/example_behaviors/src/example_ransac_registration.cpp index c25f81ea..b85d69c0 100644 --- a/src/example_behaviors/src/ransac_registration.cpp +++ b/src/example_behaviors/src/example_ransac_registration.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -43,7 +43,7 @@ getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg) } // namespace -RANSACRegistration::RANSACRegistration( +ExampleRANSACRegistration::ExampleRANSACRegistration( const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr& shared_resources) : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources) @@ -51,7 +51,7 @@ RANSACRegistration::RANSACRegistration( } -BT::PortsList RANSACRegistration::providedPorts() +BT::PortsList ExampleRANSACRegistration::providedPorts() { return { BT::InputPort("base_point_cloud", "{point_cloud}", "Point cloud to align with the target point cloud."), @@ -80,13 +80,13 @@ BT::PortsList RANSACRegistration::providedPorts() "of the base point cloud.") }; } -BT::KeyValueVector RANSACRegistration::metadata() +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 RANSACRegistration::doWork() +tl::expected ExampleRANSACRegistration::doWork() { const auto base_point_cloud_msg = getInput("base_point_cloud"); diff --git a/src/example_behaviors/src/setup_mtc_pick_from_pose.cpp b/src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp similarity index 96% rename from src/example_behaviors/src/setup_mtc_pick_from_pose.cpp rename to src/example_behaviors/src/example_setup_mtc_pick_from_pose.cpp index f83267f9..67f56007 100644 --- a/src/example_behaviors/src/setup_mtc_pick_from_pose.cpp +++ b/src/example_behaviors/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/example_behaviors/src/setup_mtc_place_from_pose.cpp b/src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp similarity index 95% rename from src/example_behaviors/src/setup_mtc_place_from_pose.cpp rename to src/example_behaviors/src/example_setup_mtc_place_from_pose.cpp index dd24d5ba..3f5e44ae 100644 --- a/src/example_behaviors/src/setup_mtc_place_from_pose.cpp +++ b/src/example_behaviors/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/example_behaviors/src/setup_mtc_wave_hand.cpp b/src/example_behaviors/src/example_setup_mtc_wave_hand.cpp similarity index 90% rename from src/example_behaviors/src/setup_mtc_wave_hand.cpp rename to src/example_behaviors/src/example_setup_mtc_wave_hand.cpp index 3f00581c..5867b1b0 100644 --- a/src/example_behaviors/src/setup_mtc_wave_hand.cpp +++ b/src/example_behaviors/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/example_behaviors/src/register_behaviors.cpp b/src/example_behaviors/src/register_behaviors.cpp index 9755dc04..80662314 100644 --- a/src/example_behaviors/src/register_behaviors.cpp +++ b/src/example_behaviors/src/register_behaviors.cpp @@ -2,19 +2,18 @@ #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 +#include +#include #include @@ -26,22 +25,21 @@ 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, "ConvertMtcSolutionToJointTrajectory", 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, "SAM2Segmentation", 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, "NDTRegistration", shared_resources); - moveit_studio::behaviors::registerBehavior(factory, "RANSACRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleNDTRegistration", shared_resources); + moveit_studio::behaviors::registerBehavior(factory, "ExampleRANSACRegistration", shared_resources); } }; } // namespace example_behaviors diff --git a/src/example_behaviors/test/test_behavior_plugins.cpp b/src/example_behaviors/test/test_behavior_plugins.cpp index c6432ea5..56dae5a8 100644 --- a/src/example_behaviors/test/test_behavior_plugins.cpp +++ b/src/example_behaviors/test/test_behavior_plugins.cpp @@ -24,23 +24,23 @@ 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", "NDTRegistration", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleNDTRegistration", BT::NodeConfiguration())); EXPECT_NO_THROW( - (void)factory.instantiateTreeNode("test_behavior_name", "RANSACRegistration", BT::NodeConfiguration())); + (void)factory.instantiateTreeNode("test_behavior_name", "ExampleRANSACRegistration", BT::NodeConfiguration())); } int main(int argc, char** argv) diff --git a/src/fanuc_sim/objectives/interpolate_to_joint_state.xml b/src/fanuc_sim/objectives/interpolate_to_joint_state.xml index 892d8e83..e4f99ff4 100644 --- a/src/fanuc_sim/objectives/interpolate_to_joint_state.xml +++ b/src/fanuc_sim/objectives/interpolate_to_joint_state.xml @@ -34,6 +34,9 @@ + + + diff --git a/src/fanuc_sim/objectives/move_to_joint_state.xml b/src/fanuc_sim/objectives/move_to_joint_state.xml index 60b6c5d1..63ab4a8e 100644 --- a/src/fanuc_sim/objectives/move_to_joint_state.xml +++ b/src/fanuc_sim/objectives/move_to_joint_state.xml @@ -32,6 +32,9 @@ + + + diff --git a/src/fanuc_sim/objectives/move_to_pose.xml b/src/fanuc_sim/objectives/move_to_pose.xml index 1e58a85e..4009dca1 100644 --- a/src/fanuc_sim/objectives/move_to_pose.xml +++ b/src/fanuc_sim/objectives/move_to_pose.xml @@ -36,6 +36,9 @@ + + + diff --git a/src/fanuc_sim/objectives/move_to_waypoint.xml b/src/fanuc_sim/objectives/move_to_waypoint.xml index ca42e19e..ad9ec51d 100644 --- a/src/fanuc_sim/objectives/move_to_waypoint.xml +++ b/src/fanuc_sim/objectives/move_to_waypoint.xml @@ -47,6 +47,9 @@ name="controller_names" default="/joint_trajectory_controller" /> + + + diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml index 9693c3f9..60490ad4 100644 --- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml +++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml @@ -119,4 +119,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml index a8b2f957..4ede36b5 100644 --- a/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml +++ b/src/lab_sim/objectives/add_waypoints_to_mtc_task.xml @@ -1,3 +1,4 @@ + + + + diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml index df3f814f..29dc823a 100644 --- a/src/lab_sim/objectives/apriltag_pick_object.xml +++ b/src/lab_sim/objectives/apriltag_pick_object.xml @@ -1,4 +1,3 @@ - - + + + + + diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml index 9b680044..838533b1 100644 --- a/src/lab_sim/objectives/constrained_pick_place.xml +++ b/src/lab_sim/objectives/constrained_pick_place.xml @@ -152,4 +152,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml index 0a5ccce0..fe951010 100644 --- a/src/lab_sim/objectives/create_pose_vector.xml +++ b/src/lab_sim/objectives/create_pose_vector.xml @@ -111,6 +111,9 @@ + + + diff --git a/src/lab_sim/objectives/cycle_between_waypoints.xml b/src/lab_sim/objectives/cycle_between_waypoints.xml index 71ff91b6..e6c79ac7 100644 --- a/src/lab_sim/objectives/cycle_between_waypoints.xml +++ b/src/lab_sim/objectives/cycle_between_waypoints.xml @@ -25,4 +25,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/force_relaxation.xml b/src/lab_sim/objectives/force_relaxation.xml index a62429fe..f137daea 100644 --- a/src/lab_sim/objectives/force_relaxation.xml +++ b/src/lab_sim/objectives/force_relaxation.xml @@ -50,6 +50,9 @@ + + + diff --git a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml index 302e2f6f..69637f54 100644 --- a/src/lab_sim/objectives/get_april_tag_pose_from_image.xml +++ b/src/lab_sim/objectives/get_april_tag_pose_from_image.xml @@ -40,6 +40,9 @@ + + + diff --git a/src/lab_sim/objectives/joint_diagnostic.xml b/src/lab_sim/objectives/joint_diagnostic.xml index 1bbb7714..aebb7879 100644 --- a/src/lab_sim/objectives/joint_diagnostic.xml +++ b/src/lab_sim/objectives/joint_diagnostic.xml @@ -35,4 +35,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml index 989a9fbb..b0b64db3 100644 --- a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml +++ b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml @@ -22,4 +22,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml index 28acf149..cce854ba 100644 --- a/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml +++ b/src/lab_sim/objectives/load_mesh_as_green_poincloud.xml @@ -31,6 +31,9 @@ + + + diff --git a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml index 4984c7c5..e7492b15 100644 --- a/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml +++ b/src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml @@ -31,6 +31,9 @@ + + + diff --git a/src/lab_sim/objectives/look_at_table.xml b/src/lab_sim/objectives/look_at_table.xml index 8aeed171..70626c00 100644 --- a/src/lab_sim/objectives/look_at_table.xml +++ b/src/lab_sim/objectives/look_at_table.xml @@ -17,4 +17,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/move_with_velocity_and_force.xml b/src/lab_sim/objectives/move_with_velocity_and_force.xml index 0026831a..32c099e5 100644 --- a/src/lab_sim/objectives/move_with_velocity_and_force.xml +++ b/src/lab_sim/objectives/move_with_velocity_and_force.xml @@ -3,7 +3,7 @@ + + + + + + + diff --git a/src/lab_sim/objectives/pick_and_place_example.xml b/src/lab_sim/objectives/pick_and_place_example.xml index 49c844c9..b039d860 100644 --- a/src/lab_sim/objectives/pick_and_place_example.xml +++ b/src/lab_sim/objectives/pick_and_place_example.xml @@ -1,10 +1,9 @@ - @@ -24,7 +23,7 @@ task="{mtc_pick_task}" /> - + - + + + + + + + + diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml index 6a932157..6ca263f1 100644 --- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml +++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml @@ -50,4 +50,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/pick_first_object_in_vector.xml b/src/lab_sim/objectives/pick_first_object_in_vector.xml index 30bd2bf4..b153b8a1 100644 --- a/src/lab_sim/objectives/pick_first_object_in_vector.xml +++ b/src/lab_sim/objectives/pick_first_object_in_vector.xml @@ -43,6 +43,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml index 2aa4018c..9ecb7981 100644 --- a/src/lab_sim/objectives/pick_object.xml +++ b/src/lab_sim/objectives/pick_object.xml @@ -29,4 +29,11 @@ /> + + + + + + + diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml index fc22a88e..c405250c 100644 --- a/src/lab_sim/objectives/pick_up_cube.xml +++ b/src/lab_sim/objectives/pick_up_cube.xml @@ -14,4 +14,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/place_first_object_in_vector.xml b/src/lab_sim/objectives/place_first_object_in_vector.xml index 0e058131..efde08c0 100644 --- a/src/lab_sim/objectives/place_first_object_in_vector.xml +++ b/src/lab_sim/objectives/place_first_object_in_vector.xml @@ -42,4 +42,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml index 21a7eaa5..548e71f5 100644 --- a/src/lab_sim/objectives/place_object.xml +++ b/src/lab_sim/objectives/place_object.xml @@ -30,6 +30,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/plan_and_save_trajectory.xml b/src/lab_sim/objectives/plan_and_save_trajectory.xml index fe26aee1..db138a7a 100644 --- a/src/lab_sim/objectives/plan_and_save_trajectory.xml +++ b/src/lab_sim/objectives/plan_and_save_trajectory.xml @@ -1,3 +1,4 @@ + - + - + + + + + diff --git a/src/lab_sim/objectives/plan_move_to_pose.xml b/src/lab_sim/objectives/plan_move_to_pose.xml index 9f1da373..0d3be8df 100644 --- a/src/lab_sim/objectives/plan_move_to_pose.xml +++ b/src/lab_sim/objectives/plan_move_to_pose.xml @@ -1,5 +1,6 @@ + - + + + + diff --git a/src/lab_sim/objectives/plan_to_pose.xml b/src/lab_sim/objectives/plan_to_pose.xml index 13d39ec9..066fac12 100644 --- a/src/lab_sim/objectives/plan_to_pose.xml +++ b/src/lab_sim/objectives/plan_to_pose.xml @@ -49,6 +49,9 @@ + + + diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml index 4c56b729..607bbae3 100644 --- a/src/lab_sim/objectives/register_cad_part.xml +++ b/src/lab_sim/objectives/register_cad_part.xml @@ -3,7 +3,7 @@ @@ -58,4 +58,11 @@ + + + + + + + diff --git a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml b/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml index ddeac5a6..caafc2a5 100644 --- a/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml +++ b/src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml @@ -6,7 +6,7 @@ @@ -15,6 +15,10 @@ - + + + + + diff --git a/src/lab_sim/objectives/take_snap.xml b/src/lab_sim/objectives/take_snap.xml index ebadf701..f6fb49d9 100644 --- a/src/lab_sim/objectives/take_snap.xml +++ b/src/lab_sim/objectives/take_snap.xml @@ -1,7 +1,7 @@ - + - + + + + + diff --git a/src/lab_sim/objectives/visualize_pose.xml b/src/lab_sim/objectives/visualize_pose.xml index 524070c5..b8a8d771 100644 --- a/src/lab_sim/objectives/visualize_pose.xml +++ b/src/lab_sim/objectives/visualize_pose.xml @@ -15,4 +15,11 @@ + + + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml index 9be4f855..70fa45ba 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/get_imarker_pose_from_mesh_visualization.xml @@ -26,6 +26,9 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml index dd1dac8a..15132ea6 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_to_waypoint.xml @@ -38,6 +38,9 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml index 51a3f183..fca077c6 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_pose.xml @@ -63,6 +63,9 @@ + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml index 4733f815..c81b5485 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_to_waypoint.xml @@ -45,6 +45,9 @@ name="controller_names" default="/joint_trajectory_controller" /> + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml index 9c4f1ec4..5fbbf52e 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/re_zero_force_torque_sensors.xml @@ -18,6 +18,10 @@ - + + + + + diff --git a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml index 78f8523c..0f26bbba 100644 --- a/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml +++ b/src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/sample_april_tag.xml @@ -56,6 +56,9 @@ + + + diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml index fede7df6..1414ad50 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/estimate_object_pose.xml @@ -71,6 +71,10 @@ - + + + + + diff --git a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml index 03509637..60cfa019 100644 --- a/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml +++ b/src/moveit_pro_mobile_manipulation/mobile_manipulation_config/objectives/move_to_a_stampedpose.xml @@ -57,6 +57,9 @@ default="/joint_trajectory_controller" /> + + + diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml index 07878d8d..89f2873c 100644 --- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml +++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/take_snap.xml @@ -14,6 +14,10 @@ - + + + + + diff --git a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml index 97c4f73b..324be61b 100644 --- a/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml +++ b/src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config/objectives/wrist_snap.xml @@ -13,6 +13,10 @@ - + + + + + diff --git a/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml b/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml index b92faa44..8fa1bfc1 100644 --- a/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml +++ b/src/moveit_pro_ur_configs/arm_on_rail_sim/objectives/pick_object.xml @@ -30,6 +30,10 @@ - + + + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml index b4f943f0..7c2e83b3 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/clear_snapshot.xml @@ -1,14 +1,14 @@ - - + diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml index e96d3dba..d280cbb4 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml @@ -1,6 +1,10 @@ - - + - + + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml index 1e0c2a5e..82d378cb 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_joint_state.xml @@ -31,6 +31,9 @@ + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml index cda5743f..c00a3227 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_multi_arm_config/objectives/move_to_pose.xml @@ -35,6 +35,9 @@ + + + diff --git a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml index ee4c82a9..24964ffa 100644 --- a/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml +++ b/src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml @@ -59,6 +59,9 @@ default="{pre_approach_robot_state}" /> + + +