Initial commit

CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(rbs_mill_assist)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+
+install(DIRECTORY launch urdf config world
+DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

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.

config/config.rviz

@@ -0,0 +1,198 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 739
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ee_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 3.529470682144165
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5553982257843018
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.5453980565071106
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000371fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000371000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000371fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f00000371000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 196

config/gz_bridge.yaml

@@ -0,0 +1,9 @@
+- topic_name: "/clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS
+
+- topic_name: "rgbd_camera/image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS

launch/simulation.launch.py

@@ -0,0 +1,335 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from rbs_utils.launch import load_xacro_args
+from robot_builder.external.ros2_control import ControllerManager
+from robot_builder.parser.urdf import URDF_parser
+from ros_gz_bridge.actions import RosGzBridge
+
+
+def launch_setup(context, *args, **kwargs):
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    use_moveit = LaunchConfiguration("use_moveit")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
+    control_space = LaunchConfiguration("control_space").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+    interactive = LaunchConfiguration("interactive").perform(context)
+    real_robot = LaunchConfiguration("real_robot").perform(context)
+
+    use_rbs_utils = LaunchConfiguration("use_rbs_utils")
+    assembly_config_name = LaunchConfiguration("assembly_config_name")
+
+    description_package_abs_path = get_package_share_directory(
+        description_package.perform(context)
+    )
+
+    controllers_file = os.path.join(
+        description_package_abs_path, "config", "controllers.yaml"
+    )
+
+    xacro_file = os.path.join(
+        description_package_abs_path,
+        "urdf",
+        description_file.perform(context),
+    )
+
+    # xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
+    xacro_config_file = os.path.join(
+        description_package_abs_path, "urdf", "xacro_args.yaml"
+    )
+
+    mappings_data = load_xacro_args(xacro_config_file, locals())
+
+    robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
+
+    robot_description_semantic_content = ""
+
+    if use_moveit.perform(context) == "true":
+        srdf_config_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            "xacro_args.yaml",
+        )
+        srdf_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            moveit_config_file.perform(context),
+        )
+        srdf_mappings = load_xacro_args(srdf_config_file, locals())
+        robot_description_semantic_content = xacro.process_file(
+            srdf_file, mappings=srdf_mappings
+        )
+        robot_description_semantic_content = (
+            robot_description_semantic_content.toprettyxml(indent=" ")
+        )
+        control_space = "joint"
+        control_strategy = "position"
+        interactive = "false"
+
+    robot_description_content = robot_description_doc.toprettyxml(indent="  ")
+    robot_description = {"robot_description": robot_description_content}
+
+    # Parse robot and configure controller's file for ControllerManager
+    robot = URDF_parser.load_string(
+        robot_description_content, ee_link_name=ee_link_name
+    )
+    ControllerManager.save_to_yaml(
+        robot, description_package_abs_path, "controllers.yaml"
+    )
+
+    rbs_robot_setup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
+                )
+            ]
+        ),
+        launch_arguments={
+            "with_gripper": with_gripper_condition,
+            "controllers_file": controllers_file,
+            "robot_type": robot_type,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_type,
+            "use_moveit": use_moveit,
+            "moveit_config_package": moveit_config_package,
+            "moveit_config_file": moveit_config_file,
+            "use_sim_time": use_sim_time,
+            "use_controllers": "true",
+            "robot_description": robot_description_content,
+            "robot_description_semantic": robot_description_semantic_content,
+            "base_link_name": base_link_name,
+            "ee_link_name": ee_link_name,
+            "control_space": control_space,
+            "control_strategy": control_strategy,
+            "interactive_control": interactive,
+            "use_rbs_utils": use_rbs_utils,
+            "assembly_config_name": assembly_config_name,
+        }.items(),
+    )
+
+    rviz_config_file = os.path.join(description_package_abs_path, "config", "config.rviz")
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        arguments=["-d", rviz_config_file],
+        parameters=[{"use_sim_time": True}]
+    )
+
+    gazebo_world = os.path.join(description_package_abs_path, "world", "default.sdf")
+
+    gazebo = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [os.path.join(get_package_share_directory('ros_gz_sim'),
+                            'launch', 'gz_sim.launch.py')]),
+            launch_arguments=[('gz_args', [' -r ', gazebo_world])],
+    )
+
+    gazebo_spawn_robot = Node(
+        package="ros_gz_sim",
+        executable="create",
+        arguments=[
+            "-name", "arm0",
+            "-x", "0.0",
+            "-z", "0.0",
+            "-y", "0.0",
+            "-topic", "/robot_description",
+        ],
+        output="screen",
+        parameters=[{"use_sim_time": True}],
+    )
+
+    gz_bridge = RosGzBridge(
+        bridge_name="ros_gz_bridge",
+        config_file=os.path.join(description_package_abs_path, "config", "gz_bridge.yaml")
+    )
+
+    nodes_to_start = [
+        rbs_robot_setup,
+        rviz_node,
+        gazebo,
+        gazebo_spawn_robot,
+        gz_bridge
+    ]
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_type",
+            description="Type of robot by name",
+            choices=[
+                "rbs_arm",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
+            default_value="rbs_arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="rbs_mill_assist",
+            description="Description package with robot URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="rbs_arm_modular.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_name",
+            default_value="arm0",
+            description="Name for robot, used to apply namespace for specific robot in multirobot setup",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            default_value="rbs_arm",
+            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            default_value="rbs_arm.srdf.xacro",
+            description="MoveIt SRDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="Make MoveIt to use simulation time.\
+            This is needed for the trajectory planing in simulation.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "with_gripper", default_value="false", description="With gripper or not?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_moveit", default_value="false", description="Launch moveit?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_perception", default_value="false", description="Launch perception?"
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_controllers",
+            default_value="true",
+            description="Launch controllers?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "scene_config_file",
+            default_value="",
+            description="Path to a scene configuration file",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ee_link_name",
+            default_value="tool0",
+            description="End effector name of robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "base_link_name",
+            default_value="base_link",
+            description="Base link name if robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "interactive",
+            default_value="true",
+            description="Wheter to run the motion_control_handle controller",
+        ),
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "real_robot",
+            default_value="false",
+            description="Wheter to run on the real robot",
+        ),
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_rbs_utils",
+            default_value="true",
+            description="Wheter to use rbs_utils",
+        ),
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "assembly_config_name",
+            default_value="",
+            description="Assembly config name from rbs_assets_library",
+        ),
+    )
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rbs_mill_assist</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ur.narmak@gmail.com">narenmak</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

