From c84e9cd2bab93b55e056fd2f0097fe268a887e19 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 24 Jul 2024 12:02:45 +0200 Subject: [PATCH 01/11] add example service node --- src/service_example/package.xml | 21 ++++++++++++ src/service_example/resource/service_example | 0 .../service_example/__init__.py | 0 .../service_example/service.py | 32 +++++++++++++++++++ src/service_example/setup.cfg | 4 +++ src/service_example/setup.py | 25 +++++++++++++++ src/service_example/test/test_copyright.py | 27 ++++++++++++++++ src/service_example/test/test_flake8.py | 25 +++++++++++++++ src/service_example/test/test_pep257.py | 23 +++++++++++++ src/service_interface/CMakeLists.txt | 15 +++++++++ src/service_interface/package.xml | 23 +++++++++++++ .../srv/ServiceInterface.srv | 2 ++ 12 files changed, 197 insertions(+) create mode 100644 src/service_example/package.xml create mode 100644 src/service_example/resource/service_example create mode 100644 src/service_example/service_example/__init__.py create mode 100644 src/service_example/service_example/service.py create mode 100644 src/service_example/setup.cfg create mode 100644 src/service_example/setup.py create mode 100644 src/service_example/test/test_copyright.py create mode 100644 src/service_example/test/test_flake8.py create mode 100644 src/service_example/test/test_pep257.py create mode 100644 src/service_interface/CMakeLists.txt create mode 100644 src/service_interface/package.xml create mode 100644 src/service_interface/srv/ServiceInterface.srv diff --git a/src/service_example/package.xml b/src/service_example/package.xml new file mode 100644 index 00000000..fb4a337e --- /dev/null +++ b/src/service_example/package.xml @@ -0,0 +1,21 @@ + + + + service_example + 0.0.0 + TODO: Package description + marioprats + TODO: License declaration + + rclpy + service_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/service_example/resource/service_example b/src/service_example/resource/service_example new file mode 100644 index 00000000..e69de29b diff --git a/src/service_example/service_example/__init__.py b/src/service_example/service_example/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/service_example/service_example/service.py b/src/service_example/service_example/service.py new file mode 100644 index 00000000..39432a13 --- /dev/null +++ b/src/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/service_example/setup.cfg b/src/service_example/setup.cfg new file mode 100644 index 00000000..2c94bd9f --- /dev/null +++ b/src/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/service_example/setup.py b/src/service_example/setup.py new file mode 100644 index 00000000..0ec47f10 --- /dev/null +++ b/src/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/service_example/test/test_copyright.py b/src/service_example/test/test_copyright.py new file mode 100644 index 00000000..ceffe896 --- /dev/null +++ b/src/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/service_example/test/test_flake8.py b/src/service_example/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/src/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/service_example/test/test_pep257.py b/src/service_example/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/src/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/service_interface/CMakeLists.txt b/src/service_interface/CMakeLists.txt new file mode 100644 index 00000000..35ec0393 --- /dev/null +++ b/src/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/service_interface/package.xml b/src/service_interface/package.xml new file mode 100644 index 00000000..ad0858d6 --- /dev/null +++ b/src/service_interface/package.xml @@ -0,0 +1,23 @@ + + + + service_interface + 0.0.0 + An example ROS2 Service interface + marioprats + TODO: License declaration + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/service_interface/srv/ServiceInterface.srv b/src/service_interface/srv/ServiceInterface.srv new file mode 100644 index 00000000..410e0f9d --- /dev/null +++ b/src/service_interface/srv/ServiceInterface.srv @@ -0,0 +1,2 @@ +--- +bool success From 4d21d861dbee10bca47d96ff43e2b196c5dff8f6 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 24 Jul 2024 12:37:23 +0200 Subject: [PATCH 02/11] add translate_pose example Behavior --- src/translate_pose_solution/CMakeLists.txt | 54 ++++++++++++++++++ .../behavior_plugin.yaml | 4 ++ .../config/tree_nodes_model.xml | 6 ++ .../include/translate_pose/translate_pose.hpp | 54 ++++++++++++++++++ src/translate_pose_solution/package.xml | 32 +++++++++++ .../src/register_behaviors.cpp | 24 ++++++++ .../src/translate_pose.cpp | 57 +++++++++++++++++++ .../test/CMakeLists.txt | 9 +++ .../test/test_behavior_plugins.cpp | 37 ++++++++++++ .../test/test_translate_pose.cpp | 57 +++++++++++++++++++ .../translate_pose_plugin_description.xml | 7 +++ 11 files changed, 341 insertions(+) create mode 100644 src/translate_pose_solution/CMakeLists.txt create mode 100644 src/translate_pose_solution/behavior_plugin.yaml create mode 100644 src/translate_pose_solution/config/tree_nodes_model.xml create mode 100644 src/translate_pose_solution/include/translate_pose/translate_pose.hpp create mode 100644 src/translate_pose_solution/package.xml create mode 100644 src/translate_pose_solution/src/register_behaviors.cpp create mode 100644 src/translate_pose_solution/src/translate_pose.cpp create mode 100644 src/translate_pose_solution/test/CMakeLists.txt create mode 100644 src/translate_pose_solution/test/test_behavior_plugins.cpp create mode 100644 src/translate_pose_solution/test/test_translate_pose.cpp create mode 100644 src/translate_pose_solution/translate_pose_plugin_description.xml diff --git a/src/translate_pose_solution/CMakeLists.txt b/src/translate_pose_solution/CMakeLists.txt new file mode 100644 index 00000000..c9985db2 --- /dev/null +++ b/src/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/translate_pose_solution/behavior_plugin.yaml b/src/translate_pose_solution/behavior_plugin.yaml new file mode 100644 index 00000000..d1b26b38 --- /dev/null +++ b/src/translate_pose_solution/behavior_plugin.yaml @@ -0,0 +1,4 @@ +objectives: + behavior_loader_plugins: + translate_pose: + - "translate_pose::TranslatePoseBehaviorsLoader" diff --git a/src/translate_pose_solution/config/tree_nodes_model.xml b/src/translate_pose_solution/config/tree_nodes_model.xml new file mode 100644 index 00000000..f3844c22 --- /dev/null +++ b/src/translate_pose_solution/config/tree_nodes_model.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/translate_pose_solution/include/translate_pose/translate_pose.hpp b/src/translate_pose_solution/include/translate_pose/translate_pose.hpp new file mode 100644 index 00000000..fb0c3511 --- /dev/null +++ b/src/translate_pose_solution/include/translate_pose/translate_pose.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include + +namespace translate_pose +{ +/** + * @brief TODO(...) + */ +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/translate_pose_solution/package.xml b/src/translate_pose_solution/package.xml new file mode 100644 index 00000000..a6f334f9 --- /dev/null +++ b/src/translate_pose_solution/package.xml @@ -0,0 +1,32 @@ + + + translate_pose + 0.0.0 + Translates a PoseStamped, given x,y,z + + + John Doe + + + John Doe + + + TODO + + 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/translate_pose_solution/src/register_behaviors.cpp b/src/translate_pose_solution/src/register_behaviors.cpp new file mode 100644 index 00000000..1f725325 --- /dev/null +++ b/src/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/translate_pose_solution/src/translate_pose.cpp b/src/translate_pose_solution/src/translate_pose.cpp new file mode 100644 index 00000000..50ccf4b3 --- /dev/null +++ b/src/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/translate_pose_solution/test/CMakeLists.txt b/src/translate_pose_solution/test/CMakeLists.txt new file mode 100644 index 00000000..fdbd5c0d --- /dev/null +++ b/src/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/translate_pose_solution/test/test_behavior_plugins.cpp b/src/translate_pose_solution/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..a54fb4b2 --- /dev/null +++ b/src/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/translate_pose_solution/test/test_translate_pose.cpp b/src/translate_pose_solution/test/test_translate_pose.cpp new file mode 100644 index 00000000..4ddcd991 --- /dev/null +++ b/src/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/translate_pose_solution/translate_pose_plugin_description.xml b/src/translate_pose_solution/translate_pose_plugin_description.xml new file mode 100644 index 00000000..b8f282e9 --- /dev/null +++ b/src/translate_pose_solution/translate_pose_plugin_description.xml @@ -0,0 +1,7 @@ + + + + From 2976a365cdd3ab0829a38bf29e498aa220d865e4 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 24 Jul 2024 13:04:17 +0200 Subject: [PATCH 03/11] add call_my_service Behavior --- src/call_my_service_solution/CMakeLists.txt | 51 +++++++++++++++++++ .../behavior_plugin.yaml | 4 ++ .../call_my_service_plugin_description.xml | 5 ++ .../config/tree_nodes_model.xml | 5 ++ .../call_my_service/call_my_service.hpp | 41 +++++++++++++++ src/call_my_service_solution/package.xml | 27 ++++++++++ .../src/call_my_service.cpp | 48 +++++++++++++++++ .../src/register_behaviors.cpp | 24 +++++++++ .../test/CMakeLists.txt | 4 ++ .../test/test_behavior_plugins.cpp | 37 ++++++++++++++ 10 files changed, 246 insertions(+) create mode 100644 src/call_my_service_solution/CMakeLists.txt create mode 100644 src/call_my_service_solution/behavior_plugin.yaml create mode 100644 src/call_my_service_solution/call_my_service_plugin_description.xml create mode 100644 src/call_my_service_solution/config/tree_nodes_model.xml create mode 100644 src/call_my_service_solution/include/call_my_service/call_my_service.hpp create mode 100644 src/call_my_service_solution/package.xml create mode 100644 src/call_my_service_solution/src/call_my_service.cpp create mode 100644 src/call_my_service_solution/src/register_behaviors.cpp create mode 100644 src/call_my_service_solution/test/CMakeLists.txt create mode 100644 src/call_my_service_solution/test/test_behavior_plugins.cpp diff --git a/src/call_my_service_solution/CMakeLists.txt b/src/call_my_service_solution/CMakeLists.txt new file mode 100644 index 00000000..21bef3b0 --- /dev/null +++ b/src/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/call_my_service_solution/behavior_plugin.yaml b/src/call_my_service_solution/behavior_plugin.yaml new file mode 100644 index 00000000..9f600878 --- /dev/null +++ b/src/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/call_my_service_solution/call_my_service_plugin_description.xml b/src/call_my_service_solution/call_my_service_plugin_description.xml new file mode 100644 index 00000000..b90d9748 --- /dev/null +++ b/src/call_my_service_solution/call_my_service_plugin_description.xml @@ -0,0 +1,5 @@ + + + + diff --git a/src/call_my_service_solution/config/tree_nodes_model.xml b/src/call_my_service_solution/config/tree_nodes_model.xml new file mode 100644 index 00000000..d3d820a0 --- /dev/null +++ b/src/call_my_service_solution/config/tree_nodes_model.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/src/call_my_service_solution/include/call_my_service/call_my_service.hpp b/src/call_my_service_solution/include/call_my_service/call_my_service.hpp new file mode 100644 index 00000000..8ee2f33d --- /dev/null +++ b/src/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/call_my_service_solution/package.xml b/src/call_my_service_solution/package.xml new file mode 100644 index 00000000..54253605 --- /dev/null +++ b/src/call_my_service_solution/package.xml @@ -0,0 +1,27 @@ + + + call_my_service + 0.0.0 + Example of a Behavior that calls a ROS2 Service + + John Doe + John Doe + + TODO + + 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/call_my_service_solution/src/call_my_service.cpp b/src/call_my_service_solution/src/call_my_service.cpp new file mode 100644 index 00000000..5279126b --- /dev/null +++ b/src/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/call_my_service_solution/src/register_behaviors.cpp b/src/call_my_service_solution/src/register_behaviors.cpp new file mode 100644 index 00000000..26eef3ba --- /dev/null +++ b/src/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/call_my_service_solution/test/CMakeLists.txt b/src/call_my_service_solution/test/CMakeLists.txt new file mode 100644 index 00000000..8db41205 --- /dev/null +++ b/src/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/call_my_service_solution/test/test_behavior_plugins.cpp b/src/call_my_service_solution/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..ac6ec11d --- /dev/null +++ b/src/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(); +} From 49b775d451de000609803f80a87bc905e5c7e368 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 24 Jul 2024 13:36:31 +0200 Subject: [PATCH 04/11] add COLCON_IGNORE to solutions --- src/call_my_service_solution/COLCON_IGNORE | 0 src/translate_pose_solution/COLCON_IGNORE | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/call_my_service_solution/COLCON_IGNORE create mode 100644 src/translate_pose_solution/COLCON_IGNORE diff --git a/src/call_my_service_solution/COLCON_IGNORE b/src/call_my_service_solution/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/src/translate_pose_solution/COLCON_IGNORE b/src/translate_pose_solution/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b From 5238d46464a876c90984941e2c88e02da662ec13 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 8 Aug 2024 19:40:16 +0200 Subject: [PATCH 05/11] Update deprecated param (#336) --- .gitmodules | 1 + src/external_dependencies/ros2_robotiq_gripper | 2 +- src/picknik_ur_base_config/config/config.yaml | 2 +- src/picknik_ur_multi_arm_config/config/config.yaml | 2 +- 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index b7bb5704..ddadb4d4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "src/external_dependencies/ros2_robotiq_gripper"] path = src/external_dependencies/ros2_robotiq_gripper url = https://github.com/PickNikRobotics/ros2_robotiq_gripper.git + branch = humble [submodule "src/external_dependencies/picknik_accessories"] path = src/external_dependencies/picknik_accessories url = https://github.com/PickNikRobotics/picknik_accessories.git diff --git a/src/external_dependencies/ros2_robotiq_gripper b/src/external_dependencies/ros2_robotiq_gripper index b6136bdc..93ecde7f 160000 --- a/src/external_dependencies/ros2_robotiq_gripper +++ b/src/external_dependencies/ros2_robotiq_gripper @@ -1 +1 @@ -Subproject commit b6136bdc866a929bfb096890ca61dde1335ffd30 +Subproject commit 93ecde7f66499d56e620d775eb984345889532aa diff --git a/src/picknik_ur_base_config/config/config.yaml b/src/picknik_ur_base_config/config/config.yaml index d2cb9ca2..5d09c7c9 100644 --- a/src/picknik_ur_base_config/config/config.yaml +++ b/src/picknik_ur_base_config/config/config.yaml @@ -55,7 +55,7 @@ hardware: - prefix: "" - use_fake_hardware: "true" - use_pinch_links: "true" - - fake_sensor_commands: "false" + - mock_sensor_commands: "false" - headless_mode: "true" - robot_ip: "0.0.0.0" - joint_limits_parameters_file: diff --git a/src/picknik_ur_multi_arm_config/config/config.yaml b/src/picknik_ur_multi_arm_config/config/config.yaml index c56f3a6d..d60f498b 100644 --- a/src/picknik_ur_multi_arm_config/config/config.yaml +++ b/src/picknik_ur_multi_arm_config/config/config.yaml @@ -47,7 +47,7 @@ hardware: - prefix: "" - use_fake_hardware: "true" - use_pinch_links: "true" - - fake_sensor_commands: "false" + - mock_sensor_commands: "false" - headless_mode: "true" - robot_ip: "0.0.0.0" - joint_limits_parameters_file: From 69ea0c7a8f779b7dc0b4c73f00a0742cb2908f46 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Fri, 9 Aug 2024 06:00:25 +0200 Subject: [PATCH 06/11] Mujoco scene cosmetic improvements (#337) * update scene camera position * rotate scene 90 deg, add adapter to linear rail --------- Co-authored-by: Michael Wrock --- .../description/picknik_ur.xacro | 51 +++- .../description/scene.xml | 10 +- .../description/ur5e.xml | 273 +++++++++--------- 3 files changed, 190 insertions(+), 144 deletions(-) diff --git a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro index de03a457..ca3c56dd 100644 --- a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro +++ b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro @@ -530,7 +530,7 @@ - + - + + + + + + + + @@ -727,7 +734,7 @@ - + @@ -877,6 +884,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -887,7 +930,7 @@ - + diff --git a/src/picknik_ur_mujoco_config/description/scene.xml b/src/picknik_ur_mujoco_config/description/scene.xml index 1d2da458..dc0a1664 100644 --- a/src/picknik_ur_mujoco_config/description/scene.xml +++ b/src/picknik_ur_mujoco_config/description/scene.xml @@ -41,9 +41,9 @@ - + - + @@ -74,9 +74,9 @@ - - + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + From a0582b5c4b68c4a8e973ccfc7d09547bb3f00d4b Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Fri, 9 Aug 2024 11:41:12 +0200 Subject: [PATCH 07/11] move behaviors under example_behaviors/ --- .../call_my_service_solution/CMakeLists.txt | 0 .../call_my_service_solution/COLCON_IGNORE | 0 .../call_my_service_solution/behavior_plugin.yaml | 0 .../call_my_service_plugin_description.xml | 0 .../call_my_service_solution/config/tree_nodes_model.xml | 0 .../include/call_my_service/call_my_service.hpp | 0 src/{ => example_behaviors}/call_my_service_solution/package.xml | 0 .../call_my_service_solution/src/call_my_service.cpp | 0 .../call_my_service_solution/src/register_behaviors.cpp | 0 .../call_my_service_solution/test/CMakeLists.txt | 0 .../call_my_service_solution/test/test_behavior_plugins.cpp | 0 src/{ => example_behaviors}/service_example/package.xml | 0 .../service_example/resource/service_example | 0 .../service_example/service_example/__init__.py | 0 .../service_example/service_example/service.py | 0 src/{ => example_behaviors}/service_example/setup.cfg | 0 src/{ => example_behaviors}/service_example/setup.py | 0 .../service_example/test/test_copyright.py | 0 src/{ => example_behaviors}/service_example/test/test_flake8.py | 0 src/{ => example_behaviors}/service_example/test/test_pep257.py | 0 src/{ => example_behaviors}/service_interface/CMakeLists.txt | 0 src/{ => example_behaviors}/service_interface/package.xml | 0 .../service_interface/srv/ServiceInterface.srv | 0 .../translate_pose_solution/CMakeLists.txt | 0 src/{ => example_behaviors}/translate_pose_solution/COLCON_IGNORE | 0 .../translate_pose_solution/behavior_plugin.yaml | 0 .../translate_pose_solution/config/tree_nodes_model.xml | 0 .../include/translate_pose/translate_pose.hpp | 0 src/{ => example_behaviors}/translate_pose_solution/package.xml | 0 .../translate_pose_solution/src/register_behaviors.cpp | 0 .../translate_pose_solution/src/translate_pose.cpp | 0 .../translate_pose_solution/test/CMakeLists.txt | 0 .../translate_pose_solution/test/test_behavior_plugins.cpp | 0 .../translate_pose_solution/test/test_translate_pose.cpp | 0 .../translate_pose_solution/translate_pose_plugin_description.xml | 0 35 files changed, 0 insertions(+), 0 deletions(-) rename src/{ => example_behaviors}/call_my_service_solution/CMakeLists.txt (100%) rename src/{ => example_behaviors}/call_my_service_solution/COLCON_IGNORE (100%) rename src/{ => example_behaviors}/call_my_service_solution/behavior_plugin.yaml (100%) rename src/{ => example_behaviors}/call_my_service_solution/call_my_service_plugin_description.xml (100%) rename src/{ => example_behaviors}/call_my_service_solution/config/tree_nodes_model.xml (100%) rename src/{ => example_behaviors}/call_my_service_solution/include/call_my_service/call_my_service.hpp (100%) rename src/{ => example_behaviors}/call_my_service_solution/package.xml (100%) rename src/{ => example_behaviors}/call_my_service_solution/src/call_my_service.cpp (100%) rename src/{ => example_behaviors}/call_my_service_solution/src/register_behaviors.cpp (100%) rename src/{ => example_behaviors}/call_my_service_solution/test/CMakeLists.txt (100%) rename src/{ => example_behaviors}/call_my_service_solution/test/test_behavior_plugins.cpp (100%) rename src/{ => example_behaviors}/service_example/package.xml (100%) rename src/{ => example_behaviors}/service_example/resource/service_example (100%) rename src/{ => example_behaviors}/service_example/service_example/__init__.py (100%) rename src/{ => example_behaviors}/service_example/service_example/service.py (100%) rename src/{ => example_behaviors}/service_example/setup.cfg (100%) rename src/{ => example_behaviors}/service_example/setup.py (100%) rename src/{ => example_behaviors}/service_example/test/test_copyright.py (100%) rename src/{ => example_behaviors}/service_example/test/test_flake8.py (100%) rename src/{ => example_behaviors}/service_example/test/test_pep257.py (100%) rename src/{ => example_behaviors}/service_interface/CMakeLists.txt (100%) rename src/{ => example_behaviors}/service_interface/package.xml (100%) rename src/{ => example_behaviors}/service_interface/srv/ServiceInterface.srv (100%) rename src/{ => example_behaviors}/translate_pose_solution/CMakeLists.txt (100%) rename src/{ => example_behaviors}/translate_pose_solution/COLCON_IGNORE (100%) rename src/{ => example_behaviors}/translate_pose_solution/behavior_plugin.yaml (100%) rename src/{ => example_behaviors}/translate_pose_solution/config/tree_nodes_model.xml (100%) rename src/{ => example_behaviors}/translate_pose_solution/include/translate_pose/translate_pose.hpp (100%) rename src/{ => example_behaviors}/translate_pose_solution/package.xml (100%) rename src/{ => example_behaviors}/translate_pose_solution/src/register_behaviors.cpp (100%) rename src/{ => example_behaviors}/translate_pose_solution/src/translate_pose.cpp (100%) rename src/{ => example_behaviors}/translate_pose_solution/test/CMakeLists.txt (100%) rename src/{ => example_behaviors}/translate_pose_solution/test/test_behavior_plugins.cpp (100%) rename src/{ => example_behaviors}/translate_pose_solution/test/test_translate_pose.cpp (100%) rename src/{ => example_behaviors}/translate_pose_solution/translate_pose_plugin_description.xml (100%) diff --git a/src/call_my_service_solution/CMakeLists.txt b/src/example_behaviors/call_my_service_solution/CMakeLists.txt similarity index 100% rename from src/call_my_service_solution/CMakeLists.txt rename to src/example_behaviors/call_my_service_solution/CMakeLists.txt diff --git a/src/call_my_service_solution/COLCON_IGNORE b/src/example_behaviors/call_my_service_solution/COLCON_IGNORE similarity index 100% rename from src/call_my_service_solution/COLCON_IGNORE rename to src/example_behaviors/call_my_service_solution/COLCON_IGNORE diff --git a/src/call_my_service_solution/behavior_plugin.yaml b/src/example_behaviors/call_my_service_solution/behavior_plugin.yaml similarity index 100% rename from src/call_my_service_solution/behavior_plugin.yaml rename to src/example_behaviors/call_my_service_solution/behavior_plugin.yaml diff --git a/src/call_my_service_solution/call_my_service_plugin_description.xml b/src/example_behaviors/call_my_service_solution/call_my_service_plugin_description.xml similarity index 100% rename from src/call_my_service_solution/call_my_service_plugin_description.xml rename to src/example_behaviors/call_my_service_solution/call_my_service_plugin_description.xml diff --git a/src/call_my_service_solution/config/tree_nodes_model.xml b/src/example_behaviors/call_my_service_solution/config/tree_nodes_model.xml similarity index 100% rename from src/call_my_service_solution/config/tree_nodes_model.xml rename to src/example_behaviors/call_my_service_solution/config/tree_nodes_model.xml diff --git a/src/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 similarity index 100% rename from src/call_my_service_solution/include/call_my_service/call_my_service.hpp rename to src/example_behaviors/call_my_service_solution/include/call_my_service/call_my_service.hpp diff --git a/src/call_my_service_solution/package.xml b/src/example_behaviors/call_my_service_solution/package.xml similarity index 100% rename from src/call_my_service_solution/package.xml rename to src/example_behaviors/call_my_service_solution/package.xml diff --git a/src/call_my_service_solution/src/call_my_service.cpp b/src/example_behaviors/call_my_service_solution/src/call_my_service.cpp similarity index 100% rename from src/call_my_service_solution/src/call_my_service.cpp rename to src/example_behaviors/call_my_service_solution/src/call_my_service.cpp diff --git a/src/call_my_service_solution/src/register_behaviors.cpp b/src/example_behaviors/call_my_service_solution/src/register_behaviors.cpp similarity index 100% rename from src/call_my_service_solution/src/register_behaviors.cpp rename to src/example_behaviors/call_my_service_solution/src/register_behaviors.cpp diff --git a/src/call_my_service_solution/test/CMakeLists.txt b/src/example_behaviors/call_my_service_solution/test/CMakeLists.txt similarity index 100% rename from src/call_my_service_solution/test/CMakeLists.txt rename to src/example_behaviors/call_my_service_solution/test/CMakeLists.txt diff --git a/src/call_my_service_solution/test/test_behavior_plugins.cpp b/src/example_behaviors/call_my_service_solution/test/test_behavior_plugins.cpp similarity index 100% rename from src/call_my_service_solution/test/test_behavior_plugins.cpp rename to src/example_behaviors/call_my_service_solution/test/test_behavior_plugins.cpp diff --git a/src/service_example/package.xml b/src/example_behaviors/service_example/package.xml similarity index 100% rename from src/service_example/package.xml rename to src/example_behaviors/service_example/package.xml diff --git a/src/service_example/resource/service_example b/src/example_behaviors/service_example/resource/service_example similarity index 100% rename from src/service_example/resource/service_example rename to src/example_behaviors/service_example/resource/service_example diff --git a/src/service_example/service_example/__init__.py b/src/example_behaviors/service_example/service_example/__init__.py similarity index 100% rename from src/service_example/service_example/__init__.py rename to src/example_behaviors/service_example/service_example/__init__.py diff --git a/src/service_example/service_example/service.py b/src/example_behaviors/service_example/service_example/service.py similarity index 100% rename from src/service_example/service_example/service.py rename to src/example_behaviors/service_example/service_example/service.py diff --git a/src/service_example/setup.cfg b/src/example_behaviors/service_example/setup.cfg similarity index 100% rename from src/service_example/setup.cfg rename to src/example_behaviors/service_example/setup.cfg diff --git a/src/service_example/setup.py b/src/example_behaviors/service_example/setup.py similarity index 100% rename from src/service_example/setup.py rename to src/example_behaviors/service_example/setup.py diff --git a/src/service_example/test/test_copyright.py b/src/example_behaviors/service_example/test/test_copyright.py similarity index 100% rename from src/service_example/test/test_copyright.py rename to src/example_behaviors/service_example/test/test_copyright.py diff --git a/src/service_example/test/test_flake8.py b/src/example_behaviors/service_example/test/test_flake8.py similarity index 100% rename from src/service_example/test/test_flake8.py rename to src/example_behaviors/service_example/test/test_flake8.py diff --git a/src/service_example/test/test_pep257.py b/src/example_behaviors/service_example/test/test_pep257.py similarity index 100% rename from src/service_example/test/test_pep257.py rename to src/example_behaviors/service_example/test/test_pep257.py diff --git a/src/service_interface/CMakeLists.txt b/src/example_behaviors/service_interface/CMakeLists.txt similarity index 100% rename from src/service_interface/CMakeLists.txt rename to src/example_behaviors/service_interface/CMakeLists.txt diff --git a/src/service_interface/package.xml b/src/example_behaviors/service_interface/package.xml similarity index 100% rename from src/service_interface/package.xml rename to src/example_behaviors/service_interface/package.xml diff --git a/src/service_interface/srv/ServiceInterface.srv b/src/example_behaviors/service_interface/srv/ServiceInterface.srv similarity index 100% rename from src/service_interface/srv/ServiceInterface.srv rename to src/example_behaviors/service_interface/srv/ServiceInterface.srv diff --git a/src/translate_pose_solution/CMakeLists.txt b/src/example_behaviors/translate_pose_solution/CMakeLists.txt similarity index 100% rename from src/translate_pose_solution/CMakeLists.txt rename to src/example_behaviors/translate_pose_solution/CMakeLists.txt diff --git a/src/translate_pose_solution/COLCON_IGNORE b/src/example_behaviors/translate_pose_solution/COLCON_IGNORE similarity index 100% rename from src/translate_pose_solution/COLCON_IGNORE rename to src/example_behaviors/translate_pose_solution/COLCON_IGNORE diff --git a/src/translate_pose_solution/behavior_plugin.yaml b/src/example_behaviors/translate_pose_solution/behavior_plugin.yaml similarity index 100% rename from src/translate_pose_solution/behavior_plugin.yaml rename to src/example_behaviors/translate_pose_solution/behavior_plugin.yaml diff --git a/src/translate_pose_solution/config/tree_nodes_model.xml b/src/example_behaviors/translate_pose_solution/config/tree_nodes_model.xml similarity index 100% rename from src/translate_pose_solution/config/tree_nodes_model.xml rename to src/example_behaviors/translate_pose_solution/config/tree_nodes_model.xml diff --git a/src/translate_pose_solution/include/translate_pose/translate_pose.hpp b/src/example_behaviors/translate_pose_solution/include/translate_pose/translate_pose.hpp similarity index 100% rename from src/translate_pose_solution/include/translate_pose/translate_pose.hpp rename to src/example_behaviors/translate_pose_solution/include/translate_pose/translate_pose.hpp diff --git a/src/translate_pose_solution/package.xml b/src/example_behaviors/translate_pose_solution/package.xml similarity index 100% rename from src/translate_pose_solution/package.xml rename to src/example_behaviors/translate_pose_solution/package.xml diff --git a/src/translate_pose_solution/src/register_behaviors.cpp b/src/example_behaviors/translate_pose_solution/src/register_behaviors.cpp similarity index 100% rename from src/translate_pose_solution/src/register_behaviors.cpp rename to src/example_behaviors/translate_pose_solution/src/register_behaviors.cpp diff --git a/src/translate_pose_solution/src/translate_pose.cpp b/src/example_behaviors/translate_pose_solution/src/translate_pose.cpp similarity index 100% rename from src/translate_pose_solution/src/translate_pose.cpp rename to src/example_behaviors/translate_pose_solution/src/translate_pose.cpp diff --git a/src/translate_pose_solution/test/CMakeLists.txt b/src/example_behaviors/translate_pose_solution/test/CMakeLists.txt similarity index 100% rename from src/translate_pose_solution/test/CMakeLists.txt rename to src/example_behaviors/translate_pose_solution/test/CMakeLists.txt diff --git a/src/translate_pose_solution/test/test_behavior_plugins.cpp b/src/example_behaviors/translate_pose_solution/test/test_behavior_plugins.cpp similarity index 100% rename from src/translate_pose_solution/test/test_behavior_plugins.cpp rename to src/example_behaviors/translate_pose_solution/test/test_behavior_plugins.cpp diff --git a/src/translate_pose_solution/test/test_translate_pose.cpp b/src/example_behaviors/translate_pose_solution/test/test_translate_pose.cpp similarity index 100% rename from src/translate_pose_solution/test/test_translate_pose.cpp rename to src/example_behaviors/translate_pose_solution/test/test_translate_pose.cpp diff --git a/src/translate_pose_solution/translate_pose_plugin_description.xml b/src/example_behaviors/translate_pose_solution/translate_pose_plugin_description.xml similarity index 100% rename from src/translate_pose_solution/translate_pose_plugin_description.xml rename to src/example_behaviors/translate_pose_solution/translate_pose_plugin_description.xml From 55f2630d945f664e209a78b99bb7acb270a4709d Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Fri, 9 Aug 2024 11:52:57 +0200 Subject: [PATCH 08/11] build behaviors so that they are checked in CI --- src/example_behaviors/call_my_service_solution/COLCON_IGNORE | 0 src/example_behaviors/translate_pose_solution/COLCON_IGNORE | 0 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/example_behaviors/call_my_service_solution/COLCON_IGNORE delete mode 100644 src/example_behaviors/translate_pose_solution/COLCON_IGNORE diff --git a/src/example_behaviors/call_my_service_solution/COLCON_IGNORE b/src/example_behaviors/call_my_service_solution/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/src/example_behaviors/translate_pose_solution/COLCON_IGNORE b/src/example_behaviors/translate_pose_solution/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 From 126ff4e5f7e818fa09ac5501c771cc8f3bdd6d06 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Fri, 9 Aug 2024 11:53:34 +0200 Subject: [PATCH 09/11] update semver and other package.xml info --- .../call_my_service_solution/package.xml | 10 ++++------ src/example_behaviors/service_example/package.xml | 9 +++++---- .../service_interface/package.xml | 7 ++++--- .../translate_pose_solution/package.xml | 14 ++++---------- 4 files changed, 17 insertions(+), 23 deletions(-) diff --git a/src/example_behaviors/call_my_service_solution/package.xml b/src/example_behaviors/call_my_service_solution/package.xml index 54253605..39b0da34 100644 --- a/src/example_behaviors/call_my_service_solution/package.xml +++ b/src/example_behaviors/call_my_service_solution/package.xml @@ -1,13 +1,11 @@ call_my_service - 0.0.0 - Example of a Behavior that calls a ROS2 Service + 6.0.0 + Example of a Behavior that calls a ROS 2 Service - John Doe - John Doe - - TODO + MoveIt Pro Maintainer + BSD-3-Clause ament_cmake diff --git a/src/example_behaviors/service_example/package.xml b/src/example_behaviors/service_example/package.xml index fb4a337e..2873c137 100644 --- a/src/example_behaviors/service_example/package.xml +++ b/src/example_behaviors/service_example/package.xml @@ -2,10 +2,11 @@ service_example - 0.0.0 - TODO: Package description - marioprats - TODO: License declaration + 6.0.0 + A basic example exposing a ROS 2 Service + + MoveIt Pro Maintainer + BSD-3-Clause rclpy service_interface diff --git a/src/example_behaviors/service_interface/package.xml b/src/example_behaviors/service_interface/package.xml index ad0858d6..d594abe2 100644 --- a/src/example_behaviors/service_interface/package.xml +++ b/src/example_behaviors/service_interface/package.xml @@ -2,10 +2,11 @@ service_interface - 0.0.0 + 6.0.0 An example ROS2 Service interface - marioprats - TODO: License declaration + + MoveIt Pro Maintainer + BSD-3-Clause ament_cmake rosidl_default_generators diff --git a/src/example_behaviors/translate_pose_solution/package.xml b/src/example_behaviors/translate_pose_solution/package.xml index a6f334f9..70372dc2 100644 --- a/src/example_behaviors/translate_pose_solution/package.xml +++ b/src/example_behaviors/translate_pose_solution/package.xml @@ -1,18 +1,12 @@ translate_pose - 0.0.0 + 6.0.0 Translates a PoseStamped, given x,y,z - - John Doe - - - John Doe - - - TODO - + MoveIt Pro Maintainer + BSD-3-Clause + ament_cmake moveit_studio_common From 31e8dff246b7605d03c6658b4d3476ff6be46299 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Fri, 9 Aug 2024 11:53:51 +0200 Subject: [PATCH 10/11] fix comment --- .../include/translate_pose/translate_pose.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index fb0c3511..06c81267 100644 --- 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 @@ -7,7 +7,7 @@ namespace translate_pose { /** - * @brief TODO(...) + * @brief Takes a pose from the blackboard and translates it by a given offset. */ class TranslatePose : public moveit_studio::behaviors::SharedResourcesNode { From 3727c564a4239754305915622b0d07c66f290230 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 14 Aug 2024 14:13:34 +0200 Subject: [PATCH 11/11] revert changes to picknik_ur.xacro --- .../description/picknik_ur.xacro | 36 ------------------- 1 file changed, 36 deletions(-) diff --git a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro index f71fb907..1a5415e6 100644 --- a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro +++ b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro @@ -926,42 +926,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -