diff --git a/src/example_behaviors/call_my_service_solution/CMakeLists.txt b/src/example_behaviors/call_my_service_solution/CMakeLists.txt new file mode 100644 index 00000000..21bef3b0 --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.22) +project(call_my_service CXX) + +find_package(moveit_studio_common REQUIRED) +moveit_studio_package() + +find_package(moveit_studio_behavior_interface REQUIRED) +find_package(service_interface REQUIRED) +find_package(pluginlib REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface service_interface pluginlib) + +add_library( + call_my_service + SHARED + src/call_my_service.cpp + src/register_behaviors.cpp) +target_include_directories( + call_my_service + PUBLIC $ + $) +ament_target_dependencies(call_my_service + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS call_my_service + EXPORT call_my_serviceTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + add_subdirectory(test) + ament_lint_auto_find_test_dependencies() +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 call_my_service_plugin_description.xml) + +ament_export_targets(call_my_serviceTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/example_behaviors/call_my_service_solution/behavior_plugin.yaml b/src/example_behaviors/call_my_service_solution/behavior_plugin.yaml new file mode 100644 index 00000000..9f600878 --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + call_my_service: + - "call_my_service::CallMyServiceBehaviorsLoader" diff --git a/src/example_behaviors/call_my_service_solution/call_my_service_plugin_description.xml b/src/example_behaviors/call_my_service_solution/call_my_service_plugin_description.xml new file mode 100644 index 00000000..b90d9748 --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/call_my_service_plugin_description.xml @@ -0,0 +1,5 @@ + + + + diff --git a/src/example_behaviors/call_my_service_solution/config/tree_nodes_model.xml b/src/example_behaviors/call_my_service_solution/config/tree_nodes_model.xml new file mode 100644 index 00000000..d3d820a0 --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/config/tree_nodes_model.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/src/example_behaviors/call_my_service_solution/include/call_my_service/call_my_service.hpp b/src/example_behaviors/call_my_service_solution/include/call_my_service/call_my_service.hpp new file mode 100644 index 00000000..8ee2f33d --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/include/call_my_service/call_my_service.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include +#include + +using moveit_studio::behaviors::BehaviorContext; +using moveit_studio::behaviors::ServiceClientBehaviorBase; +using ServiceInterface = service_interface::srv::ServiceInterface; + +namespace call_my_service +{ +class CallMyService final : public ServiceClientBehaviorBase +{ +public: + CallMyService(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + static BT::KeyValueVector metadata(); + + static BT::PortsList providedPorts(); + +private: + /** @brief User-provided function to get the name of the service when initializing the service client. */ + tl::expected getServiceName() override; + + /** @brief User-provided function to create the service request. */ + tl::expected createRequest() override; + + /** @brief Optional user-provided function to process the service response after the service has finished. */ + tl::expected processResponse(const ServiceInterface::Response& response) override; + + /** @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 call_my_service diff --git a/src/example_behaviors/call_my_service_solution/package.xml b/src/example_behaviors/call_my_service_solution/package.xml new file mode 100644 index 00000000..39b0da34 --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/package.xml @@ -0,0 +1,25 @@ + + + call_my_service + 6.0.0 + Example of a Behavior that calls a ROS 2 Service + + MoveIt Pro Maintainer + BSD-3-Clause + + ament_cmake + + moveit_studio_common + service_interface + + moveit_studio_behavior_interface + + ament_lint_auto + ament_cmake_gtest + ament_clang_format + ament_clang_tidy + + + ament_cmake + + diff --git a/src/example_behaviors/call_my_service_solution/src/call_my_service.cpp b/src/example_behaviors/call_my_service_solution/src/call_my_service.cpp new file mode 100644 index 00000000..5279126b --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/src/call_my_service.cpp @@ -0,0 +1,48 @@ +#include + +// Include the template implementation for ServiceClientBehaviorBase. +#include + +namespace call_my_service +{ +CallMyService::CallMyService( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : ServiceClientBehaviorBase(name, config, shared_resources) +{ +} + +BT::PortsList CallMyService::providedPorts() +{ + // This node has three input ports and one output port + return BT::PortsList({ + BT::InputPort("service_name"), + BT::OutputPort("result"), + }); +} + +BT::KeyValueVector CallMyService::metadata() +{ + return { { "subcategory", "Example" }, + { "description", "Example of calling a ROS2 service." } }; +} + +tl::expected CallMyService::getServiceName() +{ + const auto service_name = getInput("service_name"); + if (const auto error = moveit_studio::behaviors::maybe_error(service_name)) + { + return tl::make_unexpected("Failed to get required value from input data port: " + error.value()); + } + return service_name.value(); +} + +tl::expected CallMyService::createRequest(){ + return service_interface::build(); +} + +tl::expected CallMyService::processResponse(const ServiceInterface::Response& response){ + setOutput("result", response.success); + return { response.success }; +} +} // namespace call_my_service diff --git a/src/example_behaviors/call_my_service_solution/src/register_behaviors.cpp b/src/example_behaviors/call_my_service_solution/src/register_behaviors.cpp new file mode 100644 index 00000000..26eef3ba --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace call_my_service +{ +class CallMyServiceBehaviorsLoader : 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, "CallMyService", shared_resources); + + } +}; +} // namespace call_my_service + +PLUGINLIB_EXPORT_CLASS(call_my_service::CallMyServiceBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/example_behaviors/call_my_service_solution/test/CMakeLists.txt b/src/example_behaviors/call_my_service_solution/test/CMakeLists.txt new file mode 100644 index 00000000..8db41205 --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/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/example_behaviors/call_my_service_solution/test/test_behavior_plugins.cpp b/src/example_behaviors/call_my_service_solution/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..ac6ec11d --- /dev/null +++ b/src/example_behaviors/call_my_service_solution/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("test_node"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("call_my_service::CallMyServiceBehaviorsLoader"); + 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", "CallMyService", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/example_behaviors/service_example/package.xml b/src/example_behaviors/service_example/package.xml new file mode 100644 index 00000000..2873c137 --- /dev/null +++ b/src/example_behaviors/service_example/package.xml @@ -0,0 +1,22 @@ + + + + service_example + 6.0.0 + A basic example exposing a ROS 2 Service + + MoveIt Pro Maintainer + BSD-3-Clause + + rclpy + service_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/example_behaviors/service_example/resource/service_example b/src/example_behaviors/service_example/resource/service_example new file mode 100644 index 00000000..e69de29b diff --git a/src/example_behaviors/service_example/service_example/__init__.py b/src/example_behaviors/service_example/service_example/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/example_behaviors/service_example/service_example/service.py b/src/example_behaviors/service_example/service_example/service.py new file mode 100644 index 00000000..39432a13 --- /dev/null +++ b/src/example_behaviors/service_example/service_example/service.py @@ -0,0 +1,32 @@ +from service_interface.srv import ServiceInterface + +import rclpy +from rclpy.node import Node + + +class ExampleService(Node): + def __init__(self): + super().__init__("example_service") + self.srv = self.create_service( + ServiceInterface, "example_service", self.callback + ) + + def callback(self, request, response): + response.success = True + self.get_logger().info("ExampleService called and returned true") + + return response + + +def main(args=None): + rclpy.init(args=args) + + example_service = ExampleService() + + rclpy.spin(example_service) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/example_behaviors/service_example/setup.cfg b/src/example_behaviors/service_example/setup.cfg new file mode 100644 index 00000000..2c94bd9f --- /dev/null +++ b/src/example_behaviors/service_example/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/service_example +[install] +install_scripts=$base/lib/service_example diff --git a/src/example_behaviors/service_example/setup.py b/src/example_behaviors/service_example/setup.py new file mode 100644 index 00000000..0ec47f10 --- /dev/null +++ b/src/example_behaviors/service_example/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = "service_example" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="marioprats", + maintainer_email="mario.prats@picknik.ai", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "service = service_example.service:main", + ], + }, +) diff --git a/src/example_behaviors/service_example/test/test_copyright.py b/src/example_behaviors/service_example/test/test_copyright.py new file mode 100644 index 00000000..ceffe896 --- /dev/null +++ b/src/example_behaviors/service_example/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/src/example_behaviors/service_example/test/test_flake8.py b/src/example_behaviors/service_example/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/src/example_behaviors/service_example/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/src/example_behaviors/service_example/test/test_pep257.py b/src/example_behaviors/service_example/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/src/example_behaviors/service_example/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/src/example_behaviors/service_interface/CMakeLists.txt b/src/example_behaviors/service_interface/CMakeLists.txt new file mode 100644 index 00000000..35ec0393 --- /dev/null +++ b/src/example_behaviors/service_interface/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(service_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/ServiceInterface.srv" +) + +ament_package() diff --git a/src/example_behaviors/service_interface/package.xml b/src/example_behaviors/service_interface/package.xml new file mode 100644 index 00000000..d594abe2 --- /dev/null +++ b/src/example_behaviors/service_interface/package.xml @@ -0,0 +1,24 @@ + + + + service_interface + 6.0.0 + An example ROS2 Service interface + + MoveIt Pro Maintainer + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/example_behaviors/service_interface/srv/ServiceInterface.srv b/src/example_behaviors/service_interface/srv/ServiceInterface.srv new file mode 100644 index 00000000..410e0f9d --- /dev/null +++ b/src/example_behaviors/service_interface/srv/ServiceInterface.srv @@ -0,0 +1,2 @@ +--- +bool success diff --git a/src/example_behaviors/translate_pose_solution/CMakeLists.txt b/src/example_behaviors/translate_pose_solution/CMakeLists.txt new file mode 100644 index 00000000..c9985db2 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.22) +project(translate_pose CXX) + +find_package(moveit_studio_common REQUIRED) +moveit_studio_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + geometry_msgs + moveit_studio_behavior_interface + pluginlib) + +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + translate_pose + SHARED + src/translate_pose.cpp + src/register_behaviors.cpp) +target_include_directories( + translate_pose + PUBLIC $ + $) +ament_target_dependencies(translate_pose + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Install Libraries +install( + TARGETS translate_pose + EXPORT translate_poseTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + add_subdirectory(test) + ament_lint_auto_find_test_dependencies() +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 translate_pose_plugin_description.xml) + +ament_export_targets(translate_poseTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/example_behaviors/translate_pose_solution/behavior_plugin.yaml b/src/example_behaviors/translate_pose_solution/behavior_plugin.yaml new file mode 100644 index 00000000..d1b26b38 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + translate_pose: + - "translate_pose::TranslatePoseBehaviorsLoader" diff --git a/src/example_behaviors/translate_pose_solution/config/tree_nodes_model.xml b/src/example_behaviors/translate_pose_solution/config/tree_nodes_model.xml new file mode 100644 index 00000000..f3844c22 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/config/tree_nodes_model.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/example_behaviors/translate_pose_solution/include/translate_pose/translate_pose.hpp b/src/example_behaviors/translate_pose_solution/include/translate_pose/translate_pose.hpp new file mode 100644 index 00000000..06c81267 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/include/translate_pose/translate_pose.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include + +namespace translate_pose +{ +/** + * @brief Takes a pose from the blackboard and translates it by a given offset. + */ +class TranslatePose : public moveit_studio::behaviors::SharedResourcesNode +{ +public: + /** + * @brief Constructor for the translate_pose 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. + */ + TranslatePose(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + + /** + * @brief Implementation of the required providedPorts() function for the translate_pose 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 translate_pose 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 TranslatePose. + * @details This function is where the Behavior performs its work when the behavior tree is being run. Since + * TranslatePose 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 translate_pose diff --git a/src/example_behaviors/translate_pose_solution/package.xml b/src/example_behaviors/translate_pose_solution/package.xml new file mode 100644 index 00000000..70372dc2 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/package.xml @@ -0,0 +1,26 @@ + + + translate_pose + 6.0.0 + Translates a PoseStamped, given x,y,z + + MoveIt Pro Maintainer + BSD-3-Clause + + ament_cmake + + moveit_studio_common + geometry_msgs + + moveit_studio_behavior_interface + + ament_lint_auto + ament_cmake_gtest + ament_cmake_gmock + ament_clang_format + ament_clang_tidy + + + ament_cmake + + diff --git a/src/example_behaviors/translate_pose_solution/src/register_behaviors.cpp b/src/example_behaviors/translate_pose_solution/src/register_behaviors.cpp new file mode 100644 index 00000000..1f725325 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/src/register_behaviors.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +#include + +#include + +namespace translate_pose +{ +class TranslatePoseBehaviorsLoader : 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, "TranslatePose", shared_resources); + + } +}; +} // namespace translate_pose + +PLUGINLIB_EXPORT_CLASS(translate_pose::TranslatePoseBehaviorsLoader, + moveit_studio::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/example_behaviors/translate_pose_solution/src/translate_pose.cpp b/src/example_behaviors/translate_pose_solution/src/translate_pose.cpp new file mode 100644 index 00000000..50ccf4b3 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/src/translate_pose.cpp @@ -0,0 +1,57 @@ +#include + +#include + +namespace translate_pose +{ +TranslatePose::TranslatePose(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources) +{ +} + +BT::PortsList TranslatePose::providedPorts() +{ + return BT::PortsList({ BT::InputPort("pose_stamped"), BT::InputPort("x"), + BT::InputPort("y"), BT::InputPort("z"), + BT::OutputPort("new_pose") }); +} + +BT::KeyValueVector TranslatePose::metadata() +{ + return { { "description", "Translates a PoseStamped, given x,y,z" } }; +} + +BT::NodeStatus TranslatePose::tick() +{ + shared_resources_->logger->publishWarnMessage("TranslatePose", "Called tick()"); + + auto maybe_pose = getInput("pose_stamped"); + auto maybe_x = getInput("x"); + auto maybe_y = getInput("y"); + auto maybe_z = getInput("z"); + + if (!maybe_pose || !maybe_x || !maybe_y || !maybe_z) + { + shared_resources_->logger->publishFailureMessage("TranslatePose", "Failed to get input data"); + return BT::NodeStatus::FAILURE; + } + + double x = maybe_x.value(); + double y = maybe_y.value(); + double z = maybe_z.value(); + geometry_msgs::msg::PoseStamped pose = maybe_pose.value(); + + shared_resources_->logger->publishWarnMessage("TranslatePose", "Should translate pose by x=" + std::to_string(x) + + ", y=" + std::to_string(y) + + ", z=" + std::to_string(z)); + pose.pose.position.x += x; + pose.pose.position.y += y; + pose.pose.position.z += z; + + setOutput("new_pose", pose); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace translate_pose diff --git a/src/example_behaviors/translate_pose_solution/test/CMakeLists.txt b/src/example_behaviors/translate_pose_solution/test/CMakeLists.txt new file mode 100644 index 00000000..fdbd5c0d --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/test/CMakeLists.txt @@ -0,0 +1,9 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_gmock REQUIRED) + +ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_add_gmock(test_translate_pose test_translate_pose.cpp) +ament_target_dependencies(test_translate_pose ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(test_translate_pose translate_pose) diff --git a/src/example_behaviors/translate_pose_solution/test/test_behavior_plugins.cpp b/src/example_behaviors/translate_pose_solution/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..a54fb4b2 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/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("test_node"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("translate_pose::TranslatePoseBehaviorsLoader"); + 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", "TranslatePose", BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/example_behaviors/translate_pose_solution/test/test_translate_pose.cpp b/src/example_behaviors/translate_pose_solution/test/test_translate_pose.cpp new file mode 100644 index 00000000..4ddcd991 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/test/test_translate_pose.cpp @@ -0,0 +1,57 @@ +#include + +#include + +#include +#include +#include + +namespace translate_pose +{ + +BEGIN_BEHAVIOR_PORT_SETTER_MAP(myExpectedInputPorts) +DEFINE_BEHAVIOR_PORT_SETTER("pose_stamped", geometry_msgs::msg::PoseStamped()) +DEFINE_BEHAVIOR_PORT_SETTER("x", 0.0) +DEFINE_BEHAVIOR_PORT_SETTER("y", 0.0) +DEFINE_BEHAVIOR_PORT_SETTER("z", 0.0) +END_BEHAVIOR_PORT_SETTER_MAP() + +BEGIN_BEHAVIOR_PORT_SETTER_MAP(myExpectedOutputPorts) +DEFINE_BEHAVIOR_PORT_SETTER("new_pose", geometry_msgs::msg::PoseStamped()) +END_BEHAVIOR_PORT_SETTER_MAP() + +class TranslatePoseTest : public moveit_studio::test_utils::RosTest, + public ::moveit_studio::test_utils::WithBehavior +{ + void SetUp() override + { + initBehavior(/*name=*/"TranslatePose", myExpectedInputPorts, myExpectedOutputPorts); + } +}; + +TEST_F(TranslatePoseTest, TranslatePose) { + // Set input ports. + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.pose.position.x = 1.0; + pose_stamped.pose.position.y = 2.0; + pose_stamped.pose.position.z = 3.0; + + blackboard().set("pose_stamped", pose_stamped); + blackboard().set("x", 1.0); + blackboard().set("y", 2.0); + blackboard().set("z", 3.0); + + EXPECT_CALL(mockLogger(), publishWarnMessage(testing::_, testing::HasSubstr("Called tick"))).Times(1); + EXPECT_CALL(mockLogger(), publishWarnMessage(testing::_, testing::HasSubstr("Should translate"))).Times(1); + + // Run behavior. + EXPECT_EQ(behavior().tick(), BT::NodeStatus::SUCCESS); + + // Check output ports. + const geometry_msgs::msg::PoseStamped new_pose = blackboard().get("new_pose"); + EXPECT_EQ(new_pose.pose.position.x, 2.0); + EXPECT_EQ(new_pose.pose.position.y, 4.0); + EXPECT_EQ(new_pose.pose.position.z, 6.0); +} + +} // namespace translate_pose diff --git a/src/example_behaviors/translate_pose_solution/translate_pose_plugin_description.xml b/src/example_behaviors/translate_pose_solution/translate_pose_plugin_description.xml new file mode 100644 index 00000000..b8f282e9 --- /dev/null +++ b/src/example_behaviors/translate_pose_solution/translate_pose_plugin_description.xml @@ -0,0 +1,7 @@ + + + +