urdf/current.urdf

@@ -0,0 +1,355 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from rbs_arm_modular.xacro          | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="rbs_arm">
+  <link name="world"/>
+  <joint name="base_link_joint" type="fixed">
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <parent link="world"/>
+    <child link="base_link"/>
+  </joint>
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00078073 -0.005657 0.036649"/>
+      <mass value="0.61398"/>
+      <inertia ixx="0.0015423" ixy="2.1048E-05" ixz="-2.1541E-06" iyy="0.0014689" iyz="7.0187E-07" izz="0.0015067"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/visual/base_link.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/collision/base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- <xacro:joint_hardware joint_name="${tf_prefix}base_link" hardware="${hardware}" p="100"/> -->
+  <!-- <xacro:rbs_materials link_name="${tf_prefix}base_link" link_type="base_link"></xacro:rbs_materials> -->
+  <joint name="fork0_link_joint" type="revolute">
+    <limit effort="78" lower="-6.14159" upper="6.14159" velocity="0.52"/>
+    <origin rpy="0.0 0 -3.1416" xyz="0 0 0.11165"/>
+    <parent link="base_link"/>
+    <child link="fork0_link"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="fork0_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0050017 0.088736 0.0023758"/>
+      <mass value="0.49312"/>
+      <inertia ixx="0.0016921" ixy="-1.8404E-05" ixz="9.951E-07" iyy="0.00087841" iyz="-2.5874E-06" izz="0.0019955"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/visual/axis1_fork.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/collision/axis1_fork.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <ros2_control name="fork0_link_joint_hardware_interface" type="actuator">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    <joint name="fork0_link_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <!-- WARN When this active a robot falls down -->
+      <!-- <command_interface name="effort"/> -->
+      <param name="p">10000</param>
+      <param name="d">0.2</param>
+      <!-- <command_interface name="effort"/> -->
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="acceleration"/> -->
+    </joint>
+  </ros2_control>
+  <!-- <xacro:rbs_materials link_name="${tf_prefix}${name}" link_type="fork" /> -->
+  <joint name="main0_link_joint" type="revolute">
+    <limit effort="78" lower="-2.43" upper="2.43" velocity="0.52"/>
+    <!-- <origin rpy="0.0 0 -3.1416" xyz="0 0 0.11165"></origin> -->
+    <origin rpy="0.0 0 0" xyz="0 0.0 0.15465"/>
+    <parent link="fork0_link"/>
+    <child link="main0_link"/>
+    <axis xyz="1 0 0"/>
+  </joint>
+  <link name="main0_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00027998 -0.045366 0.0068959"/>
+      <mass value="0.42726"/>
+      <inertia ixx="0.0013794" ixy="-4.0542E-05" ixz="-8.9181E-06" iyy="0.00047255" iyz="-3.4771E-05" izz="0.001587"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/visual/axis2_main.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/collision/axis2_main.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <ros2_control name="main0_link_joint_hardware_interface" type="actuator">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    <joint name="main0_link_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <!-- WARN When this active a robot falls down -->
+      <!-- <command_interface name="effort"/> -->
+      <param name="p">10000</param>
+      <param name="d">0.2</param>
+      <!-- <command_interface name="effort"/> -->
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="acceleration"/> -->
+    </joint>
+  </ros2_control>
+  <!-- <xacro:rbs_materials link_name="${tf_prefix}${name}" link_type="main_link" /> -->
+  <joint name="fork1_link_joint" type="revolute">
+    <limit effort="78" lower="-6.14159" upper="6.14159" velocity="0.52"/>
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.11995"/>
+    <parent link="main0_link"/>
+    <child link="fork1_link"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="fork1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0050017 0.088736 0.0023758"/>
+      <mass value="0.49312"/>
+      <inertia ixx="0.0016921" ixy="-1.8404E-05" ixz="9.951E-07" iyy="0.00087841" iyz="-2.5874E-06" izz="0.0019955"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/visual/axis1_fork.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/collision/axis1_fork.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <ros2_control name="fork1_link_joint_hardware_interface" type="actuator">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    <joint name="fork1_link_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <!-- WARN When this active a robot falls down -->
+      <!-- <command_interface name="effort"/> -->
+      <param name="p">10000</param>
+      <param name="d">0.2</param>
+      <!-- <command_interface name="effort"/> -->
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="acceleration"/> -->
+    </joint>
+  </ros2_control>
+  <!-- <xacro:rbs_materials link_name="${tf_prefix}${name}" link_type="fork" /> -->
+  <joint name="main1_link_joint" type="revolute">
+    <limit effort="78" lower="-2.43" upper="2.43" velocity="0.52"/>
+    <!-- <origin rpy="0.0 0 -3.1416" xyz="0 0 0.11165"></origin> -->
+    <origin rpy="0.0 0 0" xyz="0 0.0 0.15465"/>
+    <parent link="fork1_link"/>
+    <child link="main1_link"/>
+    <axis xyz="1 0 0"/>
+  </joint>
+  <link name="main1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.00027998 -0.045366 0.0068959"/>
+      <mass value="0.42726"/>
+      <inertia ixx="0.0013794" ixy="-4.0542E-05" ixz="-8.9181E-06" iyy="0.00047255" iyz="-3.4771E-05" izz="0.001587"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/visual/axis2_main.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/collision/axis2_main.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <ros2_control name="main1_link_joint_hardware_interface" type="actuator">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    <joint name="main1_link_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <!-- WARN When this active a robot falls down -->
+      <!-- <command_interface name="effort"/> -->
+      <param name="p">1000</param>
+      <param name="d">0.2</param>
+      <!-- <command_interface name="effort"/> -->
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="acceleration"/> -->
+    </joint>
+  </ros2_control>
+  <!-- <xacro:rbs_materials link_name="${tf_prefix}${name}" link_type="main_link" /> -->
+  <joint name="ee_link_joint" type="revolute">
+    <limit effort="78" lower="-2.43" upper="2.43" velocity="0.52"/>
+    <origin rpy="0 0 0.0" xyz="0.0 0.0 0.0754"/>
+    <parent link="main1_link"/>
+    <child link="ee_link"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <joint name="tool0_joint" type="fixed">
+    <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.14100"/>
+    <parent link="ee_link"/>
+    <child link="tool0"/>
+  </joint>
+  <link name="tool0"/>
+  <link name="ee_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="1.594E-06 0.085889 0.0021969"/>
+      <mass value="0.20976"/>
+      <inertia ixx="0.00016257" ixy="4.2628E-07" ixz="-3.8857E-07" iyy="0.00022468" iyz="2.1709E-05" izz="0.00023702"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/visual/axis7_tail.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:///home/narenmak/rbs_ws/install/rbs_arm/share/rbs_arm/meshes/collision/axis7_tail.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <ros2_control name="ee_link_joint_hardware_interface" type="actuator">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    <joint name="ee_link_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <!-- WARN When this active a robot falls down -->
+      <!-- <command_interface name="effort"/> -->
+      <param name="p">20</param>
+      <param name="d">0.2</param>
+      <!-- <command_interface name="effort"/> -->
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="acceleration"/> -->
+    </joint>
+  </ros2_control>
+  <!-- <xacro:rbs_materials link_name="${tf_prefix}ee_link" link_type="ee_link" /> -->
+  <link name="rgbd_camera">
+    <inertial>
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+      <mass value="0.1"/>
+      <inertia ixx="0.000166667" ixy="0.0" ixz="0.0" iyy="0.000166667" iyz="0.0" izz="0.000166667"/>
+    </inertial>
+    <visual name="">
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.0 0.0 0.0 1.0"/>
+        <texture filename=""/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="rgbd_camera">
+    <sensor name="rgbd_camera" type="rgbd_camera">
+      <camera>
+        <horizontal_fov>1.047</horizontal_fov>
+        <image>
+          <width>320</width>
+          <height>240</height>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>100</far>
+        </clip>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>true</visualize>
+      <topic>rgbd_camera</topic>
+      <enable_metrics>true</enable_metrics>
+    </sensor>
+  </gazebo>
+  <joint name="rgbd_camera_to_parent" type="fixed">
+    <parent link="ee_link"/>
+    <child link="rgbd_camera"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+  </joint>
+  <!-- Insert gripper in robot description if exist -->
+  <!-- <xacro:if value="${gripper_name=='rbs_gripper'}"> -->
+  <!--   <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro"></xacro:include> -->
+  <!--   <xacro:rbs_gripper gripper_name="rbs_gripper" hardware="${hardware}"
+    parent="${tf_prefix}tool0" tf_prefix="${tf_prefix}"> -->
+  <!--     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"></origin> -->
+  <!--   </xacro:rbs_gripper> -->
+  <!-- </xacro:if> -->
+  <!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
+  <gazebo>
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <parameters></parameters>
+      <ros>
+        <namespace>arm0</namespace>
+      </ros>
+    </plugin>
+  </gazebo>
+</robot>

