Migrate to Gazebo Fortress

.gitlab-ci.yml

@@ -1,4 +1,4 @@
-image: ros:foxy-ros-base
+image: ros:humble-ros-base
 
 workflow:
   rules:
@@ -17,8 +17,8 @@ build-colcon-job:
   - mv * .src/robossembler-ros2
   - mv .git .src/robossembler-ros2
   - mv .src src
-  - vcs import src < src/robossembler-ros2/rasms.repos
-  - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
-  - colcon build --merge-install --symlink-install --cmake-args '-DCMAKE_BUILD_TYPE=Release' -Wall -Wextra -Wpedantic
+  - vcs import src < src/robossembler-ros2/rbs.repos
+  - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
+  - colcon build --merge-install --cmake-args '-DCMAKE_BUILD_TYPE=Release' -Wall -Wextra -Wpedantic
   rules:
     - if: $CI_COMMIT_REF_NAME != $CI_DEFAULT_BRANCH

README.md

@@ -4,77 +4,55 @@ Repo for ROS2 packages related to Robossembler
 
 ## Instructions
 ### Requirements
-* OS: Ubuntu 20.04
-* Other distributions might work (not tested).
+* OS: Ubuntu 22.04
+* ROS 2 Humble
 
 ### Dependencies
 These are the primary dependencies required to use this project.
 
-* ROS 2 Foxy
 * MoveIt 2 
     > Install/build a version based on the selected ROS 2 release
-* Gazebo
+* Gazebo Fortress
 
 ### Build
 
 1. Clone the repository
-2. Build packages
+2. Build packages `colcon build`
 
