From b36bd0e98b8fb03b656560b4c54b17cc2748a9d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Sat, 29 Nov 2025 00:40:55 +1000 Subject: [PATCH] =?UTF-8?q?=D0=98=D0=B7=D0=BC=D0=B8=D0=BD=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D1=8F:=20-=20=D0=92=20=D1=84=D0=B0=D0=B9=D0=BB=D0=B5=20`?= =?UTF-8?q?webots=5Fspawn.launch.py`=20-=20=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=D0=BE=20=D0=B8=D1=81=D0=BF=D0=BE=D0=BB=D1=8C?= =?UTF-8?q?=D0=B7=D0=BE=D0=B2=D0=B0=D0=BD=D0=B8=D0=B5=20=D0=B2=D1=80=D0=B5?= =?UTF-8?q?=D0=BC=D0=B5=D0=BD=D0=B8=20=D1=81=D0=B8=D0=BC=D1=83=D0=BB=D1=8F?= =?UTF-8?q?=D1=86=D0=B8=D0=B8=20-=20=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=20=D0=BD=D0=BE=D0=B2=D1=8B=D0=B9=20=D0=BF=D0=B0?= =?UTF-8?q?=D0=BA=D0=B5=D1=82=20`iiwa=5Fobject=5Fspawne`,=20=D0=BA=D0=BE?= =?UTF-8?q?=D1=82=D0=BE=D1=80=D1=8B=D0=B9=20=D0=BE=D1=82=D0=B2=D0=B5=D1=87?= =?UTF-8?q?=D0=B0=D0=B5=D1=82=20=D0=B7=D0=B0=20=D0=BF=D0=BE=D1=8F=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=D0=B8=D0=B5=20=D0=BE=D0=B1=D1=8A=D0=B5=D0=BA?= =?UTF-8?q?=D1=82=D0=BE=D0=B2=20(=D0=B2=20=D1=80=D0=B0=D0=B7=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=BE=D1=82=D0=BA=D0=B5)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 17 +- .../launch/supported/webots_spawn.launch.py | 2 +- src/iiwa_object_spawner/LICENSE | 202 ++++++++++++++++++ .../iiwa_object_spawner/__init__.py | 0 .../iiwa_object_spawner/object_spawner.py | 74 +++++++ src/iiwa_object_spawner/package.xml | 19 ++ .../resource/iiwa_object_spawner | 0 src/iiwa_object_spawner/setup.cfg | 4 + src/iiwa_object_spawner/setup.py | 25 +++ 9 files changed, 341 insertions(+), 2 deletions(-) create mode 100644 src/iiwa_object_spawner/LICENSE create mode 100644 src/iiwa_object_spawner/iiwa_object_spawner/__init__.py create mode 100644 src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py create mode 100644 src/iiwa_object_spawner/package.xml create mode 100644 src/iiwa_object_spawner/resource/iiwa_object_spawner create mode 100644 src/iiwa_object_spawner/setup.cfg create mode 100644 src/iiwa_object_spawner/setup.py diff --git a/README.md b/README.md index 1fc89c2..3151952 100644 --- a/README.md +++ b/README.md @@ -22,10 +22,25 @@ sudo apt install -y build-essential \ python3-vcstool \ wget +# Not Using git clone https://github.com/moveit/moveit2.git vcs import --recursive < moveit2/moveit2.repos sudo apt remove ros-$ROS_DISTRO-moveit* rosdep install -r --from-paths ./src/ --ignore-src --rosdistro $ROS_DISTRO --os=ubuntu:noble -y colcon build --mixin release -``` \ No newline at end of file + +sudo pip3 install transforms3d --break-system-packages +``` + +Спавн объекта: +```bash +{ + "data": "Solid { name \"test_box2\" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }" +} +``` + +Спавн `.proto`: + + +Можно заготовить готовые `.proto` файлы, и потом случайно спавнить объект по такому принципу + создать Node который будет вызываться и спавнить этот объекты. Может быть шаблон куда потом подставятся данные через `.format()`. По такому же принципу спавн человека. Остается понять, только задать область спавна относительно робта \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index a92feb6..e6829ba 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -33,7 +33,7 @@ def _spawn_setup(context, *args, **kwargs): parameters=[ { "robot_description": model_path, - "use_sim_time": False, + "use_sim_time": True, "set_robot_state_publisher": False }, LaunchConfiguration('controller').perform(context) diff --git a/src/iiwa_object_spawner/LICENSE b/src/iiwa_object_spawner/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/iiwa_object_spawner/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/__init__.py b/src/iiwa_object_spawner/iiwa_object_spawner/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py b/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py new file mode 100644 index 0000000..1966a28 --- /dev/null +++ b/src/iiwa_object_spawner/iiwa_object_spawner/object_spawner.py @@ -0,0 +1,74 @@ +# /Ros2Supervisor/spawn_node_from_string / webots_ros2_msgs/srv/SpawnNodeFromString +# /Ros2Supervisor/spawn_urdf_robot + + +import rclpy +from random import randint +from rclpy.node import Node +from rclpy.task import Future +from rclpy.executors import ExternalShutdownException +from webots_ros2_msgs.srv import SpawnNodeFromString + + +class ObjectSpawner(Node): + def __init__(self): + super().__init__("object_spawner") + # self._object_count: int = randint(1, 5) + self._object_count: int = 1 + self._spawned_count: int = 0 + self._call_in_progress: bool = False + + + self.cli = self.create_client(SpawnNodeFromString, + "/Ros2Supervisor/spawn_node_from_string") + + while not self.cli.wait_for_service(timeout_sec=10): + self.get_logger().warning( + "service /Ros2Supervisor/spawn_node_from_string not available, waiting again..." + ) + + self._timer = self.create_timer(0.1, self.timer_callback) + + def timer_callback(self): + if self._spawned_count >= self._object_count: + self.get_logger().info( + f"All {self._object_count} objects spawned" + ) + self._timer.cancel() + return + + if self._call_in_progress: + return + + data = "Solid { name \"test_box2\" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }" + + req = SpawnNodeFromString.Request(data=data, + check_fields=True) + self.get_logger().info( + f"Calling spawn service #{self._spawned_count+1}/{self._object_count}" + ) + self._call_in_progress = True + future = self.cli.call_async(req) + future.add_done_callback(self._response_callback) + + def _response_callback(self, future: Future): + try: + response = future.result() + self.get_logger().info(f"Spawn service response: {response}") + except Exception as e: + self.get_logger().error(f"Service call field: {e}") + finally: + self._call_in_progress = False + self._spawned_count += 1 + + +def main(args=None): + try: + with rclpy.init(args=args): + node = ObjectSpawner() + rclpy.spin(node=node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + +if __name__ == '__main__': + main() diff --git a/src/iiwa_object_spawner/package.xml b/src/iiwa_object_spawner/package.xml new file mode 100644 index 0000000..93d80d7 --- /dev/null +++ b/src/iiwa_object_spawner/package.xml @@ -0,0 +1,19 @@ + + + + iiwa_object_spawner + 0.0.1 + TODO: Package description + Даниил Грабарь + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + ament_xmllint + python3-pytest + + + ament_python + + diff --git a/src/iiwa_object_spawner/resource/iiwa_object_spawner b/src/iiwa_object_spawner/resource/iiwa_object_spawner new file mode 100644 index 0000000..e69de29 diff --git a/src/iiwa_object_spawner/setup.cfg b/src/iiwa_object_spawner/setup.cfg new file mode 100644 index 0000000..1dd0f86 --- /dev/null +++ b/src/iiwa_object_spawner/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/iiwa_object_spawner +[install] +install_scripts=$base/lib/iiwa_object_spawner diff --git a/src/iiwa_object_spawner/setup.py b/src/iiwa_object_spawner/setup.py new file mode 100644 index 0000000..ec91923 --- /dev/null +++ b/src/iiwa_object_spawner/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'iiwa_object_spawner' + +setup( + name=package_name, + version='0.0.1', + 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='Даниил Грабарь', + maintainer_email='grabardm@ml-dev.ru', + description='TODO: Package description', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'object_spawner = iiwa_object_spawner.object_spawner:main' + ], + }, +)