urdf/rbs_arm_modular.xacro

@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
+  <xacro:arg name="gripper_name" default="" />
+  <xacro:arg name="hardware" default="gazebo" />
+  <xacro:arg name="simulation_controllers" default="" />
+  <xacro:arg name="tf_prefix" default="" />
+  <xacro:arg name="namespace" default="arm0"/>
+
+  <xacro:arg name="x" default="-0.10" />
+  <xacro:arg name="y" default="0.0" />
+  <xacro:arg name="z" default="0.03" />
+
+  <xacro:arg name="roll" default="0.0" />
+  <xacro:arg name="pitch" default="0.0" />
+  <xacro:arg name="yaw" default="0.0" />
+
+  <xacro:include filename="$(find rbs_mill_assist)/urdf/rbs_arm_modular_macro.xacro" />
+  <link name="world" />
+
+  <!-- World objects-->
+  <xacro:property name="table_length" value="1.2"/>
+  <xacro:property name="table_width" value="0.7"/>
+  <xacro:property name="table_height" value="0.8"/>
+
+  <link name="table_link">
+    <visual>
+      <geometry>
+        <box size="${table_length} ${table_width} ${table_height}"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="${table_length} ${table_width} ${table_height}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10.0"/>
+      <inertia
+        ixx="${(1/12)*50*(table_width**2 + table_height**2)}"
+        iyy="${(1/12)*50*(table_length**2 + table_height**2)}"
+        izz="${(1/12)*50*(table_length**2 + table_width**2)}"
+        ixy="0"
+        ixz="0"
+        iyz="0"/>
+    </inertial>
+  </link>
+
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="table_link"/>
+    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
+  </joint>
+
+  <xacro:property name="machine_length" value="0.4"/>
+  <xacro:property name="machine_width" value="0.5"/>
+  <xacro:property name="machine_height" value="0.115"/>
+
+  <link name="machine_link">
+    <visual>
+      <geometry>
+        <box size="${machine_length} ${machine_width} ${machine_height}"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="${machine_length} ${machine_width} ${machine_height}"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="40.0"/>
+      <inertia
+        ixx="${(1/12)*30*(machine_width**2 + machine_height**2)}"
+        iyy="${(1/12)*30*(machine_length**2 + machine_height**2)}"
+        izz="${(1/12)*30*(machine_length**2 + machine_width**2)}"
+        ixy="0"
+        ixz="0"
+        iyz="0"/>
+    </inertial>
+  </link>
+
+  <joint name="machine_joint" type="fixed">
+    <parent link="world"/>
+    <child link="machine_link"/>
+    <origin xyz="0.25 0 ${0.115/2}" rpy="0 0 0"/>
+  </joint>
+
+
+  <!-- Arm-->
+  <xacro:rbs_arm namespace="$(arg namespace)" parent="world" tf_prefix="$(arg tf_prefix)" hardware="$(arg hardware)"
+    gripper_name="$(arg gripper_name)" controllers="$(arg simulation_controllers)">
+    <origin xyz="$(arg x) $(arg y) $(arg z)" rpy="$(arg roll) $(arg pitch) $(arg yaw)" />
+  </xacro:rbs_arm>
+</robot>