-For visualization install ```colcon``` with ```mixin``` (it's required to install ```moveit_visual_tools```):
-```bash
-sudo apt install python3-colcon-common-extensions python3-colcon-mixin python3-vcstool
-colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
-colcon mixin update default
-```
 
-Prepare workspace & install dependencies
+Prepare workspace & install dependencies (So far only tested with UR robot arm)
 ```
 mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
 git clone https://gitlab.com/robosphere/robossembler-ros2
-vcs import . < robossembler-ros2/rasms.repos
+vcs import . < robossembler-ros2/rbs.repos
 cd ..
-rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
-colcon build --symlink-install --mixin release
+rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
+colcon build
 ```
 
-but at the moment the visualization is not implemented
-
 ### Examples
 Add source to environment
 ```
-source install/setup.bash
-```
-Add robot model to Gazebo models directory
-
-```
-cp ~/robossembler_ws/src/robossembler-ros2/rasmt_support/ ~/.gazebo/models/
+. install/setup.bash
 ```
 
-Launch MoveIt2, Gazebo, RViz and PlanSys2 with domain from `pddl/domain.pddl`
+Launch MoveIt2, Gazebo, RViz
 ```bash
-ros2 launch robossembler robossembler_bringup.launch.py
+ros2 launch rbs_simulation rbs_simulation.launch.py
 ```
-#### With PlanSys2 Terminal
+#### Launch bt_tree
+
+It will execute `bt_tree` once from file `rbs_bt_executor/bt_trees/test_tree.xml`
 
-Launch Plansys2 Terminal
-```bash
-ros2 run plansys2_terminal plansys2_terminal
 ```
-Then into plansys2_terminal paste command (see updates into pddl/commands)
-```bash
-set instance rasmt_arm_group robot
-set instance one zone
-set goal (and (robot_move rasmt_arm_group one))
-run
+ros2 launch rbs_executor rbs_executor.launch.py
 ```
 
-#### With C++ node 
-
-```bash
-ros2 run robossembler move_controller_node
+The robot arm should move to the point from config file in path 
+```
+rbs_bt_executor/config/points.yaml
 ```
 
 ### Links
-* [plansys2_bt](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/tutorials/docs/bt_actions.html)
 * [bt_v3_cpp](https://www.behaviortree.dev)
-* [moveit2](https://moveit.picknik.ai/foxy/index.html)
+* [moveit2](https://moveit.picknik.ai/humble/index.html)

rasms.repos

@@ -1,21 +0,0 @@
-repositories:
-  rviz_visual_tools:
-    type: git
-    url: https://github.com/PickNikRobotics/rviz_visual_tools
-    version: foxy-devel
-  moveit_visual_tools:
-    type: git
-    url: https://github.com/ros-planning/moveit_visual_tools
-    version: foxy
-  plansys2:
-    type: git
-    url: https://github.com/IntelligentRoboticsLabs/ros2_planning_system
-    version: master
-  behavior_tree:
-    type: git
-    url: https://github.com/berkeleyauv/behavior_tree
-    version: master
-  gazebo_ros_link_attacher:
-    type: git
-    url: https://github.com/davidorchansky/gazebo_ros_link_attacher.git
-    version: foxy-devel

rasmt_moveit_config/config/joint_limits.yaml

@@ -39,6 +39,11 @@ joint_limits:
     has_acceleration_limits: false
     max_acceleration: 0
   rasmt_Slide_1:
+    has_velocity_limits: true
+    max_velocity: 0.2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  rasmt_Slide_2:
     has_velocity_limits: true
     max_velocity: 0.2
     has_acceleration_limits: false

rasmt_moveit_config/config/rasmt_moveit_controllers.yaml

@@ -1,6 +1,6 @@
 controller_names:
   - rasmt_arm_controller
-#  - rasmt_hand_controller
+  - rasmt_hand_controller
 
 rasmt_arm_controller:
   action_ns: follow_joint_trajectory
@@ -14,10 +14,10 @@ rasmt_arm_controller:
       - rasmt_Rot_Z_3
       - rasmt_Rot_Y_4
 
-#rasmt_hand_controller:
-#  action_ns: follow_joint_trajectory
-#  type: FollowJointTrajectory
-#  default: true
-#  joints:
-#    - rasmt_Slide_1
-#    - rasmt_Slide_2
+rasmt_hand_controller:
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+   - rasmt_Slide_1
+   - rasmt_Slide_2

rasmt_moveit_config/launch/rasmt_moveit.launch.py

@@ -97,8 +97,12 @@ def generate_launch_description():
     }
 
     # Trajectory execution
-    trajectory_execution = {"allow_trajectory_execution": True,
-                            "moveit_manage_controllers": True}
+    trajectory_execution = {
+        "moveit_manage_controllers": True,
+        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_start_tolerance": 0.01,
+    }
 
     # Controllers
     controllers_yaml = load_yaml(
@@ -156,47 +160,47 @@ def generate_launch_description():
         ]
     )
 
-    move_topose_action_server = Node(
-        package="robossembler_servers",
-        executable="move_topose_action_server",
-        name="move_to_pose_moveit",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            kinematics_yaml,
-            robot_description_planning,
-            ompl_yaml,
-            planning,
-            trajectory_execution,
-            moveit_controllers,
-            planning_scene_monitor_parameters,
-            use_sim_time
-        ]
-    )
+    # move_topose_action_server = Node(
+    #     package="robossembler_servers",
+    #     executable="move_topose_action_server",
+    #     name="move_to_pose_moveit",
+    #     parameters=[
+    #         robot_description,
+    #         robot_description_semantic,
+    #         kinematics_yaml,
+    #         robot_description_planning,
+    #         ompl_yaml,
+    #         planning,
+    #         trajectory_execution,
+    #         moveit_controllers,
+    #         planning_scene_monitor_parameters,
+    #         use_sim_time
+    #     ]
+    # )
 
-    move_cartesian_path_action_server = Node(
-        package="robossembler_servers",
-        executable="move_cartesian_path_action_server",
-        name="move_cartesian_path_action_server",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            kinematics_yaml,
-            robot_description_planning,
-            ompl_yaml,
-            planning,
-            trajectory_execution,
-            moveit_controllers,
-            planning_scene_monitor_parameters,
-            use_sim_time
-        ]
-    )
+    # move_cartesian_path_action_server = Node(
+    #     package="robossembler_servers",
+    #     executable="move_cartesian_path_action_server",
+    #     name="move_cartesian_path_action_server",
+    #     parameters=[
+    #         robot_description,
+    #         robot_description_semantic,
+    #         kinematics_yaml,
+    #         robot_description_planning,
+    #         ompl_yaml,
+    #         planning,
+    #         trajectory_execution,
+    #         moveit_controllers,
+    #         planning_scene_monitor_parameters,
+    #         use_sim_time
+    #     ]
+    # )
 
     launch_nodes = []
     launch_nodes.append(rviz)
     launch_nodes.append(move_group_node)
-    launch_nodes.append(move_topose_action_server)
-    launch_nodes.append(move_cartesian_path_action_server)
+    # launch_nodes.append(move_topose_action_server)
+    # launch_nodes.append(move_cartesian_path_action_server)
    # launch_nodes.append(move_to_joint_state_action_server)
 
 

rasmt_support/README.md

@@ -6,7 +6,7 @@ This package consists of files describing the AQUA model of the robot, the entir
 .
 ├── CMakeLists.txt
 ├── config
-│   ├── rasmt_ros2_controllers.yaml             # File describing the robot controllers
+│   ├── rasmt_effort_controllers.yaml             # File describing the robot controllers
 │   └── rasmt.rviz
 ├── launch
 │   ├── rasmt_control.launch.py                 # Launch file for running robot controlles

rasmt_support/config/rasmt_effort_controllers.yaml

@@ -0,0 +1,80 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 250
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    rasmt_arm_controller:
+      type: effort_controllers/JointGroupEffortController
+
+    rasmt_hand_controller:
+      type: effort_controllers/JointGroupEffortController
+
+rasmt_arm_controller:
+  ros__parameters:
+    joints:
+      - rasmt_Rot_Z_1
+      - rasmt_Rot_Y_1
+      - rasmt_Rot_Z_2
+      - rasmt_Rot_Y_2
+      - rasmt_Rot_Z_3
+      - rasmt_Rot_Y_4
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    gains:
+      rasmt_Rot_Z_1:
+        p: 4000.0
+        d: 10.0
+        i: 250.0
+        i_clamp: 15.0
+      rasmt_Rot_Y_1:
+        p: 10000.0
+        d: 25.0
+        i: 600.0
+        i_clamp: 45.0
+      rasmt_Rot_Z_2:
+        p: 8000.0
+        d: 20.0
+        i: 450.0
+        i_clamp: 30.0
+      rasmt_Rot_Y_2:
+        p: 6000.0
+        d: 15.0
+        i: 300.0
+        i_clamp: 30.0
+      rasmt_Rot_Z_3:
+        p: 3000.0
+        d: 5.0
+        i: 175.0
+        i_clamp: 7.0
+      rasmt_Rot_Y_4:
+        p: 2500.0
+        d: 3.0
+        i: 150.0
+        i_clamp: 6.0
+
+rasmt_hand_controller:
+  ros__parameters:
+    joints:
+      - rasmt_Slide_1
+      - rasmt_Slide_2
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    gains:
+      rasmt_Slide_1:
+        p: 225.0
+        d: 0.001
+        i: 0.4
+        i_clamp: 4.0
+      rasmt_Slide_2:
+        p: 225.0
+        d: 0.001
+        i: 0.4
+        i_clamp: 4.0

rasmt_support/config/rasmt_position_controllers.yaml

@@ -6,7 +6,7 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
 
     rasmt_hand_controller:
-      type: forward_command_controller/ForwardCommandController
+      type: joint_trajectory_controller/JointTrajectoryController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -14,11 +14,6 @@ controller_manager:
 
 rasmt_arm_controller:
   ros__parameters:
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
     joints:
       - rasmt_Rot_Z_1
       - rasmt_Rot_Y_1
@@ -26,10 +21,19 @@ rasmt_arm_controller:
       - rasmt_Rot_Y_2
       - rasmt_Rot_Z_3
       - rasmt_Rot_Y_4
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
 
 rasmt_hand_controller:
   ros__parameters:
-    joints: 
+    joints:
       - rasmt_Slide_1
       - rasmt_Slide_2
-    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

rasmt_support/config/rasmt_position_velocity_controllers.yaml

@@ -0,0 +1,41 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    rasmt_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    rasmt_hand_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+rasmt_arm_controller:
+  ros__parameters:
+    joints:
+      - rasmt_Rot_Z_1
+      - rasmt_Rot_Y_1
+      - rasmt_Rot_Z_2
+      - rasmt_Rot_Y_2
+      - rasmt_Rot_Z_3
+      - rasmt_Rot_Y_4
+    command_interfaces:
+      - position
+      - velocity
+    state_interfaces:
+      - position
+      - velocity
+
+rasmt_hand_controller:
+  ros__parameters:
+    joints:
+      - rasmt_Slide_1
+      - rasmt_Slide_2
+    command_interfaces:
+      - position
+      - velocity
+    state_interfaces:
+      - position
+      - velocity

rasmt_support/launch/rasmt_control.launch.py

@@ -1,6 +1,7 @@
+from numpy import load
 from launch.launch_description import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import UnlessCondition
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import UnlessCondition, IfCondition
 from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -27,22 +28,23 @@ def generate_launch_description():
 
     # Configure robot_description
     robot_description = {"robot_description": LaunchConfiguration("robot_description")}
-
     # Load controllers from YAML configuration file
     controller_configurations = PathJoinSubstitution([
         FindPackageShare("rasmt_support"),
         "config",
-        "rasmt_ros2_controllers.yaml"
+        "rasmt_position_velocity_controllers.yaml"
     ])
 
     # Prepare controller manager and other required nodes
-    controller_manager = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_description, controller_configurations],
-        output="screen",
-        condition=UnlessCondition(LaunchConfiguration("sim"))
-    )
+    # ros2_control_node = Node(
+    #     package="controller_manager",
+    #     executable="ros2_control_node",
+    #     parameters=[robot_description, controller_configurations],
+    #     output={
+    #         "stdout": "screen",
+    #         "stderr": "screen",
+    #     },
+    # )
 
     robot_state_publisher = Node(
         package="robot_state_publisher",
@@ -51,33 +53,32 @@ def generate_launch_description():
         parameters=[robot_description]
     )
 
-    joint_state_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner.py",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-    )
+    # action_server_controller_hand_node = Node(
+    #     package="robossembler_servers",
+    #     executable="gripper_cmd_node"
+    # )
+
+
+    # Load controllers
+    load_controllers = []
+    for controller in [
+        "rasmt_arm_controller",
+        "rasmt_hand_controller",
+        "joint_state_broadcaster",
+    ]:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=["ros2 control load_controller --set-state start {}".format(controller)],
+                shell=True,
+                output="screen",
+            )
+        ]
 
-    controller_arm = Node(
-        package="controller_manager",
-        executable="spawner.py",
-        arguments=["rasmt_arm_controller", "--controller-manager", "/controller_manager"],
-    )
-    controller_hand = Node(
-        package="controller_manager",
-        executable="spawner.py",
-        arguments=["rasmt_hand_controller", "--controller-manager", "/controller_manager"],
-    )
-    action_server_controller_hand_node = Node(
-        package="robossembler_servers",
-        executable="gripper_cmd_node"
-    )
 
     return LaunchDescription(
-        launch_args + [
-            controller_manager,
-            robot_state_publisher,
-            joint_state_broadcaster,
-            controller_arm,
-            controller_hand,
-            action_server_controller_hand_node
-    ])
+        [
+            robot_state_publisher
+        ] 
+        + load_controllers 
+        + launch_args
+    )

rasmt_support/launch/rasmt_gazebo.launch.py

@@ -21,22 +21,20 @@ def generate_launch_description():
     world_file = os.path.join(get_package_share_directory("rasmt_support"), "world", "robossembler.world")
     #test_world_file = "/home/bill-finger/gazebo_pkgs_ws/gazebo-pkgs/gazebo_grasp_plugin/test_world/test_world_full.world"
 
-    gzserver = IncludeLaunchDescription(
+    # gzserver = IncludeLaunchDescription(
+    #         PythonLaunchDescriptionSource(
+    #             os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gzserver.launch.py'),
+    #         ), launch_arguments={'world':world_file}.items()
+    # )
+    # gzclient = IncludeLaunchDescription(
+    #         PythonLaunchDescriptionSource(
+    #             os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gzclient.launch.py'),
+    #         )
+    # )
+
+    gazebo = IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                    PathJoinSubstitution([
-                            FindPackageShare("gazebo_ros"),
-                            "launch",
-                            "gzserver.launch.py"
-                    ])
-            ), launch_arguments={'world':world_file, 'extra_gazebo_args':'--verbose'}.items()
-    )
-    gzclient = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                    PathJoinSubstitution([
-                            FindPackageShare("gazebo_ros"),
-                            "launch",
-                            "gzclient.launch.py"
-                    ])
+                os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gazebo.launch.py'),
             )
     )
     
@@ -66,9 +64,9 @@ def generate_launch_description():
     )"""
 
     launch_nodes = []
-    #launch_nodes.append(gazebo)
-    launch_nodes.append(gzserver)
-    launch_nodes.append(gzclient)
+    launch_nodes.append(gazebo)
+    #launch_nodes.append(gzserver)
+    #launch_nodes.append(gzclient)
     launch_nodes.append(spawn_entity)
     #launch_nodes.append(cube_spawn)
 

rasmt_support/launch/rasmt_ignition.launch.py

@@ -0,0 +1,71 @@
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import xacro
+import os
+from ament_index_python import get_package_share_directory
+
+
+def generate_launch_description():
+
+    launch_args = []
+    launch_args.append(DeclareLaunchArgument(
+            name="robot_name",
+            default_value="rasmt",
+            description="Set robot name in gazebo env"
+        )
+    )
+    launch_args.append(DeclareLaunchArgument(
+            name="robot_description_content",
+            description="Robot XML file"
+        )
+    )
+
+    #robot_description_content = {"robot_description":LaunchConfiguration("robot_description_content")}
+    robot_description_content = LaunchConfiguration("robot_description_content")
+    # launch Ignition Gazebo
+    pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py')),
+        launch_arguments={'ign_args': '-r empty.sdf'}.items(),
+    )
+    
+    ros2_ign_bridge = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("rasmt_support"),
+                    "launch",
+                    "rasmt_ignition_bridge.launch.py"
+                ]
+            )
+        )
+    )
+
+   # Spawn
+    spawn = Node(package='ros_ign_gazebo', executable='create',
+                 arguments=[
+                    '-string', robot_description_content,
+                    '-name', 'rasmt',
+                    '-allow_renaming', 'true'],
+                 output='screen')
+
+
+    launch_nodes = []
+    launch_nodes.append(gazebo)
+    launch_nodes.append(spawn)
+    launch_nodes.append(ros2_ign_bridge)
+
+
+    return LaunchDescription(launch_args + launch_nodes)

rasmt_support/launch/rasmt_ignition_bridge.launch.py

@@ -0,0 +1,62 @@
+from launch.launch_description import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, FindExecutable, Command
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import xacro
+import os
+from ament_index_python import get_package_share_directory
+
+
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+    log_level = LaunchConfiguration('log_level', default='fatal')
+
+
+    launch_args = []
+    launch_args.append(
+        DeclareLaunchArgument(
+            name='use_sim_time',
+            default_value=use_sim_time,
+            description="If true, use simulated clock"
+        )
+    )
+    launch_args.append(
+        DeclareLaunchArgument(
+            name='log_level',
+            default_value=log_level,
+            description="Log level of all nodes launched by this script"
+        )
+    )
+
+    # JointTrajectory bridge for gripper (ROS2 -> IGN)
+    joint_trajectory_controller_bridge = Node(package='ros_ign_bridge',
+             executable='parameter_bridge',
+             name='parameter_bridge_gripper_trajectory',
+             output='screen',
+             arguments=['/gripper_trajectory@trajectory_msgs/msg/JointTrajectory]ignition.msgs.JointTrajectory',
+                        '--ros-args', '--log-level', log_level],
+             parameters=[{'use_sim_time': use_sim_time}])
+
+            # ros_ign_bridge (clock -> ROS 2)
+    ros2_ign_clock_bridge = Node(
+            package="ros_ign_bridge",
+            executable="parameter_bridge",
+            output="log",
+            arguments=[
+                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
+                "--ros-args",
+                "--log-level",
+                log_level,
+            ],
+            parameters=[{"use_sim_time": use_sim_time}],
+        )
+
+    launch_nodes = []
+    #launch_nodes.append(joint_trajectory_controller_bridge)
+    launch_nodes.append(ros2_ign_clock_bridge)
+
+    return LaunchDescription(launch_nodes + launch_args)

rasmt_support/package.xml

@@ -17,5 +17,8 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+    <!-- <gazebo_ros gazebo_plugin_path="lib"/>
+    <gazebo_ros gazebo_model_path="${prefix}"/> -->
+    <!-- <gazebo_ros gazebo_media_path="${prefix}"/> -->
   </export>
 </package>

rasmt_support/urdf/ras.sdf

@@ -0,0 +1,89 @@
+<sdf version='1.9'>
+  <world name='empty'>
+    <physics name='1ms' type='ignored'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
+    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
+    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
+    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>5.5645e-6 22.8758e-6 -42.3884e-6</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <scene>
+      <ambient>0.4 0.4 0.4 1.0</ambient>
+      <background>.7 .7 .7 1</background>
+      <shadows>true</shadows>
+    </scene>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</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>
+        <pose>0 0 0 0 -0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 -0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <enable_wind>0</enable_wind>
+      </link>
+      <pose>0 0 0 0 -0 0</pose>
+      <self_collide>0</self_collide>
+    </model>
+    <light name='sun' type='directional'>
+      <pose>0 0 10 0 -0 0</pose>
+      <cast_shadows>1</cast_shadows>
+      <intensity>1</intensity>
+      <direction>-0.5 0.1 -0.9</direction>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <linear>0.01</linear>
+        <constant>0.9</constant>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+  </world>
+</sdf>

rasmt_support/urdf/rasmt.sdf

@@ -0,0 +1,512 @@
+<sdf version='1.9'>
+  <model name='rasmt'>
+    <joint name='to_world' type='fixed'>
+      <pose relative_to='__model__'>0 0 0 0 0 0</pose>
+      <parent>world</parent>
+      <child>rasmt_Base_Link</child>
+    </joint>
+    <link name='rasmt_Base_Link'>
+      <pose relative_to='to_world'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>-0.0030651 -3.2739e-05 0.082353 0 0 0</pose>
+        <mass>5.2929</mass>
+        <inertia>
+          <ixx>0.0076169</ixx>
+          <ixy>1.0121e-05</ixy>
+          <ixz>-0.00010622</ixz>
+          <iyy>0.0076597</iyy>
+          <iyz>6.511699999999999e-05</iyz>
+          <izz>0.01165</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Base_Link_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Base_Link.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Base_Link_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Base_Link.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</diffuse>
+          <ambient>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Rot_Z_1' type='revolute'>
+      <pose relative_to='rasmt_Base_Link'>0 0 0.2533 3.141585307179587 0 0</pose>
+      <parent>rasmt_Base_Link</parent>
+      <child>rasmt_Fork_1</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-3.1416</lower>
+          <upper>3.1416</upper>
+          <effort>5.149</effort>
+          <velocity>0.99903</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Fork_1'>
+      <pose relative_to='rasmt_Rot_Z_1'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0.043764 -0.0066984 -0.032285 0 0 0</pose>
+        <mass>0.67797</mass>
+        <inertia>
+          <ixx>0.0014091</ixx>
+          <ixy>-6.2674e-05</ixy>
+          <ixz>0.00057897</ixz>
+          <iyy>0.0017329</iyy>
+          <iyz>4.7826e-05</iyz>
+          <izz>0.0019056</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Fork_1_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Fork_1.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Fork_1_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Fork_1.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</diffuse>
+          <ambient>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Rot_Y_1' type='revolute'>
+      <pose relative_to='rasmt_Fork_1'>0.1045 0 -0.0795 -3.141585307179587 0 0</pose>
+      <parent>rasmt_Fork_1</parent>
+      <child>rasmt_Link_1</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.5707</lower>
+          <upper>2.2863</upper>
+          <effort>5.149</effort>
+          <velocity>0.99903</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Link_1'>
+      <pose relative_to='rasmt_Rot_Y_1'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>-0.00042264 0 0.075171 0 0 0</pose>
+        <mass>1.8959</mass>
+        <inertia>
+          <ixx>0.0029317</ixx>
+          <ixy>-9.417e-06</ixy>
+          <ixz>5.5248e-05</ixz>
+          <iyy>0.0031666</iyy>
+          <iyz>-9.3067e-05</iyz>
+          <izz>0.0015477</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Link_1_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Link_1.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Link_1_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Link_1.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</diffuse>
+          <ambient>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Rot_Z_2' type='revolute'>
+      <pose relative_to='rasmt_Link_1'>0 0 0.17502 0 0 0</pose>
+      <parent>rasmt_Link_1</parent>
+      <child>rasmt_Fork_2</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-3.1416</lower>
+          <upper>3.1416</upper>
+          <effort>5.149</effort>
+          <velocity>0.99903</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Fork_2'>
+      <pose relative_to='rasmt_Rot_Z_2'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0.043764 0.0066984 0.032285 0 0 0</pose>
+        <mass>0.67797</mass>
+        <inertia>
+          <ixx>0.0014091</ixx>
+          <ixy>6.2674e-05</ixy>
+          <ixz>-0.00057897</ixz>
+          <iyy>0.0017329</iyy>
+          <iyz>4.7826e-05</iyz>
+          <izz>0.0019056</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Fork_2_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Fork_2.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Fork_2_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Fork_2.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</diffuse>
+          <ambient>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Rot_Y_2' type='revolute'>
+      <pose relative_to='rasmt_Fork_2'>0.1045 0 0.0795 0 0 0</pose>
+      <parent>rasmt_Fork_2</parent>
+      <child>rasmt_Link_2</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.5707</lower>
+          <upper>2.2863</upper>
+          <effort>5.149</effort>
+          <velocity>0.99903</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Link_2'>
+      <pose relative_to='rasmt_Rot_Y_2'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>-0.00042264 0 0.075171 0 0 0</pose>
+        <mass>1.8959</mass>
+        <inertia>
+          <ixx>0.0029317</ixx>
+          <ixy>-9.4171e-06</ixy>
+          <ixz>5.5248e-05</ixz>
+          <iyy>0.0031666</iyy>
+          <iyz>-9.3067e-05</iyz>
+          <izz>0.0015477</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Link_2_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Link_2.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Link_2_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Link_2.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</diffuse>
+          <ambient>0.9411749988794327 0.9411749988794327 0.9411749988794327 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Rot_Z_3' type='revolute'>
+      <pose relative_to='rasmt_Link_2'>0 0 0.175 0 0 0</pose>
+      <parent>rasmt_Link_2</parent>
+      <child>rasmt_Fork_3</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-3.14159</lower>
+          <upper>3.14159</upper>
+          <effort>5.149</effort>
+          <velocity>0.99903</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Fork_3'>
+      <pose relative_to='rasmt_Rot_Z_3'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0.0437644555284691 0.00669835350471771 0.0322846229336455 0 0 0</pose>
+        <mass>0.677972982551887</mass>
+        <inertia>
+          <ixx>0.00140908677953665</ixx>
+          <ixy>6.267434924451639e-05</ixy>
+          <ixz>-0.000578970009504215</ixz>
+          <iyy>0.00173285340301686</iyy>
+          <iyz>4.78263030621606e-05</iyz>
+          <izz>0.00190561919128035</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Fork_3_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Fork_3.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Fork_3_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Fork_3.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411764889955521 0.9411764889955521 0.9411764889955521 1</diffuse>
+          <ambient>0.9411764889955521 0.9411764889955521 0.9411764889955521 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Rot_Y_4' type='revolute'>
+      <pose relative_to='rasmt_Fork_3'>0.1045 0 0.0795 0 0 0</pose>
+      <parent>rasmt_Fork_3</parent>
+      <child>rasmt_Dock_Link</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.5707</lower>
+          <upper>2.2863</upper>
+          <effort>5.149</effort>
+          <velocity>0.99903</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Dock_Link'>
+      <pose relative_to='rasmt_Rot_Y_4'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0.000487278098416338 0.0001516907797566372 0.08254557371325712 0 0 0</pose>
+        <mass>2.53391750781824</mass>
+        <inertia>
+          <ixx>0.006064508328644976</ixx>
+          <ixy>-2.015356731520154e-06</ixy>
+          <ixz>-7.294101423638933e-05</ixz>
+          <iyy>0.006094829365861896</iyy>
+          <iyz>-0.0001246350455024134</iyz>
+          <izz>0.002069011842474237</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Dock_Link_collision'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Dock_Link.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <collision name='rasmt_Dock_Link_fixed_joint_lump__rasmt_Grip_Body_collision_1'>
+        <pose>0 0 0.12324 -3.14159265358979 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Grip_Body.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Dock_Link_visual'>
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Dock_Link.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411764889955521 0.9411764889955521 0.9411764889955521 1</diffuse>
+          <ambient>0.9411764889955521 0.9411764889955521 0.9411764889955521 1</ambient>
+        </material>
+      </visual>
+      <visual name='rasmt_Dock_Link_fixed_joint_lump__rasmt_Grip_Body_visual_1'>
+        <pose>0 0 0.12324 -3.14159265358979 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Grip_Body.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.6274499744176865 1 0.6274499744176865 1</diffuse>
+          <ambient>0.6274499744176865 1 0.6274499744176865 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Slide_1' type='prismatic'>
+      <pose relative_to='rasmt_Dock_Link'>0 -3.006528453019044e-16 0.21629 7.346410206709656e-06 -3.231089339497412e-15 1.5708</pose>
+      <parent>rasmt_Dock_Link</parent>
+      <child>rasmt_Grip_L</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.06</upper>
+          <effort>20</effort>
+          <velocity>0.2</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Grip_L'>
+      <pose relative_to='rasmt_Slide_1'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>-0.010118 0.010281 -0.0038252 0 0 0</pose>
+        <mass>0.021223</mass>
+        <inertia>
+          <ixx>6.5436e-06</ixx>
+          <ixy>-3.114e-06</ixy>
+          <ixz>2.8479e-06</ixz>
+          <iyy>1.9726e-05</iyy>
+          <iyz>1.6432e-06</iyz>
+          <izz>1.6355e-05</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Grip_L_collision'>
+        <pose>-0.02 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Grip_L.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Grip_L_visual'>
+        <pose>-0.02 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Grip_L.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 1 1 1</diffuse>
+          <ambient>0.9411749988794327 1 1 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <joint name='rasmt_Slide_2' type='prismatic'>
+      <pose relative_to='rasmt_Dock_Link'>0 -3.006528453019044e-16 0.21629 7.346410206709656e-06 3.231089339497412e-15 -1.5708</pose>
+      <parent>rasmt_Dock_Link</parent>
+      <child>rasmt_Grip_R</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.06</upper>
+          <effort>20</effort>
+          <velocity>0.2</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <link name='rasmt_Grip_R'>
+      <pose relative_to='rasmt_Slide_2'>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>-0.010118 0.010281 -0.0038252 0 0 0</pose>
+        <mass>0.021223</mass>
+        <inertia>
+          <ixx>6.5436e-06</ixx>
+          <ixy>-3.114e-06</ixy>
+          <ixz>2.8479e-06</ixz>
+          <iyy>1.9726e-05</iyy>
+          <iyz>1.6432e-06</iyz>
+          <izz>1.6355e-05</izz>
+        </inertia>
+      </inertial>
+      <collision name='rasmt_Grip_R_collision'>
+        <pose>-0.02 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/collision/Grip_R.STL</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name='rasmt_Grip_R_visual'>
+        <pose>-0.02 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rasmt_support/meshes/visual/Grip_R.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.9411749988794327 1 1 1</diffuse>
+          <ambient>0.9411749988794327 1 1 1</ambient>
+        </material>
+      </visual>
+    </link>
+    <static>false</static>
+    <plugin name='ign_ros2_control' filename='libign_ros2_control-system.so'>
+      <parameters>/home/bill-finger/rasms_ws/install/share/rasmt_support/config/rasmt_effort_controllers.yaml</parameters>
+    </plugin>
+  </model>
+</sdf>

rasmt_support/urdf/rasmt.urdf

@@ -0,0 +1,486 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from rasmt.xacro                    | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="rasmt">
+
+  <mujoco>
+    <compiler balanceinertia="true" discardvisual="true" fusestatic="true" meshdir="../meshes_mjcf/" strippath="true"/>
+  </mujoco>
+  
+  
+  <link name="world"/>
+  <joint name="to_world" type="fixed">
+    <parent link="world"/>
+    <child link="rasmt_Base_Link"/>
+    <!--origin xyz="0 0 1.15" rpy="3.14159 0 3.14159"/-->
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="rasmt_Base_Link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0030651 -3.2739E-05 0.082353"/>
+      <mass value="5.2929"/>
+      <inertia ixx="0.0076169" ixy="1.0121E-05" ixz="-0.00010622" iyy="0.0076597" iyz="6.5117E-05" izz="0.01165"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Base_Link.dae"/>
+      </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="package://rasmt_support/meshes/collision/Base_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="rasmt_Fork_1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.043764 -0.0066984 -0.032285"/>
+      <mass value="0.67797"/>
+      <inertia ixx="0.0014091" ixy="-6.2674E-05" ixz="0.00057897" iyy="0.0017329" iyz="4.7826E-05" izz="0.0019056"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Fork_1.dae"/>
+      </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="package://rasmt_support/meshes/collision/Fork_1.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Rot_Z_1" type="revolute">
+    <origin rpy="-3.1416 0 0" xyz="0 0 0.2533"/>
+    <parent link="rasmt_Base_Link"/>
+    <child link="rasmt_Fork_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5.149" lower="-3.1416" upper="3.1416" velocity="0.99903"/>
+  </joint>
+  <link name="rasmt_Link_1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00042264 0.0 0.075171"/>
+      <mass value="1.8959"/>
+      <inertia ixx="0.0029317" ixy="-9.417E-06" ixz="5.5248E-05" iyy="0.0031666" iyz="-9.3067E-05" izz="0.0015477"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Link_1.dae"/>
+      </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="package://rasmt_support/meshes/collision/Link_1.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Rot_Y_1" type="revolute">
+    <origin rpy="3.1416 0 0" xyz="0.1045 0.0 -0.0795"/>
+    <parent link="rasmt_Fork_1"/>
+    <child link="rasmt_Link_1"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="5.149" lower="-1.5707" upper="2.2863" velocity="0.99903"/>
+  </joint>
+  <link name="rasmt_Fork_2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.043764 0.0066984 0.032285"/>
+      <mass value="0.67797"/>
+      <inertia ixx="0.0014091" ixy="6.2674E-05" ixz="-0.00057897" iyy="0.0017329" iyz="4.7826E-05" izz="0.0019056"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Fork_2.dae"/>
+      </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="package://rasmt_support/meshes/collision/Fork_2.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Rot_Z_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0.0 0.17502"/>
+    <parent link="rasmt_Link_1"/>
+    <child link="rasmt_Fork_2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5.149" lower="-3.1416" upper="3.1416" velocity="0.99903"/>
+  </joint>
+  <link name="rasmt_Link_2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00042264 0.0 0.075171"/>
+      <mass value="1.8959"/>
+      <inertia ixx="0.0029317" ixy="-9.4171E-06" ixz="5.5248E-05" iyy="0.0031666" iyz="-9.3067E-05" izz="0.0015477"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Link_2.dae"/>
+      </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="package://rasmt_support/meshes/collision/Link_2.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Rot_Y_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0.1045 0.0 0.0795"/>
+    <parent link="rasmt_Fork_2"/>
+    <child link="rasmt_Link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="5.149" lower="-1.5707" upper="2.2863" velocity="0.99903"/>
+  </joint>
+  <link name="rasmt_Fork_3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0437644555284691 0.00669835350471771 0.0322846229336455"/>
+      <mass value="0.677972982551887"/>
+      <inertia ixx="0.00140908677953665" ixy="6.26743492445164E-05" ixz="-0.000578970009504215" iyy="0.00173285340301686" iyz="4.78263030621606E-05" izz="0.00190561919128035"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Fork_3.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/collision/Fork_3.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Rot_Z_3" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0.0 0.175"/>
+    <parent link="rasmt_Link_2"/>
+    <child link="rasmt_Fork_3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="5.149" lower="-3.14159" upper="3.14159" velocity="0.99903"/>
+  </joint>
+  <link name="rasmt_Dock_Link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="1.08992929317431E-05 0.0 0.0566747528784688"/>
+      <mass value="1.81040750781824"/>
+      <inertia ixx="0.00136087225665183" ixy="-1.57915998142718E-06" ixz="-1.31398366941724E-06" iyy="0.00136397029450427" iyz="-0.00010058418524025" izz="0.00124273705160787"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Dock_Link.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/collision/Dock_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Rot_Y_4" type="revolute">
+    <origin rpy="0 0 0" xyz="0.1045 0.0 0.0795"/>
+    <parent link="rasmt_Fork_3"/>
+    <child link="rasmt_Dock_Link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="5.149" lower="-1.5707" upper="2.2863" velocity="0.99903"/>
+  </joint>
+  <!-- ros_control-plugin -->
+  <!-- <xacro:if value="${sim == 'gazebo'}">
+            <gazebo>
+                <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+                    <parameters>$(find rasmt_support)/config/rasmt_effort_controllers.yaml</parameters>
+                    <robotNamespace>/${prefix}</robotNamespace>
+                </plugin>
+            </gazebo>      
+        </xacro:if> -->
+  <gazebo>
+    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+      <parameters>/home/bill-finger/rasms_ws/install/share/rasmt_support/config/rasmt_effort_controllers.yaml</parameters>
+    </plugin>
+  </gazebo>
+  <!-- <gazebo reference="${prefix}_Base_Link">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Fork_1">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Link_1">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Fork_2">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Link_2">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Fork_3">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Dock_Link">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo> -->
+  <!-- arg for control mode -->
+  <ros2_control name="rasmt_single_hi" type="system">
+    <hardware>
+      <plugin>ign_ros2_control/IgnitionSystem</plugin>
+    </hardware>
+    <joint name="rasmt_Rot_Z_1">
+      <command_interface name="position">
+        <!-- better to use radians as min max first -->
+        <param name="min">-1</param>
+        <param name="max"> 1</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rasmt_Rot_Y_1">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max"> 1</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rasmt_Rot_Z_2">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max"> 1</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rasmt_Rot_Y_2">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max"> 1</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rasmt_Rot_Z_3">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max"> 1</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rasmt_Rot_Y_4">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max"> 1</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+  <link name="rasmt_tool0"/>
+  <joint name="rasmt_tool0_fixed" type="fixed">
+    <parent link="rasmt_Dock_Link"/>
+    <child link="rasmt_tool0"/>
+    <origin rpy="0 0 0" xyz="0 0 0.12324"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!--link name="${prefix}_tool_end_point"/>
+  <joint name="${prefix}_tool_end_point_to_tool0" type="fixed">
+      <parent link="${prefix}_Dock_Link"/>
+      <child link="${prefix}_tool_end_point"/>
+      <origin xyz="0 0 ${gripper_length+0.12324}" rpy="0 0 0"/>
+      <axis xyz="0 0 0"/>
+  </joint-->
+  <link name="rasmt_Grip_Body">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0016793 -0.00053126 -0.024041"/>
+      <mass value="0.72351"/>
+      <inertia ixx="0.00045979" ixy="-2.1983E-08" ixz="-6.5154E-06" iyy="0.00048572" iyz="8.3162E-07" izz="0.00082469"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Grip_Body.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.50196 1 0.50196 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/collision/Grip_Body.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Fix" type="fixed">
+    <origin rpy="-3.14159265358979 0 0" xyz="0 0.0 0.12324"/>
+    <parent link="rasmt_Dock_Link"/>
+    <child link="rasmt_Grip_Body"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="rasmt_Grip_L">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.010118 0.010281 -0.0038252"/>
+      <mass value="0.021223"/>
+      <inertia ixx="6.5436E-06" ixy="-3.114E-06" ixz="2.8479E-06" iyy="1.9726E-05" iyz="1.6432E-06" izz="1.6355E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.02 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Grip_L.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/collision/Grip_L.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Slide_1" type="prismatic">
+    <origin rpy="3.1416 0 -1.5708" xyz="0 0 -0.09305"/>
+    <parent link="rasmt_Grip_Body"/>
+    <child link="rasmt_Grip_L"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="20" lower="0.0" upper="0.06" velocity="0.2"/>
+  </joint>
+  <link name="rasmt_Grip_R">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.010118 0.010281 -0.0038252"/>
+      <mass value="0.021223"/>
+      <inertia ixx="6.5436E-06" ixy="-3.114E-06" ixz="2.8479E-06" iyy="1.9726E-05" iyz="1.6432E-06" izz="1.6355E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.02 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/visual/Grip_R.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.02 0 0"/>
+      <geometry>
+        <mesh filename="package://rasmt_support/meshes/collision/Grip_R.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rasmt_Slide_2" type="prismatic">
+    <origin rpy="3.1416 0 1.5708" xyz="0 0 -0.09305"/>
+    <parent link="rasmt_Grip_Body"/>
+    <child link="rasmt_Grip_R"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="20" lower="0.0" upper="0.06" velocity="0.2"/>
+    <!--mimic joint="${prefix}_Slide_1"/-->
+  </joint>
+  <!-- arg for control mode -->
+  <ros2_control name="rasmt_hand_hi" type="system">
+    <hardware>
+      <plugin>ign_ros2_control/IgnitionSystem</plugin>
+    </hardware>
+    <joint name="rasmt_Slide_1">
+      <command_interface name="position">
+        <!-- better to use radians as min max first -->
+        <param name="min">0</param>
+        <param name="max">10</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="rasmt_Slide_2">
+      <!-- <param name="mimic">${prefix}_Slide_1</param>
+                <param name="multiplier">1</param> -->
+      <command_interface name="position">
+        <param name="min">0</param>
+        <param name="max">10</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+  <!-- ros_control-plugin -->
+  <!--gazebo>
+            <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+                <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
+                <parameters>$(find rasmt_support)/config/rasmt_hand_controllers.yaml</parameters>
+                <robotNamespace>/${prefix}</robotNamespace>
+            </plugin>
+        </gazebo-->
+  <!-- <gazebo reference="${prefix}_Grip_Body">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Grip_L">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo>
+
+        <gazebo reference="${prefix}_Grip_R">
+            <mu1>0.2</mu1>
+            <mu2>0.2</mu2>
+            <turnGravityOff>true</turnGravityOff>
+        </gazebo> -->
+</robot>

rasmt_support/urdf/rasmt.xacro

@@ -1,21 +1,25 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rasmt">
 
-<xacro:arg name="grip" default="false"/>
-<xacro:arg name="sim" default="true"/>
+<xacro:arg name="grip" default="true"/>
+<xacro:arg name="sim" default="ignition"/>
 <xacro:arg name="robot_name" default="rasmt"/>
-
+<xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_macro.xacro"/>
 <xacro:if value="$(arg grip)">
     <xacro:property name="gripper_length" value="0.12224"/>
 </xacro:if>
 
 <link name="world"/>
 
-<xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_macro.xacro"/>
-<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0" gripper_length="${gripper_length}"/>
+<xacro:unless value="$(arg grip)">
+    <xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0"/>
+</xacro:unless>
+
+<!-- <xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0" gripper_length="${gripper_length}"/> -->
 
 <xacro:if value="$(arg grip)">
     <xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_macro.xacro"/>
+    <xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0" gripper_length="${gripper_length}"/>
     <xacro:rasmt_hand prefix="$(arg robot_name)" sim="$(arg sim)"/>
 </xacro:if>
 

rasmt_support/urdf/robot/rasmt_single_control.xacro

@@ -4,74 +4,60 @@
         <!-- arg for control mode -->
         <ros2_control name="rasmt_single_hi" type="system">
 
-            <xacro:if value="${sim}">
+            <!-- <xacro:if value="${sim == 'gazebo'}">
                 <hardware>
                     <plugin>gazebo_ros2_control/GazeboSystem</plugin>
                 </hardware>
+            </xacro:if> -->
+            <xacro:if value="${sim == 'ignition'}">
+                <hardware>
+                    <plugin>ign_ros2_control/IgnitionSystem</plugin>
+                </hardware>
             </xacro:if>
 
-            <xacro:unless value="${sim}">
+            <xacro:if value="${sim == 'fake'}">
                 <hardware>
                     <plugin>fake_components/GenericSystem</plugin>
                 </hardware>
-            </xacro:unless>
+            </xacro:if>
 
             <joint name="${prefix}_Rot_Z_1">
-                <command_interface name="position">
-                    <!-- better to use radians as min max first -->
-                    <param name="min">-1</param>
-                    <param name="max"> 1</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>
             </joint>
 
             <joint name="${prefix}_Rot_Y_1">
-                <command_interface name="position">
-                    <param name="min">-1</param>
-                    <param name="max"> 1</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>
             </joint>
 
             <joint name="${prefix}_Rot_Z_2">
-                <command_interface name="position">
-                    <param name="min">-1</param>
-                    <param name="max"> 1</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>
             </joint>
 
             <joint name="${prefix}_Rot_Y_2">
-                <command_interface name="position">
-                    <param name="min">-1</param>
-                    <param name="max"> 1</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>
             </joint>
 
             <joint name="${prefix}_Rot_Z_3">
-                <command_interface name="position">
-                    <param name="min">-1</param>
-                    <param name="max"> 1</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>
             </joint>
 
             <joint name="${prefix}_Rot_Y_4">
-                <command_interface name="position">
-                    <param name="min">-1</param>
-                    <param name="max"> 1</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>

rasmt_support/urdf/robot/rasmt_single_gazebo.xacro

@@ -1,63 +1,26 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="rasmt_single_gazebo" params="prefix">
+    <xacro:macro name="rasmt_single_gazebo" params="prefix sim">
 
         <!-- ros_control-plugin -->
-        <gazebo>
-            <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-                <!--robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type-->
-                <parameters>$(find rasmt_support)/config/rasmt_ros2_controllers.yaml</parameters>
-                <robotNamespace>/${prefix}</robotNamespace>
-            </plugin>
-        </gazebo>
+        <!-- <xacro:if value="${sim == 'gazebo'}">
+            <gazebo>
+                <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+                    <parameters>$(find rasmt_support)/config/rasmt_position_velocity_controllers.yaml</parameters>
+                    <robotNamespace>/${prefix}</robotNamespace>
+                </plugin>
+            </gazebo>      
+        </xacro:if> -->
 
-        <!-- link 0 -->
-        <gazebo reference="${prefix}_Base_Link">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
+        <xacro:if value="${sim == 'ignition'}">
+            <gazebo reference="world"/>
+            <gazebo>
+                <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+                    <parameters>$(find rasmt_support)/config/rasmt_effort_controllers.yaml</parameters>
+                    <!--controller_manager_node_name>controller_manager</controller_manager_node_name-->
+                </plugin>
+            </gazebo>
+        </xacro:if>
 
-        <!-- link 1 -->
-        <gazebo reference="${prefix}_Fork_1">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-
-        <!-- link 2 -->
-        <gazebo reference="${prefix}_Link_1">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-
-        <!-- link 3 -->
-        <gazebo reference="${prefix}_Fork_2">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-
-        <!-- link 4 -->
-        <gazebo reference="${prefix}_Link_2">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-
-        <!-- link 5 -->
-        <gazebo reference="${prefix}_Fork_3">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
-
-        <!-- link 6 -->
-        <gazebo reference="${prefix}_Dock_Link">
-            <mu1>0.2</mu1>
-            <mu2>0.2</mu2>
-            <turnGravityOff>true</turnGravityOff>
-        </gazebo>
     </xacro:macro>
 </robot>

rasmt_support/urdf/robot/rasmt_single_macro.xacro

@@ -398,7 +398,7 @@
       effort="5.149"
       velocity="0.99903" />
   </joint>
-  <xacro:rasmt_single_gazebo prefix="${prefix}"/>
+  <xacro:rasmt_single_gazebo prefix="${prefix}" sim="${sim}"/>
   <xacro:rasmt_single_control prefix="${prefix}" sim="${sim}"/>
 
   <link name="${prefix}_tool0"/>

rasmt_support/urdf/tools/rasmt_hand_control.xacro

@@ -4,36 +4,32 @@
         <!-- arg for control mode -->
         <ros2_control name="rasmt_hand_hi" type="system">
 
-            <xacro:if value="${sim}">
+            <xacro:if value="${sim == 'gazebo'}">
                 <hardware>
                     <plugin>gazebo_ros2_control/GazeboSystem</plugin>
                 </hardware>
             </xacro:if>
+            <xacro:if value="${sim == 'ignition'}">
+                <hardware>
+                    <plugin>ign_ros2_control/IgnitionSystem</plugin>
+                </hardware>
+            </xacro:if>
 
-            <xacro:unless value="${sim}">
+            <!-- <xacro:if value="${sim == 'fake'}">
                 <hardware>
                     <plugin>fake_components/GenericSystem</plugin>
                 </hardware>
-            </xacro:unless>
+            </xacro:if> -->
 
             <joint name="${prefix}_Slide_1">
-                <command_interface name="position">
-                    <!-- better to use radians as min max first -->
-                    <param name="min">0</param>
-                    <param name="max">10</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>
             </joint>
 
             <joint name="${prefix}_Slide_2">
-                <!-- <param name="mimic">${prefix}_Slide_1</param>
-                <param name="multiplier">1</param> -->
-                <command_interface name="position">
-                    <param name="min">0</param>
-                    <param name="max">10</param>
-                </command_interface>
+                <command_interface name="effort"/>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
                 <state_interface name="effort"/>

rasmt_support/urdf/tools/rasmt_hand_gazebo.xacro

@@ -11,26 +11,23 @@
             </plugin>
         </gazebo-->
 
-        <!-- link 0 -->
-        <gazebo reference="${prefix}_Grip_Body">
+        <!-- <gazebo reference="${prefix}_Grip_Body">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
             <turnGravityOff>true</turnGravityOff>
         </gazebo>
 
-        <!-- link 1 -->
         <gazebo reference="${prefix}_Grip_L">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
             <turnGravityOff>true</turnGravityOff>
         </gazebo>
 
-        <!-- link 2 -->
         <gazebo reference="${prefix}_Grip_R">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
             <turnGravityOff>true</turnGravityOff>
-        </gazebo>
+        </gazebo> -->
 
     </xacro:macro>
 </robot>

rbs.repos

@@ -0,0 +1,17 @@
+repositories:
+  ur_description:
+    type: git
+    url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git
+    version: ros2
+  ur_moveit_config:
+    type: git
+    url: https://github.com/solid-sinusoid/ur_moveit_config.git
+    version: main
+  gz_ros2_control:
+    type: git
+    url: https://github.com/solid-sinusoid/gz_ros2_control.git
+    version: humble-fix
+  behavior_tree:
+    type: git
+    url: https://github.com/solid-sinusoid/behavior_tree.git
+    version: master

rbs_bt_executor/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.8)
+project(rbs_bt_executor)
+
+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(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rbs_skill_interfaces REQUIRED)
+find_package(behavior_tree REQUIRED)
+find_package(control_msgs REQUIRED)
+
+if (NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(dependencies
+    rclcpp
+    rclcpp_action
+    geometry_msgs    
+    tf2_geometry_msgs
+    moveit_msgs
+    moveit_core
+    moveit_ros_planning
+    moveit_ros_planning_interface
+    ament_index_cpp
+    rbs_skill_interfaces
+    behavior_tree
+    std_msgs
+    control_msgs
+)
+
+include_directories(include)
+
+add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
+list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
+
+
+foreach(bt_plugin ${plugin_libs})
+    ament_target_dependencies(${bt_plugin} ${dependencies})
+    target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+endforeach()
+
+
+
+install(DIRECTORY launch bt_trees config DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+    ${plugin_libs}
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION lib/${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_export_dependencies(${dependencies})
+
+ament_package()

rbs_bt_executor/bt_trees/test_tree.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="MainTree">
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+                <Action ID="MoveToPose"  
+                    robot_name="ur_manipulator" 
+                    pose="pose1"
+
+                    server_name="/move_topose" 
+                    server_timeout="1000"
+                    cancel_timeout="500" />
+
+                    <!-- <Action ID="MoveToPose"  
+                    robot_name="ur_manipulator" 
+                    pose="pose2"
+
+                    server_name="/move_topose" 
+                    server_timeout="10" 
+                    cancel_timeout="5" />
+
+                    <Action ID="MoveToPose"  
+                    robot_name="ur_manipulator" 
+                    pose="pose3"
+
+                    server_name="/move_topose" 
+                    server_timeout="10" 
+                    cancel_timeout="5" /> -->
+        </Sequence>
+    </BehaviorTree>
+
+    <TreeNodesModel>
+        <Action ID="MoveToPose">
+            <input_port name="robot_name"/>
+            <input_port name="pose"/>
+        </Action>
+    </TreeNodesModel>
+</root>

rbs_bt_executor/config/params.yaml

@@ -0,0 +1,6 @@
+# Here is a nodes which calling from launch for bt_tree
+simple_move_bt_nodes:
+  ros__parameters:
+    plugins:
+      - rbs_skill_move_topose_bt_action_client
+

rbs_bt_executor/config/points.yaml

@@ -0,0 +1,29 @@
+bt_engine:
+  ros__parameters:
+    pose1: [
+        0.11724797630931184,    #X  position
+        0.46766635768602544,    #Y
+        0.5793862343094913,     #Z
+        0.9987999001314066,     #X  orientation
+        0.041553846820940925,   #Y
+        -0.004693314677006583,  #Z
+        -0.025495295825239704   #W
+    ]
+    pose2: [
+        -0.11661364861606047,
+        0.4492600889665261,
+        0.5591700913807053,
+        0.9962273179258467,
+        0.04057380155886888,
+        0.009225849745372298,
+        0.07615629548377048
+    ]
+    pose3: [
+        -0.07133612047767886,
+        0.41038906578748613,
+        0.2844649846989331,
+        0.999078481789772,
+        0.04109234110437432,
+        0.006539754292177074,
+        0.010527978961032304
+    ]

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -0,0 +1,35 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory 
+import os
+
+
+def generate_launch_description():
+
+    bt_config = os.path.join(
+        get_package_share_directory('rbs_bt_executor'),
+        'config',
+        'params.yaml'
+    )
+    points = os.path.join(
+        get_package_share_directory('rbs_bt_executor'),
+        'config',
+        'points.yaml'
+    )
+
+
+    return LaunchDescription([
+        Node(
+            package='behavior_tree',
+            namespace='',
+            executable='bt_engine',
+            name="bt_engine",
+            # Do not declare a node name otherwise it messes with the action node names and will result in
+            # duplicate nodes!
+            parameters=[
+                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_queue'), 'bt_xmls/test_tree.xml')},
+                {'plugins': ['rbs_skill_move_topose_bt_action_client']},
+                points
+            ]
+        ),
+    ])

rbs_bt_executor/package.xml

@@ -1,25 +1,20 @@
 <?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>scene_monitor_interfaces</name>
+  <name>rbs_bt_executor</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
+  <maintainer email="ur.narmak@gmail.com">root</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>rosidl_default_generators</build_depend>
-  
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <depend>geometry_msgs</depend>
-  <depend>std_msgs</depend>
 
+  <depend>rbs_skill_interfaces</depend>
+  <depend>behavior_tree</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

rbs_bt_executor/src/MoveToPose.cpp

@@ -1,4 +1,4 @@
-#include "robossembler_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
 
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behavior_tree/BtAction.hpp"
@@ -8,7 +8,7 @@
 #include "rclcpp/parameter.hpp"
 
 using namespace BT;
-using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
+using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
 
 class MoveToPose : public BtAction<MoveToPoseAction>
@@ -17,22 +17,33 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<MoveToPoseAction>(name, config) 
         {
+            //auto params = _node->declare_parameters("Poses")
+            RCLCPP_INFO(_node->get_logger(), "Start the node");
+            pose_des.position.x = 0.11724797630931184;
+            pose_des.position.y = 0.46766635768602544;
+            pose_des.position.z = 0.5793862343094913;
+            pose_des.orientation.x = 0.9987999001314066;
+            pose_des.orientation.y = 0.041553846820940925;
+            pose_des.orientation.z = -0.004693314677006583;
+            pose_des.orientation.w = -0.025495295825239704;
         }
 
         MoveToPoseAction::Goal populate_goal() override
         {
             getInput<std::string>("robot_name", robot_name_);
-            getInput<geometry_msgs::msg::Pose>("pose", pose_);
+            getInput<std::string>("pose", pose_);
+
+            //auto pose =  _node->get_parameter(pose_).get_parameter_value().get<geometry_msgs::msg::Pose>();
 
             RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
-                pose_.position.x, pose_.position.y, pose_.position.z,
-                pose_.orientation.x, pose_.orientation.y, pose_.orientation.z, pose_.orientation.w);
+               pose_des.position.x, pose_des.position.y, pose_des.position.z,
+               pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);
 
             auto goal = MoveToPoseAction::Goal();
             RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
             
             goal.robot_name = robot_name_;
-            goal.target_pose = pose_;
+            goal.target_pose = pose_des;
             goal.end_effector_acceleration = 1.0;
             goal.end_effector_velocity = 1.0;
             
@@ -45,13 +56,16 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         {
             return providedBasicPorts({
                 InputPort<std::string>("robot_name"),
-                InputPort<geometry_msgs::msg::Pose>("pose")
+                InputPort<std::string>("pose")
             });
         }
 
     private:
         std::string robot_name_;
-        geometry_msgs::msg::Pose pose_;
+        std::string pose_;
+        geometry_msgs::msg::Pose pose_des;
+        std::map<std::string, geometry_msgs::msg::Pose> Poses;
+
 
 }; // class MoveToPose
 

rbs_simulation/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(rbs_simulation)
+
+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
+  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()

rbs_simulation/launch/rbs_simulation.launch.py

@@ -0,0 +1,389 @@
+import os
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from ament_index_python.packages import get_package_share_directory, get_package_share_path
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ur_moveit_config.launch_common import load_yaml
+
+
+def generate_launch_description():
+    declared_arguments = []
+    # UR specific arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "rbs_robot_type",
+            description="Type of robot by name",
+            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            default_value="ur5e",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_limits",
+            default_value="true",
+            description="Enables the safety limits controller if true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_pos_margin",
+            default_value="0.15",
+            description="The margin to lower and upper limits in the safety controller.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "safety_k_position",
+            default_value="20",
+            description="k-position factor in the safety controller.",
+        )
+    )
+    # General arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "runtime_config_package",
+            default_value="ur_moveit_config",
+            description='Package with the controller\'s configuration in "config" folder. \
+        Usually the argument is not set, it enables use of a custom setup.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controllers_file",
+            default_value="ur_controllers.yaml",
+            description="YAML file with the controllers configuration.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="ur_description",
+            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="ur.urdf.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value='""',
+            description="Prefix of the joint names, useful for \
+        multi-robot setup. If changed than also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "start_joint_controller",
+            default_value="true",
+            description="Enable headless mode for robot control",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="joint_trajectory_controller",
+            description="Robot controller to start.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            default_value="ur_moveit_config",
+            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="ur.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("launch_rviz", default_value="true", description="Launch RViz?")
+    )
+
+    # Initialize Arguments
+    # TODO тут всё переименовать и сделать боеле универсальным как под нашего робото так и под UR чтобы запускалось одинаково
+    rbs_robot_type = LaunchConfiguration("rbs_robot_type")
+    safety_limits = LaunchConfiguration("safety_limits")
+    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
+    safety_k_position = LaunchConfiguration("safety_k_position")
+    # General arguments
+    runtime_config_package = LaunchConfiguration("runtime_config_package")
+    controllers_file = LaunchConfiguration("controllers_file")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    prefix = LaunchConfiguration("prefix")
+    start_joint_controller = LaunchConfiguration("start_joint_controller")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+
+    initial_joint_controllers = PathJoinSubstitution(
+        [FindPackageShare(runtime_config_package), "config", controllers_file]
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"]
+    )
+
+    world_config_file = PathJoinSubstitution(
+        [FindPackageShare("sdf_models"), "worlds", "empty_world.sdf"] # TODO тут пакет извне должен задаваться
+    )
+
+    print(world_config_file)
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare(description_package), "urdf", description_file]
+            ),
+            " ",
+            "safety_limits:=",
+            safety_limits,
+            " ",
+            "safety_pos_margin:=",
+            safety_pos_margin,
+            " ",
+            "safety_k_position:=",
+            safety_k_position,
+            " ",
+            "name:=",
+            "ur",
+            " ",
+            "ur_type:=", # TODO сделать более универсальным
+            rbs_robot_type,
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+            "sim_ignition:=true",
+            " ",
+            "simulation_controllers:=",
+            initial_joint_controllers,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[{"use_sim_time": True}, robot_description],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # There may be other controllers of the joints, but this is the initially-started one
+    initial_joint_controller_spawner_started = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[initial_joint_controller, "--controller-manager", "/controller_manager"],
+        condition=IfCondition(start_joint_controller),
+    )
+    initial_joint_controller_spawner_stopped = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"],
+        condition=UnlessCondition(start_joint_controller),
+    )
+
+    # Gazebo nodes
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_path('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')),
+        launch_arguments={'ign_args': ['-r'," ", world_config_file]}.items(),
+    )
+    # Spawn robot
+    gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
+                 arguments=[
+                    '-name', rbs_robot_type,
+                    '-x', '0.0',
+                    '-z', '0.0',
+                    '-y', '0.0',
+                    '-topic', '/robot_description'],
+                 output='screen')
+
+    # MoveIt Configuration
+    robot_description_semantic_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
+            ),
+            " ",
+            "name:=",
+            "ur",
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+        ]
+    )
+    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
+
+    robot_description_kinematics = PathJoinSubstitution(
+        [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
+    )
+
+    # robot_description_planning = {
+    # "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
+    # }
+
+    # Planning Configuration
+    ompl_planning_pipeline_config = {
+        "move_group": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+            "start_state_max_bounds_error": 0.1,
+        }
+    }
+    ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") # TODO сделать извне
+    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
+
+    # Trajectory Execution Configuration
+    controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
+    # the scaled_joint_trajectory_controller does not work on fake hardware
+    change_controllers = True
+    if change_controllers == "true":
+        controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
+        controllers_yaml["joint_trajectory_controller"]["default"] = True
+
+    moveit_controllers = {
+        "moveit_simple_controller_manager": controllers_yaml,
+        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+    }
+
+    trajectory_execution = {
+        "moveit_manage_controllers": True,
+        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_start_tolerance": 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        "publish_planning_scene": True,
+        "publish_geometry_updates": True,
+        "publish_state_updates": True,
+        "publish_transforms_updates": True,
+    }
+
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        name="move_group",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            # robot_description_planning,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            robot_description_kinematics,
+            # robot_description_planning,
+        ],
+        condition=IfCondition(launch_rviz),
+    )
+
+    move_topose_action_server = Node(
+        package="rbs_skill_servers",
+        executable="move_topose_action_server",
+        name="move_to_pose_moveit",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            # robot_description_planning,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {"use_sim_time": use_sim_time},
+        ]
+    )
+
+    move_cartesian_path_action_server = Node(
+        package="rbs_skill_servers",
+        executable="move_cartesian_path_action_server",
+        name="move_cartesian_path_action_server",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            # robot_description_planning,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {"use_sim_time": use_sim_time},
+        ]
+    )
+
+    nodes_to_start = [
+        robot_state_publisher_node,
+        joint_state_broadcaster_spawner,
+        rviz_node,
+        initial_joint_controller_spawner_stopped,
+        initial_joint_controller_spawner_started,
+        gazebo,
+        gazebo_spawn_robot,
+        move_group_node,
+        move_topose_action_server,
+        move_cartesian_path_action_server
+    ]
+
+    return LaunchDescription(declared_arguments + nodes_to_start)

rbs_simulation/package.xml

@@ -1,10 +1,10 @@
 <?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>robossembler_scene_monitor</name>
+  <name>rbs_simulation</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
+  <maintainer email="ur.narmak@gmail.com">root</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -13,11 +13,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>std_msgs</depend>
-  <!-- <depend>visualization_msgs</depend> -->
-  <depend>gazebo_msgs</depend>
   <depend>geometry_msgs</depend>
-  <!-- <depend>action_msgs</depend> -->
-  <depend>scene_monitor_interfaces</depend>
   <depend>nlohmann_json</depend>
 
   <test_depend>ament_lint_auto</test_depend>
@@ -26,4 +22,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

rbs_skill_interfaces/CMakeLists.txt

@@ -1,15 +1,5 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler_interfaces)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
+cmake_minimum_required(VERSION 3.8)
+project(rbs_skill_interfaces)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -19,6 +9,9 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
     "action/MoveitSendPose.action"
@@ -27,11 +20,20 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/PropertyValuePair.msg"
     "msg/ActionFeedbackStatusConstants.msg"
     "msg/ActionResultStatusConstants.msg"
-    DEPENDENCIES geometry_msgs std_msgs
+    "srv/BtInit.srv"
+    DEPENDENCIES geometry_msgs
 )
 
+
 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()
 

rbs_skill_interfaces/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.

rbs_skill_interfaces/action/MoveitSendJointStates.action

@@ -2,7 +2,7 @@
 
 #goal definition
 string robot_name
-robossembler_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value)
+rbs_skill_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value)
 float32 joint_value # send joint value to gripper
 float32 joints_velocity_scaling_factor
 float32 joints_acceleration_scaling_factor

rbs_skill_interfaces/package.xml

@@ -1,21 +1,23 @@
 <?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>robossembler_interfaces</name>
+  <name>rbs_skill_interfaces</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rosidl_default_generators</build_depend>
-  
   <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rbs_skill_interfaces/srv/BtInit.srv

@@ -0,0 +1,3 @@
+geometry_msgs/Pose[] target_poses
+---
+string result

rbs_skill_servers/CMakeLists.txt

@@ -1,5 +1,5 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler_servers)
+cmake_minimum_required(VERSION 3.8)
+project(rbs_skill_servers)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
@@ -26,9 +26,8 @@ find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(robossembler_interfaces REQUIRED)
+find_package(rbs_skill_interfaces REQUIRED)
 find_package(rmw REQUIRED)
-find_package(gazebo_ros_link_attacher REQUIRED)
 
 set(deps
   rclcpp
@@ -40,8 +39,7 @@ set(deps
   geometry_msgs
   tf2_ros
   rclcpp_components
-  robossembler_interfaces
-  gazebo_ros_link_attacher
+  rbs_skill_interfaces
 )
 #
 # include_directories(include)
@@ -55,13 +53,13 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp)
-ament_target_dependencies(gripper_cmd_action_server ${deps})
-target_include_directories(gripper_cmd_action_server PRIVATE
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_DIR}/include>
-    $<INSTALL_INTERFACE:include>)
-target_compile_definitions(gripper_cmd_action_server PRIVATE "GRIPPER_CMD_ACTION_SERVER_CPP_BUILDING_DLL")
-rclcpp_components_register_node(gripper_cmd_action_server PLUGIN "robossembler_servers::GripperCmd" EXECUTABLE gripper_cmd_node)
+# add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp)
+# ament_target_dependencies(gripper_cmd_action_server ${deps})
+# target_include_directories(gripper_cmd_action_server PRIVATE
+#     $<BUILD_INTERFACE:${CMAKE_CURRENT_DIR}/include>
+#     $<INSTALL_INTERFACE:include>)
+# target_compile_definitions(gripper_cmd_action_server PRIVATE "GRIPPER_CMD_ACTION_SERVER_CPP_BUILDING_DLL")
+# rclcpp_components_register_node(gripper_cmd_action_server PLUGIN "robossembler_servers::GripperCmd" EXECUTABLE gripper_cmd_node)
 
 add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
 ament_target_dependencies(move_to_joint_states_action_server ${deps})
@@ -77,7 +75,7 @@ install(
   move_topose_action_server
   move_to_joint_states_action_server
   move_cartesian_path_action_server
-  gripper_cmd_action_server
+  #gripper_cmd_action_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

rbs_skill_servers/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.

rbs_skill_servers/package.xml

@@ -1,11 +1,11 @@
 <?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>robossembler_servers</name>
+  <name>rbs_skill_servers</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -18,7 +18,7 @@
   <depend>rclcpp_action</depend>
   <depend>geometry_msgs</depend>
   <depend>action_msgs</depend>
-  <depend>robossembler_interfaces</depend>
+  <depend>rbs_skill_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rbs_skill_servers/src/move_cartesian_path_action_server.cpp

@@ -8,8 +8,8 @@
 
 // action libs
 #include "rclcpp_action/rclcpp_action.hpp"
-#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
-#include "robossembler_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
@@ -32,13 +32,13 @@
 #include "moveit_msgs/msg/robot_trajectory.hpp"
 #include "moveit_msgs/action/move_group.hpp"
 
-namespace robossembler_actions
+namespace rbs_skill_actions
 {
 
 class MoveCartesianActionServer : public rclcpp::Node
 {
 public:
-    using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
+    using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
 
     //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) 
     explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node)
@@ -159,7 +159,7 @@ private:
     }
 }; // class MoveCartesianActionServer
 
-}// namespace robossembler_actions
+}// namespace rbs_skill_actions
 
 int main(int argc, char ** argv)
 {
@@ -168,7 +168,7 @@ int main(int argc, char ** argv)
     node_options.automatically_declare_parameters_from_overrides(true);
     auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
 
-    robossembler_actions::MoveCartesianActionServer server(node);
+    rbs_skill_actions::MoveCartesianActionServer server(node);
     std::thread run_server([&server]() {
         rclcpp::sleep_for(std::chrono::seconds(3));
         server.init();

rbs_skill_servers/src/move_to_joint_states_action_server.cpp

@@ -8,7 +8,7 @@
 
 // action libs
 #include "rclcpp_action/rclcpp_action.hpp"
-#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
@@ -30,13 +30,13 @@
 #include "moveit_msgs/msg/robot_trajectory.hpp"
 #include "moveit_msgs/action/move_group.hpp"
 
-namespace robossembler_actions
+namespace rbs_skill_actions
 {
 
 class MoveToJointStateActionServer : public rclcpp::Node
 {
 public:
-    using MoveitSendJointStates = robossembler_interfaces::action::MoveitSendJointStates;
+    using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates;
 
     //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) 
     explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node)
@@ -123,7 +123,7 @@ private:
 
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
         if(success)
         {
             RCLCPP_INFO(this->get_logger(), "Planning success");
@@ -145,7 +145,7 @@ private:
     }
 }; // class MoveToJointStateActionServer
 
-}// namespace robossembler_actions
+}// namespace rbs_skill_actions
 
 int main(int argc, char ** argv)
 {
@@ -154,7 +154,7 @@ int main(int argc, char ** argv)
     node_options.automatically_declare_parameters_from_overrides(true);
     auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
 
-    robossembler_actions::MoveToJointStateActionServer server(node);
+    rbs_skill_actions::MoveToJointStateActionServer server(node);
     std::thread run_server([&server]() {
         rclcpp::sleep_for(std::chrono::seconds(3));
         server.init();

rbs_skill_servers/src/move_topose_action_server.cpp

@@ -8,8 +8,8 @@
 
 // action libs
 #include "rclcpp_action/rclcpp_action.hpp"
-#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
-#include "robossembler_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
@@ -32,13 +32,13 @@
 #include "moveit_msgs/msg/robot_trajectory.hpp"
 #include "moveit_msgs/action/move_group.hpp"
 
-namespace robossembler_actions
+namespace rbs_skill_actions
 {
 
 class MoveToPoseActionServer : public rclcpp::Node
 {
 public:
-    using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
+    using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
 
     //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) 
     explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node)
@@ -119,7 +119,7 @@ private:
         move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
         if(success)
         {
             RCLCPP_INFO(this->get_logger(), "Planning success");
@@ -141,7 +141,7 @@ private:
     }
 }; // class MoveToPoseActionServer
 
-}// namespace robossembler_actions
+}// namespace rbs_skill_actions
 
 int main(int argc, char ** argv)
 {
@@ -150,7 +150,7 @@ int main(int argc, char ** argv)
     node_options.automatically_declare_parameters_from_overrides(true);
     auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
 
-    robossembler_actions::MoveToPoseActionServer server(node);
+    rbs_skill_actions::MoveToPoseActionServer server(node);
     std::thread run_server([&server]() {
         rclcpp::sleep_for(std::chrono::seconds(3));
         server.init();

robonomics/package.xml

@@ -8,7 +8,7 @@
   <license>Apache license 2.0</license>
 
   <depend>rclpy</depend>
-  <depend>plansys2_msgs</depend>
+  <!-- <depend>plansys2_msgs</depend> -->
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

robossembler/CMakeLists.txt

@@ -1,106 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler)
-
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(moveit_msgs REQUIRED)
-find_package(moveit_core REQUIRED)
-find_package(moveit_ros_planning REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
-find_package(plansys2_msgs REQUIRED)
-find_package(plansys2_domain_expert REQUIRED)
-find_package(plansys2_executor REQUIRED)
-find_package(plansys2_planner REQUIRED)
-find_package(plansys2_problem_expert REQUIRED)
-find_package(plansys2_pddl_parser REQUIRED)
-find_package(ament_index_cpp REQUIRED)
-find_package(plansys2_bt_actions REQUIRED)
-find_package(gazebo_msgs REQUIRED)
-find_package(robossembler_interfaces REQUIRED)
-find_package(behavior_tree REQUIRED)
-find_package(control_msgs REQUIRED)
-find_package(scene_monitor_interfaces REQUIRED)
-
-if (NOT CMAKE_CXX_STANDARD)
-    set(CMAKE_CXX_STANDARD 17)
-endif()
-
-set(dependencies
-    rclcpp
-    rclcpp_action
-    geometry_msgs    tf2_geometry_msgs
-    moveit_msgs
-    moveit_core
-    moveit_ros_planning
-    moveit_ros_planning_interface
-    plansys2_msgs
-    plansys2_domain_expert
-    plansys2_executor
-    plansys2_planner
-    plansys2_problem_expert
-    plansys2_pddl_parser
-    ament_index_cpp
-    plansys2_bt_actions
-    gazebo_msgs
-    robossembler_interfaces
-    behavior_tree
-    std_msgs
-    control_msgs
-    scene_monitor_interfaces
-)
-
-include_directories(include)
-
-add_library(robossembler_move_topose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp)
-list(APPEND plugin_libs robossembler_move_topose_bt_action_client)
-
-add_library(robossembler_get_entity_state_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp)
-list(APPEND plugin_libs robossembler_get_entity_state_bt_action_client)
-
-add_library(robossembler_move_gripper_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
-list(APPEND plugin_libs robossembler_move_gripper_bt_action_client)
-
-add_library(robossembler_print_bt_node SHARED src/behavior_tree_nodes/Print.cpp)
-list(APPEND plugin_libs robossembler_print_bt_node)
-
-add_library(robossembler_get_grasp_poses_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp)
-list(APPEND plugin_libs robossembler_get_grasp_poses_bt_action_client)
-
-add_library(robossembler_get_workspace_placement_pose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp)
-list(APPEND plugin_libs robossembler_get_workspace_placement_pose_bt_action_client)
-
-add_library(robossembler_construct_workspace_placement_pose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp)
-list(APPEND plugin_libs robossembler_construct_workspace_placement_pose_bt_action_client)
-
-foreach(bt_plugin ${plugin_libs})
-    ament_target_dependencies(${bt_plugin} ${dependencies})
-    target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
-endforeach()
-
-add_executable(move_controller_node src/move_controller_node.cpp)
-ament_target_dependencies(move_controller_node ${dependencies})
-
-
-install(DIRECTORY launch pddl behavior_trees_xml config DESTINATION share/${PROJECT_NAME})
-
-install(TARGETS
-    move_controller_node
-    ${plugin_libs}
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
-    find_package(ament_lint_auto REQUIRED)
-    ament_lint_auto_find_test_dependencies()
-
-    find_package(ament_cmake_gtest REQUIRED)
-endif()
-
-ament_export_dependencies(${dependencies})
-
-ament_package()

robossembler/README.md

@@ -1,40 +0,0 @@
-# Robossembler Plansys2
-
-This package is the main one and contains a task planner working in combination with MoveIt2. For more information about the requirements, read the main README.md
-
-
-```
-.
-├── behavior_trees_xml                          # Folder with BT nodes in XML file
-│   └── atomic_skills_xml
-│       ├── EmuPoseEstimation.xml
-│       └── move.xml
-|       └── move_gripper.xml
-├── CMakeLists.txt
-├── config
-│   └── params.yaml                             # Config params for PlanSys2 robot planning
-├── include                                     # Libs which used
-│   └── robot_bt
-│       └── behavior_tree_nodes
-│           └── atomic_skills
-│               ├── EmuPoseEstimation.hpp       # Lib for get Pose of model from Gazebo
-│               └── Move.hpp                    # Lib for P2P arm move
-|               └── MoveGripper.hpp             # Lib for P2P gripper move
-├── launch
-│   ├── robossembler_bringup.launch.py          # Launch file for start all scene
-│   └── task_planner.launch.py                  # Launch file for PlanSys2
-├── package.xml
-├── pddl
-│   ├── commands                                # Example commands for PlanSys2 terminal
-│   └── domain.pddl                             # The main domain file for launch PDDL
-├── README.md
-└── src
-    ├── atomic_tasks
-    ├── behavior_tree_nodes
-    │   └── atomic_skills
-    │       ├── EmuPoseEstimation.cpp
-    │       └── Move.cpp
-    |       └── MoveGripper.cpp
-    ├── move_action_node.cpp                    # Temporarily not used
-    └── move_controller_node.cpp                # Executable move arm controller (plan without terminal)
-```

robossembler/behavior_trees_xml/test_tree.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
-    <BehaviorTree ID="BehaviorTree">
-        <Sequence>
-            <Print server_name="/spawn_entity" server_timeout="10" frame="${arg0}" printer="${arg1}" material="${arg2}"/>
-        </Sequence>
-    </BehaviorTree>
-</root>

robossembler/config/params.yaml

@@ -1,22 +0,0 @@
-# assemble:
-#   ros__parameters:
-#     plugins:
-#       - robossembler_get_entity_state_bt_action_client
-#       - robossembler_move_topose_bt_action_client
-#       - robossembler_move_gripper_bt_action_client 
-#       - robossembler_get_grasp_poses_bt_action_client
-
-print_part:
-  ros__parameters:
-    plugins:
-      - robossembler_print_bt_node
-
-remove_part:
-  ros__parameters:
-    plugins:
-      - robossembler_get_grasp_poses_bt_action_client
-      - robossembler_move_topose_bt_action_client
-      - robossembler_move_gripper_bt_action_client
-      - robossembler_get_workspace_placement_pose_bt_action_client
-      - robossembler_construct_workspace_placement_pose_bt_action_client
-

robossembler/include/robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp

@@ -1,36 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <behavior_tree/BtService.hpp>
-#include <scene_monitor_interfaces/srv/construct_workspace_placement_pose.hpp>
-
-#include <behaviortree_cpp_v3/bt_factory.h>
-#include <geometry_msgs/msg/pose.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-
-class ConstructWorkspacePlacementPose : public BtService<scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose>
-{
-public:
-    ConstructWorkspacePlacementPose(const std::string &xml_tag_name, 
-        const BT::NodeConfiguration &conf);
-
-    scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose::Request::SharedPtr populate_request() override;
-
-    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose::Response::SharedPtr response) override;
-
-    static BT::PortsList providedPorts()
-    {
-        return providedBasicPorts({
-            BT::InputPort<std::string>("part_id"),
-            BT::InputPort<std::string>("workspace"),
-            BT::OutputPort<std::string>("constructed_part"),
-        });
-    }
-
-private:
-    std::string frame_;
-    std::string workspace_;
-};

robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp

@@ -1,38 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <behavior_tree/BtService.hpp>
-#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
-
-#include <behaviortree_cpp_v3/bt_factory.h>
-#include <geometry_msgs/msg/pose.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-
-class GetGraspPoses : public BtService<scene_monitor_interfaces::srv::GetGraspPoses>
-{
-public:
-    GetGraspPoses(const std::string &xml_tag_name, 
-        const BT::NodeConfiguration &conf);
-
-    scene_monitor_interfaces::srv::GetGraspPoses::Request::SharedPtr populate_request() override;
-
-    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetGraspPoses::Response::SharedPtr response) override;
-
-    static BT::PortsList providedPorts()
-    {
-        return providedBasicPorts({
-            BT::InputPort<std::string>("part_id"),
-            BT::OutputPort<geometry_msgs::msg::Pose>("grasp_pose"),
-            BT::OutputPort<geometry_msgs::msg::Pose>("pre_grasp_pose"),
-            BT::OutputPort<double>("pre_gap_distance"),
-            BT::OutputPort<double>("gap_distance")
-        });
-    }
-
-private:
-    std::string frame_;
-    scene_monitor_interfaces::msg::GraspPose grasp_pose_;
-};

robossembler/include/robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp

@@ -1,39 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <behavior_tree/BtService.hpp>
-#include <scene_monitor_interfaces/srv/get_workspace_placement_pose.hpp>
-
-#include <behaviortree_cpp_v3/bt_factory.h>
-#include <geometry_msgs/msg/pose.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-
-class GetWorkspacePlacementPose : public BtService<scene_monitor_interfaces::srv::GetWorkspacePlacementPose>
-{
-public:
-    GetWorkspacePlacementPose(const std::string &xml_tag_name, 
-        const BT::NodeConfiguration &conf);
-
-    scene_monitor_interfaces::srv::GetWorkspacePlacementPose::Request::SharedPtr populate_request() override;
-
-    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetWorkspacePlacementPose::Response::SharedPtr response) override;
-
-    static BT::PortsList providedPorts()
-    {
-        return providedBasicPorts({
-            BT::InputPort<std::string>("part_id"),
-            BT::InputPort<std::string>("workspace"),
-            BT::OutputPort<geometry_msgs::msg::Pose>("placement_pose"),
-            BT::OutputPort<geometry_msgs::msg::Pose>("pre_placement_pose"),
-        });
-    }
-
-private:
-    std::string frame_;
-    std::string workspace_;
-    geometry_msgs::msg::Pose placement_pose_;
-    geometry_msgs::msg::Pose pre_placement_pose_;
-};

robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp

@@ -1,36 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <behavior_tree/BtService.hpp>
-#include <scene_monitor_interfaces/srv/print_part.hpp>
-
-#include <behaviortree_cpp_v3/bt_factory.h>
-#include <geometry_msgs/msg/pose.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-
-class Print : public BtService<scene_monitor_interfaces::srv::PrintPart>
-{
-public:
-    Print(const std::string &xml_tag_name, 
-        const BT::NodeConfiguration &conf);
-
-    scene_monitor_interfaces::srv::PrintPart::Request::SharedPtr populate_request() override;
-
-    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::PrintPart::Response::SharedPtr response) override;
-
-    static BT::PortsList providedPorts()
-    {
-        return providedBasicPorts({
-            BT::InputPort<std::string>("frame"),
-            BT::InputPort<std::string>("printer")
-        });
-    }
-
-private:
-
-    std::string printer_;
-    std::string frame_;
-};

robossembler/launch/bt_check.launch.py

@@ -1,20 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory 
-import os
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='robossembler',
-            namespace='',
-            executable='bt_check',
-            # Do not declare a node name otherwise it messes with the action node names and will result in
-            # duplicate nodes!
-            parameters=[
-                {'bt_file_path': os.path.join(get_package_share_directory('robossembler'), 'behavior_trees_xml/atomic_skills_xml/simple_sequence.xml')},
-                {'plugins': ['robossembler_get_entity_state_bt_action_client', 'robossembler_move_topose_bt_action']}
-            ]
-        ),
-    ])

robossembler/launch/robossembler_bringup.launch.py

@@ -1,117 +0,0 @@
-from launch.actions.declare_launch_argument import DeclareLaunchArgument
-from launch.launch_description import LaunchDescription
-from launch.actions import IncludeLaunchDescription, OpaqueFunction
-from launch.conditions import IfCondition
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
-from launch.substitutions.launch_configuration import LaunchConfiguration
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from ament_index_python import get_package_share_directory
-import xacro
-import os
-
-def generate_launch_description():
-    launch_args = []
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="grip",
-            default_value="true",
-            description="On or OFF gripper for launch"
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="sim",
-            default_value="true",
-            description="On or OFF simulation"
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="robot_name",
-            default_value="rasmt",
-            description="Robot name"
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="plansys2",
-            default_value="false",
-            description="enable plansys2"
-        )
-    )
-
-    # Load robot description
-    robot_description_content = Command(
-        [
-        FindExecutable(name="xacro"), " ",
-        PathJoinSubstitution(
-            [FindPackageShare("rasmt_support"), "urdf/rasmt.xacro"]
-        ), " ",
-        "robot_name:=", LaunchConfiguration("robot_name"), " ",
-        "sim:=", LaunchConfiguration("sim"), " ",
-        "grip:=", LaunchConfiguration("grip")
-        ]
-    )
-
-    control = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare("rasmt_support"),
-                "launch",
-                "rasmt_control.launch.py"
-            ])
-        ), launch_arguments=[
-            ("robot_description", robot_description_content),
-            ("sim", LaunchConfiguration("sim"))
-        ]
-    )
-
-    simulation = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare("rasmt_support"),
-                "launch",
-                "rasmt_gazebo.launch.py"
-            ])
-        ), launch_arguments=[
-            ("robot_name", LaunchConfiguration("robot_name"))
-        ],
-        condition=IfCondition(LaunchConfiguration("sim"))
-    )
-
-    moveit = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare("rasmt_moveit_config"),
-                "launch",
-                "rasmt_moveit.launch.py"
-            ])
-        ), launch_arguments=[
-            ("robot_description", robot_description_content),
-            ("sim",LaunchConfiguration("sim"))
-        ]
-    )
-
-    task_planner_init = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare("robossembler"),
-                "launch",
-                "task_planner.launch.py"
-            ])
-        ), launch_arguments=[
-            ("robot_description", robot_description_content)
-        ],
-        condition=IfCondition(LaunchConfiguration("plansys2"))
-    )
-
-    launch_nodes = []
-    launch_nodes.append(control)
-    launch_nodes.append(simulation)
-    launch_nodes.append(moveit)
-    launch_nodes.append(task_planner_init)
-
-
-    return LaunchDescription(launch_args + launch_nodes)

robossembler/launch/task_planner.launch.py

@@ -1,163 +0,0 @@
-import os
-import yaml
-import xacro
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-__debug = True
-
-def load_file(package_name, file_path):
-    package_path = get_package_share_directory(package_name)
-    absolute_file_path = os.path.join(package_path, file_path)
-
-    try:
-        with open(absolute_file_path, "r") as file:
-            return file.read()
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
-        return None
-
-
-def load_yaml(package_name, file_path):
-    package_path = get_package_share_directory(package_name)
-    absolute_file_path = os.path.join(package_path, file_path)
-
-    try:
-        with open(absolute_file_path, "r") as file:
-            return yaml.safe_load(file)
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
-        return None
-
-
-
-def generate_launch_description():
-
-    robot_description_semantic_config = load_file(
-        "rasmt_moveit_config", "config/rasmt.srdf"
-    )
-    robot_description_semantic = {
-        "robot_description_semantic": robot_description_semantic_config
-    }
-
-    kinematics_yaml = load_yaml(
-        "rasmt_moveit_config", "config/kinematics.yaml"
-    )
-
-    pkg_dir = get_package_share_directory('robossembler')
-    namespace = LaunchConfiguration('namespace')
-
-    declare_namespace_cmd = DeclareLaunchArgument(
-        name="namespace",
-        default_value='',
-        description='Namespace')
-
-    """declare_robot_description = DeclareLaunchArgument(
-        name="robot_description",
-        description="Robot description XML file"
-    )"""
-    # stdout_linebuf_envvar = SetEnvironmentVariable(
-    #     'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
-
-    if __debug:
-        # get xacro file path
-        xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro")
-        # get error if xacro file if missing
-        assert os.path.exists(xacro_file), "The xacro file of neptun_description doesnt exist"+str(xacro_file)
-        # parse xacro file from file with arguments
-        robot_description_file = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"false",'robot_name':"rasmt"})
-        # convert file to xml format
-        robot_description_content = robot_description_file.toxml()
-        robot_description = {'robot_description': robot_description_content}
-
-    plansys2_cmd = IncludeLaunchDescription(
-      PythonLaunchDescriptionSource(os.path.join(
-        get_package_share_directory('plansys2_bringup'),
-        'launch',
-        'plansys2_bringup_launch_monolithic.py')),
-        launch_arguments={
-          'model_file': pkg_dir + '/pddl/domain.pddl',
-          'namespace': namespace
-        }.items())
-    """
-    assemble = Node(
-        package='plansys2_bt_actions',
-        executable='bt_action_node',
-        name='assemble',
-        namespace=namespace,
-        output='screen',
-        parameters=[
-          pkg_dir + '/config/params.yaml',
-          robot_description, 
-          robot_description_semantic, 
-          kinematics_yaml,
-          {
-            'action_name': 'assemble',
-            # 'publisher_port': 1666,
-            # 'server_port': 1667,
-            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
-          }
-        ])
-    """
-    print_part = Node(
-        package='plansys2_bt_actions',
-        executable='bt_action_node',
-        name='print_part',
-        namespace=namespace,
-        output='screen',
-        parameters=[
-          pkg_dir + '/config/params.yaml',
-          {
-            'action_name': 'print',
-            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/print.xml',
-          }
-        ])
-    
-    remove_part = Node(
-        package='plansys2_bt_actions',
-        executable='bt_action_node',
-        name='remove_part',
-        namespace=namespace,
-        output='screen',
-        parameters=[
-          pkg_dir + '/config/params.yaml',
-          robot_description, 
-          robot_description_semantic, 
-          kinematics_yaml,
-          {
-            'action_name': 'remove',
-            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/remove.xml',
-          }
-        ])
-
-    """gz_get_entity_state = Node(
-        package='plansys2_bt_actions',
-        executable='bt_action_node',
-        name='get_part_state',
-        namespace=namespace,
-        output='screen',
-        parameters=[
-            pkg_dir + '/config/params.yaml',
-            {
-                'action_name':'get_part_state',
-                'bt_xml_file':pkg_dir + '/behavior_trees_xml/atomic_skills_xml/get_part_state.xml'
-            }
-        ]
-    )"""
-    
-    ld = LaunchDescription()
-    
-    ld.add_action(declare_namespace_cmd)
-    #ld.add_action(declare_robot_description)
-    #ld.add_action(gz_get_entity_state)
-
-    # Declare the launch options
-    ld.add_action(plansys2_cmd)
-    # ld.add_action(assemble)
-    ld.add_action(print_part)
-    ld.add_action(remove_part)
-    
-    return ld

robossembler/package.xml

@@ -1,42 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xds" schematypens="http://www.w3.org/2001/XMLSchema"?>
-
-<package format="3">
- <name>robossembler</name>
- <version>0.0.1</version>
-
- <description>ROS2 task planner for manipulator</description>
- <maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
- 
- <license>Apache License, Version 2.0</license>
- <buildtool_depend>ament_cmake</buildtool_depend>
-
- <depend>rclcpp</depend>
- <depend>rclcpp_action</depend>
- <depend>geometry_msgs</depend>
- <depend>tf2_geometry_msgs</depend>
- <depend>gazebo_msgs</depend>
- <depend>moveit_msgs</depend>
- <depend>moveit_core</depend>
- <depend>moveit_ros_planning</depend>
- <depend>moveit_ros_planning_interface</depend>
- <depend>plansys2_msgs</depend>
- <depend>plansys2_domain_expert</depend>
- <depend>plansys2_executor</depend>
- <depend>plansys2_planner</depend>
- <depend>plansys2_problem_expert</depend>
- <depend>plansys2_pddl_parser</depend>
- <depend>plansys2_bt_actions</depend>
- <depend>ament_index_cpp</depend>
- <depend>robossembler_interfaces</depend>
- <depend>behavior_tree</depend>
-  <depend>scene_monitor_interfaces</depend>
-
- <test_depend>ament_lint_common</test_depend>
- <test_depend>ament_lint_auto</test_depend>
- <test_depend>ament_cmake_gtest</test_depend>
-
- <export>
-  <build_type>ament_cmake</build_type>
- </export>
-</package>

robossembler/pddl/commands

@@ -1,11 +0,0 @@
-// Move arm to pose
-set instance rasmt_arm_group robot
-set instance one zone
-set goal (and (robot_move rasmt_arm_group one))
-run
-
-// Move gripper to pose
-set instance rasmt_hand_arm_group robot
-set instance open gap
-set goal (and (gripper_move rasmt_hand_arm_group open))
-run

robossembler/pddl/domain.pddl

@@ -1,67 +0,0 @@
-;; Modified domain taken from
-;; "Knowledge transfer in robot manipulation tasks" by Jacob O. Huckaby 2014 
-(define (domain robossembler)
-    (:requirements :strips :typing :adl :fluents :durative-actions)
-    (:types
-        printer workspace - zone
-        part
-        arm
-        assembly
-    )
-
-    (:predicates
-        (arm_available ?a - arm)
-        (part_at ?p - part ?z - zone)
-        (printer_ready ?p - printer)
-        (part_of ?part - part ?whole - assembly)
-        (assembly_order ?prev ?next - assembly)
-        (assembled ?whole - assembly ?z - zone)
-    )
-
-    (:functions)
-
-    (:durative-action print
-        :parameters (?p - part ?pr - printer)
-        :duration ( = ?duration 10)
-        :condition (and
-            (at start(printer_ready ?pr))
-        )
-        :effect (and
-            (at start (not (printer_ready ?pr)))
-            (at end(part_at ?p ?pr))
-        )
-    )
-
-    (:durative-action remove
-        :parameters (?p - part ?pr - printer ?z - zone ?a - arm)
-        :duration (= ?duration 1)
-        :condition (and 
-            (at start (part_at ?p ?pr))
-            (at start (arm_available ?a))
-        )
-        :effect (and 
-            (at start (not (arm_available ?a)))
-            (at end (part_at ?p ?z))
-            (at end (arm_available ?a))
-            (at end (printer_ready ?pr))
-        )
-    )
-
-    (:durative-action assemble
-        :parameters (?p - part ?prev ?next - assembly ?w - workspace ?arm - arm)
-        :duration (= ?duration 5)
-        :condition (and
-            (at start (assembled ?prev ?w))
-            (at start (part-at ?p ?w))
-            (at start (part-of ?p ?next))
-            (at start (arm-available ?arm))
-            (at start (assembly-order ?prev ?next))
-        )
-        :effect (and
-            (at start (not (arm-available ?arm)))
-            (at end (not (part-at ?p ?w)))
-            (at end (arm-available ?arm))
-            (at end (assembled ?next ?w))
-        )
-    )
-);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;

robossembler/pddl/problem.pddl

@@ -1,18 +0,0 @@
-(define (problem p1)
-(:domain robossembler)
-(:objects
-	rasmt_arm_group - arm
-	printer1 printer2 - printer
-	workspace1 workspace2 - workspace
-	box1 - part
-)
-(:init
-	(printer_ready printer1)
-	(printer_ready printer2)
-	(arm_available rasmt_arm_group)
-)
-(:goal (and
-	(part_at box1 workspace1)
-	)
-)
-)

robossembler/pddl/problem_full.pddl

@@ -1,29 +0,0 @@
-(define (problem p1)
-(:domain robossembler)
-(:objects
-	;; information from Scene
-	rasmt - arm
-	printer1 printer2 - printer
-	workspace1 - workspace
-	;; information from CAD
-	part1 part2 - part
-	subasm0 subasm1 subasm2 - assembly
-)
-(:init
-	;; information from Scene
-	(printer-ready printer1)
-	(printer-ready printer2)
-	(arm-available rasmt)
-	;; information from CAD
-	(assembled subasm0 workspace1)
-	(part-of part1 subasm1)
-	(part-of part2 subasm2)
-	(assembly-order subasm0 subasm1)
-	(assembly-order subasm1 subasm2)
-)
-(:goal (and
-	;; information from CAD
-	(assembled subasm2 workspace1)
-	)
-)
-)

robossembler/src/behavior_tree_nodes/Print.cpp

@@ -1,54 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/Print.hpp"
-
-#include <scene_monitor_interfaces/srv/print_part.hpp>
-#include <rclcpp/qos.hpp>
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include <iostream>
-#include <filesystem>
-#include <fstream>
-#include <sstream>
-#include <thread>
-
-
-using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
-
-Print::Print(const std::string &xml_tag_name,
-                const BT::NodeConfiguration &conf)
-:BtService<PrintPartSrv>(xml_tag_name, conf)
-{}
-
-PrintPartSrv::Request::SharedPtr Print::populate_request()
-{
-    std::string frame, printer;
-    getInput<std::string>("frame", frame_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-    
-    getInput<std::string>("printer", printer_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< printer_ <<"]");
-
-    auto request = std::make_shared<PrintPartSrv::Request>();
-    request->frame_id = frame_;
-    request->printer_id = printer_;
-    
-    return request;
-}
-
-BT::NodeStatus Print::handle_response(PrintPartSrv::Response::SharedPtr response)
-{
-    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-    
-    if (!response->success)
-    {
-        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
-        return BT::NodeStatus::FAILURE;
-    }
-
-    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-    return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
-    factory.registerNodeType<Print>("Print");
-}

robossembler/src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp

@@ -1,58 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp"
-
-#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
-#include <scene_monitor_interfaces/srv/construct_workspace_placement_pose.hpp>
-#include <rclcpp/qos.hpp>
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include <iostream>
-#include <filesystem>
-#include <fstream>
-#include <sstream>
-#include <thread>
-
-
-using ConstructWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose;
-
-ConstructWorkspacePlacementPose::ConstructWorkspacePlacementPose(const std::string &xml_tag_name,
-                const BT::NodeConfiguration &conf)
-:BtService<ConstructWorkspacePlacementPoseSrv>(xml_tag_name, conf)
-{}
-
-ConstructWorkspacePlacementPoseSrv::Request::SharedPtr ConstructWorkspacePlacementPose::populate_request()
-{
-    std::string frame, printer;
-    getInput<std::string>("part_id", frame_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-    
-    getInput<std::string>("workspace", workspace_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< workspace_ <<"]");
-
-    auto request = std::make_shared<ConstructWorkspacePlacementPoseSrv::Request>();
-    request->frame_id = frame_;
-    request->workspace_id = workspace_;
-    
-    return request;
-}
-
-BT::NodeStatus ConstructWorkspacePlacementPose::handle_response(ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response)
-{
-    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-    
-    if (!response->success)
-    {
-        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
-        return BT::NodeStatus::FAILURE;
-    }
-
-    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-
-    setOutput<std::string>("constructed_part", frame_ + "_" + workspace_);
-
-    return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
-    factory.registerNodeType<ConstructWorkspacePlacementPose>("ConstructWorkspacePlacementPose");
-}

robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp

@@ -1,47 +0,0 @@
-#include "gazebo_msgs/srv/get_entity_state.hpp"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behavior_tree/BtService.hpp"
-
-using namespace BT;
-using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
-
-class GetEntityState : public BtService<GetEntityStateSrv>
-{
-    public:
-        GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
-        : BtService<GetEntityStateSrv>(name, config){
-            
-        }
-
-        GetEntityStateSrv::Request::SharedPtr populate_request() override
-        {
-            getInput<std::string>("part_name", part_name_);
-            //part_name_ = getInput<std::string>("PartName").value();
-
-            RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]");
-            
-            //GetEntityStateSrv::Request::SharedPtr request;
-            auto request = std::make_shared<GetEntityStateSrv::Request>();
-            request->name = part_name_;
-            RCLCPP_INFO(_node->get_logger(), "Request populated");
-            return request;
-        }
-        BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
-        {
-            RCLCPP_INFO_STREAM(_node->get_logger(), "part_name ["<< part_name_ <<"] Entity state x = "<< response->state.pose.position.x <<" y = "<< response->state.pose.position.y <<" z = "<< response->state.pose.position.z <<"");
-            return BT::NodeStatus::SUCCESS;
-        }
-
-        static PortsList providedPorts()
-        {
-            return providedBasicPorts({
-                    InputPort<std::string>("part_name")});
-        }
-    private:
-        std::string part_name_;
-};
-
-BT_REGISTER_NODES(factory)
-{
-    factory.registerNodeType<GetEntityState>("GetEntityState");
-}

robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp

@@ -1,58 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/GetGraspPoses.hpp"
-
-#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
-#include <rclcpp/qos.hpp>
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include <iostream>
-#include <filesystem>
-#include <fstream>
-#include <sstream>
-#include <thread>
-
-
-using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
-
-GetGraspPoses::GetGraspPoses(const std::string &xml_tag_name,
-                const BT::NodeConfiguration &conf)
-:BtService<GetGraspPosesSrv>(xml_tag_name, conf)
-{}
-
-GetGraspPosesSrv::Request::SharedPtr GetGraspPoses::populate_request()
-{
-    std::string frame, printer;
-    getInput<std::string>("part_id", frame_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-    
-    auto request = std::make_shared<GetGraspPosesSrv::Request>();
-    request->frame_id = frame_;
-    
-    return request;
-}
-
-BT::NodeStatus GetGraspPoses::handle_response(GetGraspPosesSrv::Response::SharedPtr response)
-{
-    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-    
-    if (!response->success)
-    {
-        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
-        return BT::NodeStatus::FAILURE;
-    }
-
-    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-
-    scene_monitor_interfaces::msg::GraspPose grasp_pose = response->grasp_poses[0];
-
-    setOutput<geometry_msgs::msg::Pose>("grasp_pose", grasp_pose.grasp_pose);
-    setOutput<geometry_msgs::msg::Pose>("pre_grasp_pose", grasp_pose.pre_grasp_pose);
-    setOutput<double>("pre_gap_distance", grasp_pose.gap_distance+0.006);
-    setOutput<double>("gap_distance", grasp_pose.gap_distance);
-
-    return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
-    factory.registerNodeType<GetGraspPoses>("GetGraspPoses");
-}

robossembler/src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp

@@ -1,61 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp"
-
-#include <scene_monitor_interfaces/srv/get_workspace_placement_pose.hpp>
-#include <rclcpp/qos.hpp>
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include <iostream>
-#include <filesystem>
-#include <fstream>
-#include <sstream>
-#include <thread>
-
-
-using GetWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::GetWorkspacePlacementPose;
-
-GetWorkspacePlacementPose::GetWorkspacePlacementPose(const std::string &xml_tag_name,
-                const BT::NodeConfiguration &conf)
-:BtService<GetWorkspacePlacementPoseSrv>(xml_tag_name, conf)
-{}
-
-GetWorkspacePlacementPoseSrv::Request::SharedPtr GetWorkspacePlacementPose::populate_request()
-{
-    std::string frame, printer;
-    getInput<std::string>("part_id", frame_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-    
-    getInput<std::string>("workspace", workspace_);
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< workspace_ <<"]");
-
-    auto request = std::make_shared<GetWorkspacePlacementPoseSrv::Request>();
-    request->frame_id = frame_;
-    request->workspace_id = workspace_;
-    
-    return request;
-}
-
-BT::NodeStatus GetWorkspacePlacementPose::handle_response(GetWorkspacePlacementPoseSrv::Response::SharedPtr response)
-{
-    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-    
-    if (!response->success)
-    {
-        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
-        return BT::NodeStatus::FAILURE;
-    }
-
-    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-
-    scene_monitor_interfaces::msg::GraspPose pose = response->grasp_poses[0];
-    pose.grasp_pose.position.z += 0.015;
-    setOutput<geometry_msgs::msg::Pose>("placement_pose", pose.grasp_pose);
-    pose.pre_grasp_pose.position.z += 0.015;
-    setOutput<geometry_msgs::msg::Pose>("pre_placement_pose", pose.pre_grasp_pose);
-
-    return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
-    factory.registerNodeType<GetWorkspacePlacementPose>("GetWorkspacePlacementPose");
-}

robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp

@@ -1,50 +0,0 @@
-//#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
-//#include "control_msgs/action/follow_joint_trajectory.hpp"
-#include "robossembler_interfaces/action/gripper_command.hpp"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behavior_tree/BtAction.hpp"
-
-using namespace BT;
-using GripperCmd = robossembler_interfaces::action::GripperCommand;
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_cmd");
-
-class SendGripperCmd : public BtAction<GripperCmd>
-{
-    public:
-        SendGripperCmd(const std::string & name, const BT::NodeConfiguration & config) 
-        : BtAction<GripperCmd>(name, config) 
-        {
-        }
-
-        GripperCmd::Goal populate_goal() override
-        {
-
-            auto goal = GripperCmd::Goal();
-
-            getInput<double>("gap_distance", distance_);
-            getInput<std::string>("frame_name", frame_);
-            goal.value = distance_;
-            goal.frame = frame_;
-
-            return goal;
-        }
-
-        static PortsList providedPorts()
-        {
-            return providedBasicPorts({
-                InputPort<double>("gap_distance"),
-                InputPort<std::string>("frame_name")
-            });
-        }
-
-    private:
-        std::vector<std::string> joint_names_ = {"rasmt_Slide_1", "rasmt_Slide_2"};
-        double distance_;
-        std::string frame_;
-
-}; // class MoveToJointState
-
-BT_REGISTER_NODES(factory)
-{
-    factory.registerNodeType<SendGripperCmd>("GripperCmd");
-}

robossembler/src/move_controller_node.cpp

@@ -1,141 +0,0 @@
-// Copyright 2022 Roman Andrianov
-//
-// /* LICENSE (Checkout: https://github.com/ament/ament_lint/tree/master/ament_copyright/test/cases) */
-
-#include <plansys2_pddl_parser/Utils.h>
-
-#include <memory>
-
-#include "plansys2_msgs/msg/action_execution_info.hpp"
-#include "plansys2_msgs/msg/plan.hpp"
-
-#include "plansys2_domain_expert/DomainExpertClient.hpp"
-#include "plansys2_executor/ExecutorClient.hpp"
-#include "plansys2_planner/PlannerClient.hpp"
-#include "plansys2_problem_expert/ProblemExpertClient.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-class MoveController : public rclcpp::Node
-{
-public:
-    MoveController()
-        : rclcpp::Node("move_controller")
-    {
-    }
-
-    bool init()
-    {
-        domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
-        planner_client_ = std::make_shared<plansys2::PlannerClient>();
-        problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
-        executor_client_ = std::make_shared<plansys2::ExecutorClient>();
-
-
-        init_knowledge();
-
-        auto domain = domain_expert_->getDomain();
-        auto problem = problem_expert_->getProblem();
-        auto plan = planner_client_->getPlan(domain, problem);
-
-        if (!plan.has_value()) {
-            std::cout << "Could not find plan to reach goal " <<
-            parser::pddl::toString(problem_expert_->getGoal()) << std::endl;
-            return false;
-        }
-
-        if (!executor_client_->start_plan_execution(plan.value()))
-        {
-            RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
-        }
-        return true;
-    }
-
-    void init_knowledge()
-    {
-        problem_expert_->addInstance(plansys2::Instance{"printer1", "printer"});
-        problem_expert_->addInstance(plansys2::Instance{"box1", "part"});
-        problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "arm"});
-        problem_expert_->addInstance(plansys2::Instance{"workspace1", "workspace"});
-
-
-        // problem_expert_->addInstance(plansys2::Instance("rasmt_hand_arm_group", "gripper"));
-
-        problem_expert_->addPredicate(plansys2::Predicate{"(printer_ready printer1)"});
-        problem_expert_->addPredicate(plansys2::Predicate{"(arm_available rasmt_arm_group)"});
-
-        problem_expert_->setGoal(plansys2::Goal("(and(part_at box1 workspace1))"));
-        // problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
-
-    }
-
-    /*std::string status_to_string(int8_t status)
-    {
-        switch (status)
-        {
-        case plansys2_msgs::msg::ActionExecutionInfo::NOT_EXECUTED:
-            return "NOT_EXECUTED";
-            break;
-        case plansys2_msgs::msg::ActionExecutionInfo::EXECUTING:
-            return "EXECUTING";
-            break;
-        case plansys2_msgs::msg::ActionExecutionInfo::FAILED:
-            return "FAILED";
-            break;
-        case plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED:
-            return "SUCCEEDED";
-            break;
-        case plansys2_msgs::msg::ActionExecutionInfo::CANCELLED:
-            return "CANCELLED";
-            break;
-        default:
-            return "UNKNOWN";
-            break;
-        }
-    }*/
-
-    void step()
-    {   
-        if (!executor_client_->execute_and_check_plan()) {  // Plan finished
-            auto result = executor_client_->getResult();
-
-        if (result.value().success) {
-            RCLCPP_INFO(get_logger(), "Plan succesfully finished");
-        } else {
-            RCLCPP_ERROR(get_logger(), "Plan finished with error");
-        }
-    }
-    }
-
-private:
-    std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
-    std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
-    std::shared_ptr<plansys2::ExecutorClient> executor_client_;
-    std::shared_ptr<plansys2::PlannerClient> planner_client_;
-};
-
-int main(int argc, char ** argv)
-{
-    rclcpp::init(argc, argv);
-    auto node = std::make_shared<MoveController>();
- 
-    //node->init();
-    if (!node->init())
-    {
-        RCLCPP_ERROR(node->get_logger(), "Error in Init");
-        return 0;
-    }
-
-    rclcpp::Rate rate(5);
-    while (rclcpp::ok()) {
-        node->step();
-
-        rate.sleep();
-        rclcpp::spin_some(node->get_node_base_interface());
-    }
-    
-    rclcpp::shutdown();
-    
-    return 0;
-}

robossembler_scene_monitor/nlohmann_json/CMakeLists.txt

@@ -1,37 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(nlohmann_json)
-
-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)
-
-add_library(${PROJECT_NAME} INTERFACE)
-
-target_include_directories(${PROJECT_NAME} INTERFACE
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include>"
-)
-
-install(TARGETS ${PROJECT_NAME}
-  EXPORT "export_${PROJECT_NAME}"
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib
-  INCLUDES DESTINATION include
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION include/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-#ament_export_libraries(${PROJECT_NAME})
-ament_export_targets("export_${PROJECT_NAME}")
-ament_package()

robossembler_scene_monitor/nlohmann_json/package.xml

@@ -1,18 +0,0 @@
-<?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>nlohmann_json</name>
-  <version>0.0.1</version>
-  <description>JSON library</description>
-  <maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
-  <license>BSD</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>

robossembler_scene_monitor/scene_monitor/CMakeLists.txt

@@ -1,76 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler_scene_monitor)
-
-if (NOT CMAKE_CXX_STANDARD)
-    set(CMAKE_CXX_STANDARD 17)
-endif()
-
-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)
-find_package(rclcpp REQUIRED)
-find_package(ament_index_cpp REQUIRED)
-# find_package(visualization_msgs REQUIRED)
-find_package(gazebo_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(scene_monitor_interfaces REQUIRED)
-find_package(nlohmann_json REQUIRED)
-
-set(dependencies
-    # visualization_msgs
-    std_msgs  
-    tf2
-    tf2_ros
-    gazebo_msgs
-    rclcpp
-    geometry_msgs
-    ament_index_cpp
-    nlohmann_json
-    scene_monitor_interfaces
-)
-
-include_directories(include)
-
-add_library(component_state_lib SHARED
-            src/component_pose_estimation.cpp
-            src/component_spawner.cpp
-            src/component.cpp
-            src/component_state_monitor.cpp
-)
-
-ament_target_dependencies(component_state_lib ${dependencies})
-
-add_executable(component_state_monitor_node src/component_state_monitor_node.cpp)
-target_link_libraries(component_state_monitor_node component_state_lib)
-ament_target_dependencies(component_state_monitor_node ${dependencies})
-
-install(DIRECTORY
-  launch
-  config
-  DESTINATION share/${PROJECT_NAME}
-)
-
-install(TARGETS
-  component_state_lib
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-
-install(TARGETS 
-    component_state_monitor_node
-    DESTINATION lib/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
-    find_package(ament_lint_auto REQUIRED)
-    ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml

@@ -1,20 +0,0 @@
-component_state_monitor:
-  ros__parameters:
-    models-package: "sdf_models"
-    global_frame: "world"
-    printers_names: ["printer1", "printer2"]
-    printers:
-      printer1:
-        position: [0.5, 0.0, 0.012]
-        orientation: [0.0, 0.0, 0.0, 1.0]
-      printer2:
-        position: [0.0, 0.5, 0.012]
-        orientation: [0.0, 0.0, 0.0, 0.0]
-    workspaces_names: ["workspace1", "workspace2"]
-    workspaces:
-      workspace1:
-        position: [0.3, 0.4, 0.012]
-        orientation: [0.0, 0.0, 0.5, 1.0]
-      workspace2:
-        position: [0.0, -0.5, 0.012]
-        orientation: [0.0, 0.0, 0.0, 0.0]

robossembler_scene_monitor/scene_monitor/include/component.hpp

@@ -1,77 +0,0 @@
-#pragma once
-
-#include <string>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/pose.hpp>
-
-#include "scene_monitor_interfaces/msg/grasp_pose.hpp"
-#include "nlohmann_json/json.hpp"
-
-namespace component_state_monitor
-{
-
-using json = nlohmann::json;
-
-/* 
-Description:
-    Initialized: 
-        - state when model is initialized by provided metadata
-    Printed:
-        - model is ready for manipulation and other operations in sim/real
-    Posted:
-        - model is posted with another
-*/
-
-enum ComponentState
-{
-    Initialized,
-    Printed,
-    Posted
-};
-
-enum ComponentType
-{
-    Part,
-    Assembly
-};
-
-class Component
-{
-public:
-
-    Component();
-    Component(const std::string &frame_name);
-
-    ComponentState getCurrentState() const;
-    ComponentType getType() const;
-    std::vector<std::string> getComponentLinks() const;
-    std::string getFrameName() const;
-    scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const;
-    std::vector<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
-    geometry_msgs::msg::Pose getPlacementPose() const;
-    bool getIsImage() const;
-
-    void setFrameName(const std::string &frame_name);
-    void setGraspPoses(const std::vector<scene_monitor_interfaces::msg::GraspPose> &grasp_poses);
-    void setPlacementPose(const geometry_msgs::msg::Pose &pose);
-    void setIsImage(const bool &is_image);
-    
-    void initializeFromJson(json &input);
-
-    void updateState(const ComponentState &state);
-
-private:
-
-    std::string _frame_name;
-    ComponentState _current_state;
-    ComponentType _component_type;
-    geometry_msgs::msg::Pose _placement;
-    std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
-    std::vector<std::string> _component_links;
-
-    bool _is_image;
-
-    geometry_msgs::msg::Pose _current_pose;
-};
-
-}

robossembler_scene_monitor/scene_monitor/include/component_pose_estimation.hpp

@@ -1,23 +0,0 @@
-#pragma once
-
-#include <rclcpp/rclcpp.hpp>
-#include <gazebo_msgs/srv/get_entity_state.hpp>
-
-namespace component_state_monitor
-{
-
-using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
-
-class ComponentPoseEstimation: public rclcpp::Node
-{
-
-public:
-    ComponentPoseEstimation();
-    geometry_msgs::msg::Pose framePose(const std::string& frame_id);
-
-private:
-    rclcpp::Client<GetEntityStateSrv>::SharedPtr _client;
-    geometry_msgs::msg::Pose _pose;
-};
-
-}// end namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/include/component_spawner.hpp

@@ -1,36 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <rclcpp/rclcpp.hpp>
-#include <gazebo_msgs/srv/spawn_entity.hpp>
-
-#include "component.hpp"
-
-namespace component_state_monitor
-{
-
-using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
-
-class ComponentSpawner: public rclcpp::Node
-{
-
-public:
-    ComponentSpawner();
-    ~ComponentSpawner();
-
-    bool spawnComponent(const component_state_monitor::Component& component, 
-                        const geometry_msgs::msg::Pose& printer_pose, 
-                        const std::string& printer_name);
-
-    void handleResponse(SpawnEntitySrv::Response::SharedPtr response);
-
-private:
-    rclcpp::Client<SpawnEntitySrv>::SharedPtr _spawn_component_client;
-    bool _result;
-    
-};
-
-
-} // namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -1,87 +0,0 @@
-#pragma once
-
-#include <string>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp/service.hpp>
-#include <rclcpp/timer.hpp>
-#include <gazebo_msgs/srv/spawn_entity.hpp>
-#include <tf2_ros/static_transform_broadcaster_node.hpp>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/buffer.h>
-#include <visualization_msgs/msg/marker_array.hpp>
-
-#include "scene_monitor_interfaces/srv/get_part.hpp"
-#include "scene_monitor_interfaces/srv/get_grasp_pose.hpp"
-#include "scene_monitor_interfaces/srv/get_grasp_poses.hpp"
-#include "scene_monitor_interfaces/srv/print_part.hpp"
-#include "scene_monitor_interfaces/srv/get_workspace_placement_pose.hpp"
-#include "scene_monitor_interfaces/srv/construct_workspace_placement_pose.hpp"
-
-#include "component.hpp"
-#include "component_spawner.hpp"
-#include "component_pose_estimation.hpp"
-
-namespace component_state_monitor
-{
-
-using GetPartSrv = scene_monitor_interfaces::srv::GetPart;
-using GetGraspPoseSrv = scene_monitor_interfaces::srv::GetGraspPose;
-using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
-using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
-using GetWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::GetWorkspacePlacementPose;
-using ConstructWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose;
-
-class ComponentStateMonitor: public rclcpp::Node
-{
-
-public:
-    ComponentStateMonitor();
-    ~ComponentStateMonitor();
-
-    void initializeComponents(const std::string &package_name);
-    void initializeParameters();
-
-private:
-
-    void getComponent(const GetPartSrv::Request::SharedPtr request,
-        GetPartSrv::Response::SharedPtr response);
-    void getGraspPoses(const GetGraspPosesSrv::Request::SharedPtr request,
-        GetGraspPosesSrv::Response::SharedPtr response);
-    void getGraspPose(const GetGraspPoseSrv::Request::SharedPtr request,
-        GetGraspPoseSrv::Response::SharedPtr response);
-    void printComponent(const PrintPartSrv::Request::SharedPtr request,
-        PrintPartSrv::Response::SharedPtr response);
-    void getWorkspacePlacementPose(const GetWorkspacePlacementPoseSrv::Request::SharedPtr request,
-        GetWorkspacePlacementPoseSrv::Response::SharedPtr response);
-    void constructWorkspacePlacementPose(const ConstructWorkspacePlacementPoseSrv::Request::SharedPtr request,
-        ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response);
-
-    void makeTransform(const Component& component, const std::string& parent);
-    void makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose, 
-                                                                      const std::string& parent);
-    void updateComponents();
-
-    std::map<std::string, Component> _loaded_components;
-    std::map<std::string, Component> _imaginary_components; 
-    rclcpp::Service<GetPartSrv>::SharedPtr _get_part_service;
-    rclcpp::Service<GetGraspPoseSrv>::SharedPtr _get_grasp_pose_service;
-    rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
-    rclcpp::Service<PrintPartSrv>::SharedPtr _print_part_service;
-    rclcpp::Service<GetWorkspacePlacementPoseSrv>::SharedPtr _get_workspace_placement_service;
-    rclcpp::Service<ConstructWorkspacePlacementPoseSrv>::SharedPtr _construct_workspace_placement_pose;
-
-    std::string _models_package;
-    std::map<std::string, geometry_msgs::msg::Pose> _loaded_printers;
-    std::map<std::string, geometry_msgs::msg::Pose> _loaded_workspaces;
-    ComponentSpawner _component_spawner_node;
-    ComponentPoseEstimation _pose_estimation_node;
-
-    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tf2_broadcaster;
-    std::shared_ptr<tf2_ros::TransformListener> _transform_listener;
-    std::unique_ptr<tf2_ros::Buffer> _tf2_buffer;
-    rclcpp::TimerBase::SharedPtr _timer;
-
-    std::string _global_frame;
-
-};
-}

robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp

@@ -1,13 +0,0 @@
-#pragma once 
-
-#include <rclcpp/rclcpp.hpp>
-
-namespace scene_state_monitor
-{
-
-class SceneStateMonitor: public rclcpp::Node
-{
-
-};
-
-}

robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py

@@ -1,41 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-
-    namespace = LaunchConfiguration('namespace')
-
-    component_state_params = os.path.join(
-        get_package_share_directory('robossembler_scene_monitor'),
-        'config', 'component_state_node.param.yaml')
-
-    stdout_linebuf_envvar = SetEnvironmentVariable(
-        'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
-
-    declare_namespace_cmd = DeclareLaunchArgument(
-        'namespace',
-        default_value='',
-        description='Namespace')
-
-    component_state_monitor_node = Node(
-        package="robossembler_scene_monitor",
-        executable="component_state_monitor_node",
-        name="component_state_monitor",
-        namespace=namespace,
-        output="screen",
-        parameters=[
-            component_state_params
-        ]
-    )
-
-    ld = LaunchDescription()
-    
-    ld.add_action(stdout_linebuf_envvar)
-    ld.add_action(declare_namespace_cmd)
-    ld.add_action(component_state_monitor_node)
-
-    return ld

robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/include/pose_estimation.hpp

@@ -1,23 +0,0 @@
-#pragma once
-#include <rclcpp/rclcpp.hpp>
-#include <gazebo_msgs/srv/get_entity_state.hpp>
-
-namespace pose_estimation
-{
-
-using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
-
-class PoseEstimation: public rclcpp::Node
-{
-public:
-    PoseEstimation();
-    geometry_msgs::msg::Pose frame_pose();
-    void set_frame(const std::string frame_name);
-
-private:
-    rclcpp::Client<GetEntityStateSrv>::SharedPtr _gz_state_client;
-    geometry_msgs::msg::Pose _pose;
-    std::string _frame;
-};
-
-}

robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/src/pose_estimation.cpp

@@ -1,48 +0,0 @@
-#include "pose_estimation.hpp"
-using namespace std::chrono_literals;
-
-namespace pose_estimation
-{
-PoseEstimation::PoseEstimation()
-: Node("pose_estimation_node")
-{
-    _gz_state_client = create_client<GetEntityStateSrv>("/get_entity_state");
-    while (!_gz_state_client->wait_for_service(1s))
-    {
-        if (!rclcpp::ok())
-        {
-            RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
-            rclcpp::shutdown();
-            return;
-        }
-        
-        RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
-    }
-};
-
-geometry_msgs::msg::Pose PoseEstimation::frame_pose()
-{
-    auto request = std::make_shared<gazebo_msgs::srv::GetEntityState::Request>();
-    request->name = _frame;
-    RCLCPP_INFO(this->get_logger(), "Provided service call with frame name [%s]", request->name.c_str());
-
-    auto future_response = _gz_state_client->async_send_request(
-        request, [this, request](rclcpp::Client<gazebo_msgs::srv::GetEntityState>::SharedFuture response) 
-        {
-            auto result = response.get();
-            if (!result->success)
-            {
-                RCLCPP_WARN(this->get_logger(), "Get failure response from service");
-                return;
-            }
-            
-            _pose = result->state.pose;
-
-            RCLCPP_INFO(this->get_logger(), "Get success response status");
-            RCLCPP_INFO(this->get_logger(), "update frame [%s] with \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
-                this->_frame.c_str(), result->state.pose.position.x, result->state.pose.position.y, result->state.pose.position.z,
-                result->state.pose.orientation.x, result->state.pose.orientation.y, result->state.pose.orientation.z, result->state.pose.orientation.w);
-        });
-}
-
-} // end pose_estimation namespace

robossembler_scene_monitor/scene_monitor/src/component.cpp

@@ -1,168 +0,0 @@
-#include "component.hpp"
-#include <rclcpp/rclcpp.hpp>
-
-namespace component_state_monitor
-{
-    Component::Component() {}
-
-    Component::Component(const std::string& frame_name): _frame_name(frame_name) {}
-
-    void Component::updateState(const ComponentState& state)
-    {
-        _current_state = state;
-    }
-
-    ComponentState Component::getCurrentState() const 
-    {
-        return _current_state;
-    }
-
-    std::string Component::getFrameName() const 
-    {
-        return _frame_name;
-    }
-
-    ComponentType Component::getType() const
-    {
-        return _component_type;
-    }
-
-    std::vector<std::string> Component::getComponentLinks() const
-    {
-        return _component_links;
-    }
-
-    scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const
-    {
-        if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end())
-        {
-            RCLCPP_WARN(rclcpp::get_logger("component"), "GraspPose with name (%s) not found", grasp_pose_name.c_str());
-            return scene_monitor_interfaces::msg::GraspPose();
-        }
-
-        return _grasp_poses.at(grasp_pose_name);
-    }
-
-    std::vector<scene_monitor_interfaces::msg::GraspPose> Component::getGraspPoses() const
-    {
-        std::vector<scene_monitor_interfaces::msg::GraspPose> grasp_poses;
-        for (auto &pose: _grasp_poses)
-        {
-            grasp_poses.push_back(pose.second);
-        }
-
-        return grasp_poses;
-    }
-
-    void Component::setFrameName(const std::string &frame_name)
-    {
-        _frame_name = frame_name;
-    }
-
-    void Component::setGraspPoses(const std::vector<scene_monitor_interfaces::msg::GraspPose> &grasp_poses)
-    {
-        _grasp_poses.clear();
-
-        for(const auto grasp_pose: grasp_poses)
-        {
-            _grasp_poses.insert({grasp_pose.label, grasp_pose});
-        }
-    }
-
-    void Component::setPlacementPose(const geometry_msgs::msg::Pose &pose)
-    {
-        _placement = pose;
-    }
-
-    void Component::setIsImage(const bool &is_image)
-    {
-        _is_image = is_image;
-    }
-
-    bool Component::getIsImage() const
-    {
-        return _is_image;
-    }
-
-    geometry_msgs::msg::Pose Component::getPlacementPose() const
-    {
-        return _placement;
-    }
-
-    inline geometry_msgs::msg::Point construct_point(const json& point)
-    {
-        geometry_msgs::msg::Point result_point;
-
-        result_point.x = point.at("x");
-        result_point.y = point.at("y");
-        result_point.z = point.at("z");
-
-        return result_point;
-    }
-
-    inline geometry_msgs::msg::Quaternion construct_quaternion(const json& quaternion)
-    {
-        geometry_msgs::msg::Quaternion result_quaternion;
-
-        result_quaternion.x = quaternion.at("x");
-        result_quaternion.y = quaternion.at("y");
-        result_quaternion.z = quaternion.at("z");
-        result_quaternion.w = quaternion.at("w");
-
-        return result_quaternion;
-    }
-
-    inline geometry_msgs::msg::Pose construct_pose(const json& pose)
-    {
-        geometry_msgs::msg::Pose result_pose;
-
-        result_pose.position = construct_point(pose.at("position"));
-        result_pose.orientation = construct_quaternion(pose.at("orientation"));
-
-        return result_pose;
-    }
-
-    void Component::initializeFromJson(json &input)
-    {
-        if (input.empty())
-        {
-            RCLCPP_WARN(rclcpp::get_logger("component"), "Provided empty json for initialize Component");
-            return; 
-        }
-
-        std::string type = input.at("type");
-        _component_type = type == "part"? ComponentType::Part: ComponentType::Assembly;
-
-        size_t size = input.at("include").size();
-        if (size > 0)
-        {
-            for (auto it=input.at("include").begin(); it != input.at("include").end(); ++it)
-            {
-                _component_links.push_back(it.value());
-            }
-        }
-
-        _placement = construct_pose(input.at("features").at("placements").at("placement"));
-
-        auto grasp_poses = input.at("features").at("grasp-poses");
-        for (auto it = grasp_poses.begin(); it != grasp_poses.end(); ++it)
-        {
-            scene_monitor_interfaces::msg::GraspPose grasp_pose;
-            grasp_pose.label = it.key();
-            grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
-            
-            geometry_msgs::msg::Pose pre_pose;
-            pre_pose.position.z -= 0.05;
-
-            grasp_pose.pre_grasp_pose = pre_pose;
-
-            grasp_pose.gap_distance = it.value().at("distance");
-
-            _grasp_poses.insert({grasp_pose.label, grasp_pose});
-        }
-
-        _current_state = ComponentState::Initialized;
-        _is_image = false;
-    }
-
-}

robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp

@@ -1,55 +0,0 @@
-#include "component_pose_estimation.hpp"
-
-namespace component_state_monitor
-{
-
-using namespace std::chrono_literals;
-
-ComponentPoseEstimation::ComponentPoseEstimation(): Node("component_pose_estimation_node")
-{
-    _client = create_client<GetEntityStateSrv>("/get_entity_state");
-    while (!_client->wait_for_service(1s))
-    {
-        if (!rclcpp::ok())
-        {
-            RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
-            rclcpp::shutdown();
-            return;
-        }
-        
-        RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
-    }
-}
-
-geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& frame_id)
-{
-    auto request = std::make_shared<GetEntityStateSrv::Request>();
-    request->name = frame_id;
-    // RCLCPP_INFO(this->get_logger(), "Provided service call with frame name [%s]", request->name.c_str());
-
-    auto future = _client->async_send_request(request);
-
-    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Could not build future");
-        return geometry_msgs::msg::Pose();
-    }
-
-    auto result = future.get();
-    if (!result->success)
-    {
-        RCLCPP_WARN(this->get_logger(), "Get bad response from service call");
-        return geometry_msgs::msg::Pose();
-    }
-
-    _pose = result->state.pose;
-    
-    // RCLCPP_INFO(this->get_logger(), "Get success response status");
-    // RCLCPP_INFO(this->get_logger(), "update frame (%s) with \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
-    //             frame_id.c_str(), _pose.position.x, _pose.position.y, _pose.position.z,
-    //             _pose.orientation.x, _pose.orientation.y, _pose.orientation.z, _pose.orientation.w);
-
-    return _pose;
-}
-
-}// end namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/src/component_spawner.cpp

@@ -1,109 +0,0 @@
-#include "component_spawner.hpp"
-
-#include <filesystem>
-#include <fstream>
-#include <sstream>
-
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-namespace component_state_monitor
-{
-
-using namespace std::chrono_literals;
-
-ComponentSpawner::ComponentSpawner(): Node("component_spawner_node")
-{
-    _spawn_component_client = create_client<SpawnEntitySrv>("/spawn_entity");
-
-    while (!_spawn_component_client->wait_for_service(1s))
-    {
-        if (!rclcpp::ok())
-        {
-            RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
-            rclcpp::shutdown();
-            return;
-        }
-        
-        RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
-    }
-}
-
-ComponentSpawner::~ComponentSpawner() {}
-
-bool ComponentSpawner::spawnComponent(const component_state_monitor::Component& component, 
-                                      const geometry_msgs::msg::Pose& printer_pose, 
-                                      const std::string& package_name)
-{
-    auto request = std::make_shared<SpawnEntitySrv::Request>();
-    request->name = component.getFrameName();
-    RCLCPP_INFO(this->get_logger(), "Provided service call with frame name (%s)", request->name.c_str());
-
-    std::string package_share_directory = ament_index_cpp::get_package_share_directory(package_name);
-    std::filesystem::path sdf_path = package_share_directory + "/models/" + component.getFrameName() + "/model.sdf";
-    
-    RCLCPP_INFO(this->get_logger(), "Start read frame (%s) frame_path (%s)", component.getFrameName().c_str(), sdf_path.c_str());
-
-    std::ifstream sdf_file(sdf_path);
-    std::stringstream buffer;
-    
-    if (sdf_file.is_open()) {       
-        buffer << sdf_file.rdbuf();         
-        sdf_file.close();
-    } else {
-        
-        RCLCPP_WARN(this->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
-        _result = false;
-        return _result;
-    }  
-
-    std::string xml = buffer.str();
-
-    geometry_msgs::msg::Pose spawn_pose;
-    auto placement_pose = component.getPlacementPose();
-
-    spawn_pose.position.x = printer_pose.position.x + placement_pose.position.x;
-    spawn_pose.position.y = printer_pose.position.y + placement_pose.position.y;
-    spawn_pose.position.z = printer_pose.position.z + placement_pose.position.z;
-
-    spawn_pose.orientation.x = printer_pose.orientation.x + placement_pose.orientation.x;
-    spawn_pose.orientation.y = printer_pose.orientation.y + placement_pose.orientation.y;
-    spawn_pose.orientation.z = printer_pose.orientation.z + placement_pose.orientation.z;
-    spawn_pose.orientation.w = printer_pose.orientation.w + placement_pose.orientation.w;
-
-    request->initial_pose = spawn_pose;
-    request->xml = xml;
-    RCLCPP_INFO(this->get_logger(), "Connecting to '/spawn_entity' service...");
-
-    auto future = _spawn_component_client->async_send_request(request);
-    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
-    {
-        RCLCPP_ERROR(this->get_logger(), "Could not build future");
-        return false;
-    }
-
-    auto result = future.get();
-    if (!result->success)
-    {
-        RCLCPP_WARN(this->get_logger(), "Get bad response from service call");
-        return false;
-    }
-
-    return true;
-}
-
-void ComponentSpawner::handleResponse(SpawnEntitySrv::Response::SharedPtr response)
-{
-    RCLCPP_INFO(this->get_logger(), "Handle Response call");
-    
-    if (!response->success)
-    {
-        RCLCPP_WARN(this->get_logger(), "Get failure response from service with msg: `%s`", response->status_message.c_str());
-        _result = false;
-        return;
-    }
-
-    RCLCPP_INFO(this->get_logger(), "Get success response status with msg: `%s`", response->status_message.c_str());
-    _result= true;
-}
-
-}

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -1,556 +0,0 @@
-#include <filesystem>
-#include <fstream>
-#include <sstream>
-#include <chrono>
-
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include "component_state_monitor.hpp"
-
-namespace component_state_monitor
-{
-    using namespace std::chrono_literals;
-    ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor")
-    {   
-        this->declare_parameter<std::string>("models-package", "sdf_models");
-        this->declare_parameter<std::string>("global_frame", "world");
-        this->declare_parameter<std::vector<std::string>>("printers_names", std::vector<std::string>{});
-        this->declare_parameter<std::vector<std::string>>("workspaces_names", std::vector<std::string>{});
-
-        RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
-        _get_part_service = this->create_service<GetPartSrv>("scene_monitor/get_part", 
-                                        std::bind(&ComponentStateMonitor::getComponent, this, 
-                                        std::placeholders::_1, std::placeholders::_2));
-
-        RCLCPP_INFO(this->get_logger(), "Initialize [get_grasp_poses] service");
-        _get_grasp_poses_service = this->create_service<GetGraspPosesSrv>("scene_monitor/get_grasp_poses", 
-                                        std::bind(&ComponentStateMonitor::getGraspPoses, this, 
-                                        std::placeholders::_1, std::placeholders::_2));
-
-        RCLCPP_INFO(this->get_logger(), "Initialize [get_grasp_pose] service");
-        _get_grasp_pose_service = this->create_service<GetGraspPoseSrv>("scene_monitor/get_grasp_pose", 
-                                        std::bind(&ComponentStateMonitor::getGraspPose, this, 
-                                        std::placeholders::_1, std::placeholders::_2));
-
-        RCLCPP_INFO(this->get_logger(), "Initialize [print_part] service");
-        _print_part_service = this->create_service<PrintPartSrv>("scene_monitor/print_part", 
-                                        std::bind(&ComponentStateMonitor::printComponent, this, 
-                                        std::placeholders::_1, std::placeholders::_2));
-        
-        RCLCPP_INFO(this->get_logger(), "Initialize [get_workspace_placement_pose] service");
-        _get_workspace_placement_service = this->create_service<GetWorkspacePlacementPoseSrv>("scene_monitor/get_workspace_placement_pose",
-                                        std::bind(&ComponentStateMonitor::getWorkspacePlacementPose, this,
-                                        std::placeholders::_1, std::placeholders::_2));
-        
-        RCLCPP_INFO(this->get_logger(), "Initialize [construct_workspace_placement_pose] service");
-        _construct_workspace_placement_pose = this->create_service<ConstructWorkspacePlacementPoseSrv>("scene_monitor/construct_workspace_placement_pose",
-                                        std::bind(&ComponentStateMonitor::constructWorkspacePlacementPose, this,
-                                        std::placeholders::_1, std::placeholders::_2));
-        
-        _tf2_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
-
-        _tf2_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
-        _transform_listener = std::make_shared<tf2_ros::TransformListener>(*_tf2_buffer);
-
-        initializeParameters();
-        initializeComponents(_models_package);
-
-        _timer = this->create_wall_timer(
-            20ms, std::bind(&ComponentStateMonitor::updateComponents, this)
-        );
-    }
-
-    inline geometry_msgs::msg::Pose transformToPose(const geometry_msgs::msg::TransformStamped transformStamped)
-    {
-        geometry_msgs::msg::Pose pose;
-
-        pose.position.x = transformStamped.transform.translation.x;
-        pose.position.y = transformStamped.transform.translation.y;
-        pose.position.z = transformStamped.transform.translation.z;
-
-        pose.orientation.x = transformStamped.transform.rotation.x;
-        pose.orientation.y = transformStamped.transform.rotation.y;
-        pose.orientation.z = transformStamped.transform.rotation.z;
-        pose.orientation.w = transformStamped.transform.rotation.w;
-
-        return pose;
-    }
-
-    void ComponentStateMonitor::updateComponents()
-    {
-        for (const auto& component: _loaded_components)
-        {
-            if (component.second.getCurrentState() == ComponentState::Printed)
-            {
-                // if (component.second.getIsImage() == true)
-                // {
-                //     makeTransform(component.first, component.second.getPlacementPose(), _global_frame);
-                // } else {
-                    
-                    makeTransform(component.second, _global_frame);
-                // }
-                // if (component.second.getType() == ComponentType::Assembly)
-                // {
-                //     auto links = component.second.getComponentLinks();
-                //     for (const auto link: links)
-                //     {
-                //         auto pose = _pose_estimation_node.framePose(component.first + "::" + link);
-                //         makeTransform(link, pose, _global_frame);
-                //     }
-                // }
-                auto grasp_poses = component.second.getGraspPoses();
-                for (const auto& grasp_pose: grasp_poses)
-                {
-                    makeTransform(grasp_pose.label, grasp_pose.grasp_pose, component.first);
-                    makeTransform(std::string("pre_" + grasp_pose.label), grasp_pose.pre_grasp_pose, grasp_pose.label);
-                }
-            }
-        }
-
-        if (!_imaginary_components.empty())
-        {
-            for (const auto& component: _imaginary_components)
-            {
-                makeTransform(component.first, component.second.getPlacementPose(), _global_frame);
-                auto grasp_poses = component.second.getGraspPoses();
-                for (const auto& grasp_pose: grasp_poses)
-                {
-                    makeTransform(grasp_pose.label, grasp_pose.grasp_pose, component.first);
-                    makeTransform(std::string("pre_" + grasp_pose.label), grasp_pose.pre_grasp_pose, grasp_pose.label);
-                }
-            }
-        }
-    }
-
-    void ComponentStateMonitor::makeTransform(const Component& component, const std::string& parent)
-    {
-        rclcpp::Time now = this->get_clock()->now();
-        geometry_msgs::msg::TransformStamped t;
-
-        t.header.stamp = now;
-        t.header.frame_id = parent;
-        t.child_frame_id = component.getFrameName();
-
-        auto fp = _pose_estimation_node.framePose(component.getFrameName());
-
-        t.transform.translation.x = fp.position.x;
-        t.transform.translation.y = fp.position.y;
-        t.transform.translation.z = fp.position.z;
-
-        t.transform.rotation.x = fp.orientation.x;
-        t.transform.rotation.y = fp.orientation.y;
-        t.transform.rotation.z = fp.orientation.z;
-        t.transform.rotation.w = fp.orientation.w;
-
-        _tf2_broadcaster->sendTransform(t);
-    }
-
-    void ComponentStateMonitor::makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose,
-                                                                                              const std::string& parent)
-    {
-        rclcpp::Time now = this->get_clock()->now();
-        geometry_msgs::msg::TransformStamped t;
-
-        t.header.stamp = now;
-        t.header.frame_id = parent;
-        t.child_frame_id = frame_name;
-
-        t.transform.translation.x = frame_pose.position.x;
-        t.transform.translation.y = frame_pose.position.y;
-        t.transform.translation.z = frame_pose.position.z;
-
-        t.transform.rotation.x = frame_pose.orientation.x;
-        t.transform.rotation.y = frame_pose.orientation.y;
-        t.transform.rotation.z = frame_pose.orientation.z;
-        t.transform.rotation.w = frame_pose.orientation.w;
-
-        _tf2_broadcaster->sendTransform(t);
-    }
-
-    void ComponentStateMonitor::initializeParameters()
-    {
-        this->get_parameter("models-package", _models_package);
-        RCLCPP_INFO(this->get_logger(), "Initialize with models-package (%s)", _models_package.c_str());
-
-        this->get_parameter("global_frame", _global_frame);
-        RCLCPP_INFO(this->get_logger(), "Initialize with global_frame (%s)", _global_frame.c_str());
-        
-        std::vector<std::string> printers;
-        this->get_parameter("printers_names", printers);
-        
-        for (const auto& printer: printers)
-        {
-            std::vector<double> position;
-            std::vector<double> orientation;
-            
-            this->declare_parameter<std::vector<double>>("printers." + printer + ".position", std::vector<double>{0.0, 0.0, 0.0});
-            this->declare_parameter<std::vector<double>>("printers." + printer + ".orientation", std::vector<double>{0.0, 0.0, 0.0, 0.0});
-
-            this->get_parameter("printers." + printer + ".position", position);
-            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) position (%f, %f, %f)",
-                printer.c_str(), position.at(0), position.at(1), position.at(2));
-
-            this->get_parameter("printers." + printer + ".orientation", orientation);
-            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) orientation (%f, %f, %f, %f)",
-                printer.c_str(), orientation.at(0), orientation.at(1), orientation.at(2), orientation.at(3));
-            
-            geometry_msgs::msg::Pose tmp_pose;
-            tmp_pose.position.x = position.at(0);
-            tmp_pose.position.y = position.at(1);
-            tmp_pose.position.z = position.at(2);
-
-            tmp_pose.orientation.x = orientation.at(0);
-            tmp_pose.orientation.y = orientation.at(1);
-            tmp_pose.orientation.z = orientation.at(2);
-            tmp_pose.orientation.w = orientation.at(3);
-
-            _loaded_printers.insert({printer, tmp_pose});
-        }
-
-        std::vector<std::string> workspaces;
-        this->get_parameter("workspaces_names", workspaces);
-
-        for (const auto& workspace: workspaces)
-        {
-            std::vector<double> position;
-            std::vector<double> orientation;
-
-            this->declare_parameter<std::vector<double>>("workspaces." + workspace + ".position", std::vector<double>{0.0, 0.0, 0.0});
-            this->declare_parameter<std::vector<double>>("workspaces." + workspace + ".orientation", std::vector<double>{0.0, 0.0, 0.0, 0.0});
-
-            this->get_parameter("workspaces." + workspace + ".position", position);
-            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) position (%f, %f, %f)",
-                workspace.c_str(), position.at(0), position.at(1), position.at(2));
-
-            this->get_parameter("workspaces." + workspace + ".orientation", orientation);
-            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) orientation (%f, %f, %f, %f)",
-                workspace.c_str(), orientation.at(0), orientation.at(1), orientation.at(2), orientation.at(3));
-            
-            geometry_msgs::msg::Pose tmp_pose;
-            tmp_pose.position.x = position.at(0);
-            tmp_pose.position.y = position.at(1);
-            tmp_pose.position.z = position.at(2);
-
-            tmp_pose.orientation.x = orientation.at(0);
-            tmp_pose.orientation.y = orientation.at(1);
-            tmp_pose.orientation.z = orientation.at(2);
-            tmp_pose.orientation.w = orientation.at(3);
-
-            _loaded_workspaces.insert({workspace, tmp_pose});
-        }
-    }
-
-    ComponentStateMonitor::~ComponentStateMonitor() {}
-
-    void ComponentStateMonitor::getComponent(const GetPartSrv::Request::SharedPtr request,
-            GetPartSrv::Response::SharedPtr response)
-    {
-        RCLCPP_INFO(this->get_logger(), "[get_part] Incoming request");
-
-        if (_loaded_components.find(request->frame_id) != _loaded_components.end())
-        {
-            RCLCPP_INFO(this->get_logger(), "[get_part] found frame (%s) in component_state_monitor", request->frame_id.c_str());
-            auto component = _loaded_components.at(request->frame_id);
-            response->frame_id = component.getFrameName();
-            response->grasp_poses = component.getGraspPoses();
-            response->placement_pose = component.getPlacementPose();
-            response->success = true;
-            response->message = "successfully received a response from the service [get_part] with frame (" + response->frame_id + ")";
-        } else {
-
-            RCLCPP_WARN(this->get_logger(), "[get_part] Frame with name (%s) not loaded in component_state_monitor", request->frame_id.c_str());
-            response->success = false;
-            response->message = "could not found frame (%s) in component_state_monitor";
-        }
-
-        RCLCPP_INFO(this->get_logger(), "[get_part] Sending back response");
-    }
-
-    void ComponentStateMonitor::constructWorkspacePlacementPose(const ConstructWorkspacePlacementPoseSrv::Request::SharedPtr request,
-            ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response)
-    {
-        RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] Incoming request");
-
-        if (_loaded_workspaces.find(request->workspace_id) != _loaded_workspaces.end())
-        {
-            RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] found available workspace (%s) in component_state_monitor", request->workspace_id.c_str());
-            auto workspace = _loaded_workspaces.at(request->workspace_id);
-            
-            if (_loaded_components.find(request->frame_id) != _loaded_components.end())
-            {
-                RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] found frame (%s) in component_state_monitor", request->frame_id.c_str());
-                auto component = _loaded_components.at(request->frame_id);
-                auto component_frame_name = component.getFrameName()+"_"+request->workspace_id;
-
-                component.setFrameName(component_frame_name);
-
-                auto frame_pose = component.getPlacementPose();
-                frame_pose.position.x += workspace.position.x;
-                frame_pose.position.y += workspace.position.y;
-                frame_pose.position.z += workspace.position.z;
-
-                frame_pose.orientation.x += workspace.orientation.x;
-                frame_pose.orientation.y += workspace.orientation.y;
-                frame_pose.orientation.z += workspace.orientation.z;
-                frame_pose.orientation.w += workspace.orientation.w;
-
-                component.setPlacementPose(frame_pose);
-
-                auto grasp_poses = component.getGraspPoses();
-                for (auto& grasp_pose: grasp_poses)
-                {
-                    grasp_pose.label += "_" + request->workspace_id;
-                }
-
-                component.setGraspPoses(grasp_poses);
-                component.updateState(ComponentState::Printed);
-                component.setIsImage(true);
-
-                _imaginary_components.insert({component.getFrameName(), component});
-
-                response->success = true;
-                response->message = "successfully received a response from the service [construct_workspace_placement_pose] with workspace (" + request->workspace_id + ")";
-
-            } else {
-                RCLCPP_WARN(this->get_logger(), "[construct_workspace_placement_pose] frame with name (%s) not loaded ", request->frame_id.c_str());
-                response->success = false;
-                response->message = "frame with name (" + request->frame_id + ") not loaded";
-            }
-        } else {
-
-            RCLCPP_WARN(this->get_logger(), "[construct_workspace_placement_pose] workspaces with name (%s) not available now", request->workspace_id.c_str());
-            response->success = false;
-            response->message = "workspace with name (" + request->workspace_id + ") not available";
-        }
-
-        RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] Sending back response");
-    }
-
-    void ComponentStateMonitor::getWorkspacePlacementPose(const GetWorkspacePlacementPoseSrv::Request::SharedPtr request,
-            GetWorkspacePlacementPoseSrv::Response::SharedPtr response)
-    {
-        RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] Incoming request");
-
-        if (_loaded_workspaces.find(request->workspace_id) != _loaded_workspaces.end())
-        {
-            RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] found available workspace (%s) in component_state_monitor", request->workspace_id.c_str());
-            // auto workspace = _loaded_workspaces.at(request->workspace_id);
-            
-            if (_imaginary_components.find(request->frame_id) != _imaginary_components.end())
-            {
-                auto component = _imaginary_components.at(request->frame_id);
-                RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] found frame (%s) in component_state_monitor", request->frame_id.c_str());
-
-                auto grasp_poses = component.getGraspPoses();
-                std::vector<scene_monitor_interfaces::msg::GraspPose> result_poses;
-
-                for (auto pose: grasp_poses)
-                {
-                    geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
-                        _global_frame, pose.label, tf2::TimePointZero 
-                    );
-                    pose.grasp_pose = transformToPose(transformStamped);
-
-                    geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
-                        _global_frame, std::string("pre_"+pose.label), tf2::TimePointZero 
-                    );
-                    pose.pre_grasp_pose = transformToPose(preTransformStamped);
-
-                    result_poses.push_back(pose);
-                }
-
-                response->grasp_poses = result_poses;
-                response->success = true;
-                response->message = "successfully received a response from the service [get_workspace_placement_pose] with workspace (" + request->workspace_id + ")";
-            } else {
-                RCLCPP_WARN(this->get_logger(), "[get_workspace_placement_pose] frame with name (%s) not loaded ", request->frame_id.c_str());
-                response->success = false;
-                response->message = "frame with name (" + request->frame_id + ") not loaded";
-            }
-        } else {
-
-            RCLCPP_WARN(this->get_logger(), "[get_workspace_placement_pose] workspaces with name (%s) not available now", request->workspace_id.c_str());
-            response->success = false;
-            response->message = "workspace with name (" + request->workspace_id + ") not available";
-        }
-
-        RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] Sending back response");
-    }
-
-    void ComponentStateMonitor::getGraspPoses(const GetGraspPosesSrv::Request::SharedPtr request,
-        GetGraspPosesSrv::Response::SharedPtr response)
-    {
-        RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Incoming request");
-
-        if (_loaded_components.find(request->frame_id) != _loaded_components.end())
-        {
-            auto component = _loaded_components.at(request->frame_id);
-            auto grasp_poses = component.getGraspPoses();
-            std::vector<scene_monitor_interfaces::msg::GraspPose> result_poses;
-
-            for (auto pose: grasp_poses)
-            {
-                geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
-                    _global_frame, pose.label, tf2::TimePointZero 
-                );
-                pose.grasp_pose = transformToPose(transformStamped);
-
-                geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
-                    _global_frame, std::string("pre_"+pose.label), tf2::TimePointZero 
-                );
-                pose.pre_grasp_pose = transformToPose(preTransformStamped);
-
-                result_poses.push_back(pose);
-            }
-
-            response->grasp_poses = result_poses;
-            response->success = true;
-            response->message = "succesfully received a response from service [get_grasp_poses]";
-        } else {
-
-            RCLCPP_WARN(this->get_logger(), "[get_grasp_poses] Frame with name (%s) not loaded in component_state_monitor", request->frame_id.c_str());
-            response->success = false;
-            response->message = "could not found frame (%s) in component_state_monitor";
-        }
-
-        RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Sending back response");
-    }
-
-    void ComponentStateMonitor::getGraspPose(const GetGraspPoseSrv::Request::SharedPtr request,
-        GetGraspPoseSrv::Response::SharedPtr response)
-    {
-        RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Incoming request");
-
-        if (_loaded_components.find(request->frame_id) != _loaded_components.end())
-        {
-            auto component = _loaded_components.at(request->frame_id);
-            
-            if (component.getCurrentState() == ComponentState::Printed)
-            {
-                geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
-                    _global_frame, request->grasp_pose_name, tf2::TimePointZero 
-                );
-
-                auto grasp_pose = component.getGraspPose(request->grasp_pose_name);
-
-                grasp_pose.grasp_pose = transformToPose(transformStamped);
-
-                geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
-                    _global_frame, std::string("pre_"+grasp_pose.label), tf2::TimePointZero 
-                );
-
-                grasp_pose.pre_grasp_pose = transformToPose(preTransformStamped);
-
-                response->grasp_pose = grasp_pose;
-                response->success = true;
-                response->message = "succesfully received a response from service [get_grasp_pose]";
-            } else {
-                
-                RCLCPP_WARN(this->get_logger(), "[get_grasp_pose] Frame with name (%s) not printed", request->frame_id.c_str());
-                response->success = false;
-                response->message = "Frame not printed";
-            }
-        } else {
-
-            RCLCPP_WARN(this->get_logger(), "[get_grasp_pose] Frame with name (%s) not loaded in component_state_monitor", request->frame_id.c_str());
-            response->success = false;
-            response->message = "could not found frame in component_state_monitor";
-        }
-
-        RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Sending back response");
-    }
-
-    void ComponentStateMonitor::printComponent(const PrintPartSrv::Request::SharedPtr request,
-        PrintPartSrv::Response::SharedPtr response)
-    {
-        RCLCPP_INFO(this->get_logger(), "[print_part] Incoming request");
-
-        if (_loaded_components.find(request->frame_id) != _loaded_components.end())
-        {
-            auto component = _loaded_components.at(request->frame_id);
-
-            if (_loaded_printers.find(request->printer_id) != _loaded_printers.end())
-            {
-                auto printer_pose = _loaded_printers.at(request->printer_id);
-
-                if (_component_spawner_node.spawnComponent(component, printer_pose, _models_package))
-                {
-                    RCLCPP_INFO(this->get_logger(), "[print_part] Succesfully spawn frame (%s) at printer (%s)",
-                    request->frame_id.c_str(), request->printer_id.c_str());
-                    response->success = true;
-                    response->message = "succesfully received a response from service [print_part]";
-
-                    _loaded_components.at(request->frame_id).updateState(ComponentState::Printed);
-
-                } else {
-                    RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
-                    request->frame_id.c_str(), request->printer_id.c_str());
-                    response->success = false;
-                    response->message = "printer not available";
-                }
-            } else {
-                RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
-                request->frame_id.c_str(), request->printer_id.c_str());
-                response->success = false;
-                response->message = "could not found printer in component_state_monitor";
-            }
-        } else {
-            RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
-            request->frame_id.c_str(), request->printer_id.c_str());
-            response->success = false;
-            response->message = "could not found frame in component_state_monitor";
-        }
-
-        RCLCPP_INFO(this->get_logger(), "[print_part] Sending back response");
-    }
-
-    void ComponentStateMonitor::initializeComponents(const std::string& package_name)
-    {
-        RCLCPP_INFO(this->get_logger(), "Start read package (%s)", package_name.c_str());
-        if (package_name.empty())
-        {
-            RCLCPP_WARN(this->get_logger(), "Provide incorrect packege name for initialize component");
-            return;
-        }
-
-        std::string package_share_directory = ament_index_cpp::get_package_share_directory(package_name);
-        std::filesystem::path models_path = package_share_directory + "/models/";
-        
-        if (!std::filesystem::exists(models_path))
-        {
-            RCLCPP_WARN(this->get_logger(), "Package (%s) does not contain '/models/' directory", package_name.c_str());
-            return;
-        }
-
-        for (const auto &entry: std::filesystem::directory_iterator(models_path))
-        {
-            std::string frame_directory_name = entry.path().string();
-            std::string frame_name = frame_directory_name.substr(frame_directory_name.find_last_of("/")+1, frame_directory_name.size()-1);
-
-            std::filesystem::path json_path = entry.path().string() + "/frames.json";
-
-            Component component(frame_name);
-
-            if (!std::filesystem::exists(json_path))
-            {
-                RCLCPP_WARN(this->get_logger(), "Package (%s) with frame (%s) does not contain 'frames.json' file", package_name.c_str(), frame_name.c_str());
-                RCLCPP_INFO(this->get_logger(), "Initialize empty component with frame name (%s)", frame_name.c_str());
-
-                component.setFrameName(frame_name);
-            } else {
-
-                RCLCPP_INFO(this->get_logger(), "Start read frame metadata (%s/frames.json)", frame_name.c_str());
-
-                std::ifstream json_file(json_path);
-                auto input = json::parse(json_file);
-
-                component.initializeFromJson(input);
-            }
-
-            component.updateState(ComponentState::Initialized);
-            _loaded_components.insert({component.getFrameName(), component});
-
-            RCLCPP_INFO(this->get_logger(), "Successfully initialize component (%s)", component.getFrameName().c_str());
-        }
-    }
-} // end namespace component_state_monitor