urdf/rbs_arm_modular_macro.xacro

@@ -0,0 +1,53 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="rbs_arm" params="namespace parent *origin tf_prefix hardware gripper_name controllers">
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_base_link.xacro"></xacro:include>
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_ee_link.xacro"></xacro:include>
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_fork_link.xacro"></xacro:include>
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_main_link.xacro"></xacro:include>
+    <xacro:include filename="$(find rbs_arm)/urdf/sensors.xacro"></xacro:include>
+    <!-- BEGIN robot description  -->
+    <!-- link 0-->
+    <xacro:base_link hardware="${hardware}" parent="${parent}" tf_prefix="${tf_prefix}">
+      <xacro:insert_block name="origin"></xacro:insert_block>
+    </xacro:base_link>
+
+    <!-- link 1-->
+    <xacro:fork_link d="0.2" hardware="${hardware}" name="fork0_link" p="10000" parent="base_link" tf_prefix="${tf_prefix}" initial_joint_position="1.57"></xacro:fork_link>
+    <!-- link 2-->
+    <xacro:main_link d="0.2" hardware="${hardware}" name="main0_link" p="10000" parent="fork0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.5"></xacro:main_link>
+    <!-- link 3-->
+    <xacro:fork_link d="0.2" hardware="${hardware}" name="fork1_link" p="10000" parent="main0_link" tf_prefix="${tf_prefix}" initial_joint_position="0.0"></xacro:fork_link>
+    <!-- link 4-->
+    <xacro:main_link d="0.2" hardware="${hardware}" name="main1_link" p="1000" parent="fork1_link" tf_prefix="${tf_prefix}" initial_joint_position="1.5"></xacro:main_link>
+    <!-- ee link also contain tool0-->
+    <!-- link 5-->
+    <xacro:ee_link d="0.2" hardware="${hardware}" p="20" parent="main1_link" tf_prefix="${tf_prefix}"></xacro:ee_link>
+    <!-- END robot description -->
+    <xacro:rgbd parent="${tf_prefix}ee_link" tf_prefix="${tf_prefix}">
+      <origin rpy="0.0 -1.57 0.0" xyz="0.0 0.0 0.0754"></origin>
+    </xacro:rgbd>
+
+    <!-- Insert gripper in robot description if exist -->
+    <!-- <xacro:if value="${gripper_name=='rbs_gripper'}"> -->
+    <!--   <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro"></xacro:include> -->
+    <!--   <xacro:rbs_gripper gripper_name="rbs_gripper" hardware="${hardware}"
+    parent="${tf_prefix}tool0" tf_prefix="${tf_prefix}"> -->
+    <!--     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"></origin> -->
+    <!--   </xacro:rbs_gripper> -->
+    <!-- </xacro:if> -->
+
+    <!-- If hardware == gazebo so insert additional parameters for it -->
+    <xacro:if value="${hardware=='gazebo'}">
+      <!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
+      <gazebo>
+        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+          <parameters>$(arg simulation_controllers)</parameters>
+          <ros>
+            <namespace>${namespace}</namespace>
+          </ros>
+        </plugin>
+      </gazebo>
+    </xacro:if>
+  </xacro:macro>
+</robot>

urdf/sensors.xacro

@@ -0,0 +1,88 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rack">
+    <xacro:macro name="rgbd" params="*origin parent tf_prefix">
+        <link name="${tf_prefix}rgbd_camera">
+            <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="0.1"/>
+                <inertia ixx="0.000166667" ixy="0.0" ixz="0.0" iyy="0.000166667" iyz="0.0" izz="0.000166667"/>
+            </inertial>
+            <visual name="">
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="0.01 0.01 0.01"/>
+                </geometry>
+                <material name="">
+                    <color rgba="0.0 0.0 0.0 1.0"/>
+                    <texture filename=""/>
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="0.01 0.01 0.01"/>
+                </geometry>
+            </collision>
+        </link>
+
+        <gazebo reference = "${tf_prefix}rgbd_camera">
+            <sensor name="${tf_prefix}rgbd_camera" type="rgbd_camera">
+                <camera>
+                    <horizontal_fov>1.047</horizontal_fov>
+                    <image>
+                        <width>320</width>
+                        <height>240</height>
+                    </image>
+                    <clip>
+                        <near>0.1</near>
+                        <far>100</far>
+                    </clip>
+                </camera>
+                <always_on>1</always_on>
+                <update_rate>30</update_rate>
+                <visualize>true</visualize>
+                <topic>${tf_prefix}rgbd_camera</topic>
+                <enable_metrics>true</enable_metrics>
+            </sensor>
+        </gazebo>
+
+        <joint name="${tf_prefix}rgbd_camera_to_parent" type="fixed">
+            <parent link="${parent}" />
+            <child link="${tf_prefix}rgbd_camera" />
+            <xacro:insert_block name="origin" />
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="fts" params="name tf_prefix link">
+
+    <ros2_control name="${tf_prefix}${name}" type="sensor">
+      <hardware>
+          <plugin>gz_ros2_control/GzFts</plugin>
+      </hardware>
+
+      <sensor name="${tf_prefix}${name}">
+        <state_interface name="force.x"/>
+        <state_interface name="force.y"/>
+        <state_interface name="force.z"/>
+        <state_interface name="torque.x"/>
+        <state_interface name="torque.y"/>
+        <state_interface name="torque.z"/>
+      </sensor>
+    </ros2_control>
+
+    <gazebo reference="${tf_prefix}${link}_joint">
+        <preserveFixedJoint>true</preserveFixedJoint>
+        <sensor name="${tf_prefix}${name}" type="force_torque">
+          <always_on>true</always_on>
+          <update_rate>50</update_rate>
+          <visualize>true</visualize>
+          <topic>${tf_prefix}ft_data</topic>
+          <force_torque>
+            <frame>sensor</frame>
+            <measure_direction>child_to_parent</measure_direction>
+          </force_torque>
+        </sensor>
+      </gazebo>
+  </xacro:macro>
+
+</robot>

urdf/xacro_args.yaml

@@ -0,0 +1,4 @@
+gripper_name: "rbs_gripper"
+hardware: "gazebo"
+simulation_controllers: !variable controllers_file
+namespace: ""

world/default.sdf

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<sdf version="1.9">
+  <world name="rbs_freazer_world">
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <grid>false</grid>
+    </scene>
+
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <!-- <model name="ground_plane"> -->
+    <!--   <static>true</static> -->
+    <!--   <link name="link"> -->
+    <!--     <collision name="collision"> -->
+    <!--       <geometry> -->
+    <!--         <plane> -->
+    <!--           <normal>0 0 1</normal> -->
+    <!--           <size>1 1</size> -->
+    <!--         </plane> -->
+    <!--       </geometry> -->
+    <!--     </collision> -->
+    <!--     <visual name="visual"> -->
+    <!--       <geometry> -->
+    <!--         <plane> -->
+    <!--           <normal>0 0 1</normal> -->
+    <!--           <size>1 1</size> -->
+    <!--         </plane> -->
+    <!--       </geometry> -->
+    <!--       <material> -->
+    <!--         <ambient>0.8 0.8 0.8 1</ambient> -->
+    <!--         <diffuse>0.8 0.8 0.8 1</diffuse> -->
+    <!--         <specular>0.8 0.8 0.8 1</specular> -->
+    <!--       </material> -->
+    <!--     </visual> -->
+    <!--   </link> -->
+    <!-- </model> -->
+
+
+  </world>
+</sdf>