Merge branch 'main' into 109-nix-jazzy

Dockerfile

@@ -1,46 +1,65 @@
-FROM billf1nger/ros2-humble-cuda:v12.2.2-cudnn8-ubuntu22.04-gz-nvidia
+FROM nvidia/cuda:12.6.2-cudnn-devel-ubuntu22.04
-
 ARG WSDIR=rbs_ws
-ARG config_type=sim
 
 ENV RBS_ASSEMBLY_DIR=/assembly
+ENV DEBIAN_FRONTEND=noninteractive
 
 # COPY /home/bill-finger/assembly /assembly
 ENV IGN_GAZEBO_RESOURCE_PATH=/${WSDIR}/install/rbs_simulation/share/rbs_simulation/
 
-RUN apt update && apt install -y \ 
+RUN apt-get update && apt-get upgrade -y && apt-get install -y \ 
     git \
+    software-properties-common \
     python3-pip \ 
     lsb-release \
-    wget \
+    curl \
-    gnupg
+    wget
 
-RUN pip install vcstool
 
-WORKDIR /libs
+# WORKDIR /libs
-RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\
+# RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\
-    tar -xf v3.11.3.tar.gz &&\
+#     tar -xf v3.11.3.tar.gz &&\
-    cd json-3.11.3 &&\
+#     cd json-3.11.3 &&\
-    mkdir build &&\
+#     mkdir build &&\
-    cd build &&\
+#     cd build &&\
-    cmake .. &&\
+#     cmake .. &&\
-    make &&\
+#     make &&\
-    make install
+#     make install
 
-RUN git clone https://gitlab.com/robossembler/forks/megapose6d.git &&\
+RUN add-apt-repository universe
-    cd megapose6d &&\
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
-    pip install bokeh joblib pin torch transforms3d webdataset omegaconf tqdm &&\
+RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
-    pip install -e .
+
-RUN git clone https://github.com/thodan/bop_toolkit &&\
+
-    cd bop_toolkit &&\
+RUN apt-get update && apt-get upgrade && apt-get install -y ros-humble-ros-base
-    pip install -e .
+
+# RUN git clone https://gitlab.com/robossembler/forks/megapose6d.git &&\
+#     cd megapose6d &&\
+#     pip install bokeh joblib pin torch transforms3d webdataset omegaconf tqdm &&\
+#     pip install -e .
+# RUN git clone https://github.com/thodan/bop_toolkit &&\
+#     cd bop_toolkit &&\
+#     pip install -e .
 
 WORKDIR /${WSDIR}
 
 COPY . src/robossembler-ros2/
 
-RUN vcs import src/. < src/robossembler-ros2/rbs.${config_type}.repos
+RUN pip install vcstool uv
-RUN apt update && rosdep update && \
+
+# Install framework and dependencies
+RUN vcs import src/. < src/robossembler-ros2/repos/all-deps.repos
+RUN uv pip install --system -r src/robossembler-ros2/repos/requirements.txt
+RUN apt-get update && rosdep update && \
     rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
 RUN . /opt/ros/humble/setup.sh && \
     colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1
+
+
+WORKDIR /${RBS_ASSEMBLY_DIR}
+RUN curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | bash
+RUN apt-get install git-lfs
+RUN git clone https://github.com/solid-sinusoid/rbs_assets_library.git 
+RUN cd rbs_assets_library && git lfs pull && pip install -e .
+
+WORKDIR /${WSDIR}

doc/en/add_new_robot.md

@@ -0,0 +1,110 @@
+# Instructions for Adding a New Robot to the Robossembler ROS 2 Framework
+
+First, you need to download the robot package containing `xacro` or `urdf` files, as well as geometry files in formats such as `.stl`, `.dae`, `.obj`, etc.
+
+Before starting, it is important to understand the basics of the [xacro](https://github.com/ros/xacro/wiki) format. This format allows you to reuse existing fragments of a robot's URDF description, making it easier to create and modify descriptions.
+
+### Steps for Adding a New Robot:
+
+1. **Install the Robot Package**  
+   After installing the robot package, create a file named `xacro_args.yaml` in the `{description_package}/config/` directory. This file should specify the arguments needed to convert the `xacro` file into a `urdf`.
+
+2. **Configure the Launch File**  
+   Edit the file [`rbs_bringup.launch.py`](../../rbs_bringup/launch/rbs_bringup.launch.py) to define the parameters required to launch the robot.
+
+   Example of a standard implementation:
+   ```python
+   main_script = IncludeLaunchDescription(
+       PythonLaunchDescriptionSource(
+           [
+               PathJoinSubstitution(
+                   [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"]
+               )
+           ]
+       ),
+       launch_arguments={
+           "with_gripper": "true",
+           "gripper_name": "rbs_gripper",
+           "robot_type": "rbs_arm",
+           "description_package": "rbs_arm",
+           "description_file": "rbs_arm_modular.xacro",
+           "robot_name": "rbs_arm",
+           "use_moveit": "false",
+           "moveit_config_package": "rbs_arm",
+           "moveit_config_file": "rbs_arm.srdf.xacro",
+           "use_sim_time": "true",
+           "hardware": "gazebo",
+           "use_controllers": "true",
+           "scene_config_file": "",
+           "base_link_name": "base_link",
+           "ee_link_name": "gripper_grasp_point",
+       }.items(),
+   )
+   ```
+   This configuration launches another file with specific arguments. Below is a description of each argument to help you decide whether it is needed.
+
+### Parameter Descriptions:
+
+- **`with_gripper`**  
+  Indicates whether the robot has a gripper. If set to `true`, the `gripper_controller` will be configured and launched.
+
+- **`gripper_name`**  
+  Used as a keyword to identify links and joints related to the gripper. It is also applied in `xacro` arguments.
+
+- **`robot_type`**  
+  Specifies a group of robots of the same type, allowing you to semantically group robots with different names but similar designs.
+
+- **`description_package`**  
+  The package containing the robot's URDF description. This parameter is mandatory and is used to locate the URDF file and controller configuration files.
+
+- **`description_file`**  
+  The name of the description file, which should be located in `{description_package}/urdf/`.
+
+- **`robot_name`**  
+  A unique name for the robot to distinguish it from others in the scene.
+
+- **`use_moveit`**  
+  Indicates whether [MoveIt 2](https://moveit.picknik.ai/humble/index.html) should be used. While it is not required for basic movements, it is recommended when working with obstacles.
+
+- **`moveit_config_package`**  
+  The name of the MoveIt 2 configuration package generated based on the `{description_package}`. This parameter is required if you plan to use MoveIt 2.
+
+- **`moveit_config_file`**  
+  The MoveIt 2 launch file located in `{moveit_config_package}/launch/`.
+
+- **`use_sim_time`**  
+  A mandatory parameter for simulation. It ensures that time is synchronized with the simulator.
+
+- **`hardware`**  
+  Specifies the interface to be used for controlling the robot. For example, `gazebo`. This parameter is primarily used in `xacro` files.
+
+- **`use_controllers`**  
+  Indicates whether to use standard controllers. If set to `false`, the robot will not be able to move. This parameter controls the execution of the [control.launch.py](../../rbs_bringup/launch/control.launch.py) file. You can write a custom implementation instead of using this flag.
+
+- **`scene_config_file`**  
+  A YAML file that defines the scene configuration. A default example is available [here](../../env_manager/rbs_runtime/config/default-scene-config.yaml). Ensure that the degrees of freedom of your robot match the configuration file.
+
+- **`base_link_name`**  
+  The name of the robot's base link, which defines where the robot begins. This parameter is important for skill configuration. Review your robot's URDF to set this correctly.
+
+- **`ee_link_name`**  
+  The name of the end-effector link, which defines where the robot ends. This parameter is also important for skill configuration and should be set based on the URDF.
+
+- **`control_space`**  
+  Specifies the space in which the robot will be controlled.  
+  Possible values:  
+  - `task` — control in task space (e.g., using positions and orientations in the workspace).  
+  - `joint` — control in joint space (e.g., using angular values for each robot joint).  
+  Default value: `task`.
+
+- **`control_strategy`**  
+  Specifies the control strategy for the robot.  
+  Possible values:  
+  - `position` — position control, where desired positions are set for joints or the workspace target point.  
+  - `velocity` — velocity control, where desired movement speeds are set.  
+  - `effort` — effort control, where torques or forces applied to the joints are specified.  
+  Default value: `position`.
+
+- **`interactive`**  
+  Specifies whether to run the `motion_control_handle` controller.  
+  Default value: `true`.  

doc/en/index.md

@@ -0,0 +1,18 @@
+Robossembler ROS 2 Packages
+
+- **`env_manager`** - virtual environment manager:
+    - **`env_manager`** - manages objects in Gazebo simulation scenes.
+    - **`env_manager_interfaces`** - ROS 2 interfaces for configuring, loading, activating, and unloading environments.
+    - **`rbs_gym`** - reinforcement learning module: training management, simulation environment creation, action and observation space handling, utilities.
+    - **`rbs_runtime`** - runs the main runtime using `env_manager`.
+
+- **`rbs_bringup`** - launch scenarios: simulation, real robot, multi-robot configurations.
+- **`rbs_bt_executor`** - executes behavior trees with Behavior Tree CPP v4.
+- **`rbs_interface`** - interface linking behavior trees with skill servers (recommended to merge with `rbs_bt_executor`).
+- **`rbs_perception`** - machine vision module with multiple implementations.
+- **`rbs_simulation`** - simulation models (recommended to merge with `env_manager` or `rbs_gym`).
+- **`rbs_skill_interfaces`** - common interfaces for interacting with skill servers and behavior trees.
+- **`rbs_skill_servers`** - packages for skill servers (recommended to replace with individual packages for each server).
+- **`rbs_task_planner`** - task planner based on PDDL.
+- **`rbs_utils`** - utilities for working with configurations containing grasp positions.
+- **`rbss_objectdetection`** - skill server for object detection using YOLOv8.

doc/index.md

@@ -1,20 +0,0 @@
-Robossembler ROS 2 Packages
-
-- `env_manager` - менеджер переключения виртуальных сред
-    - `env_interface` - базовый класс для создания конкретной среды на базе ROS 2 LifeCycle Node 
-    - `env_manager` - основной пакет менеджера переключения виртуальных сред
-    - `env_manager_interfaces` - интерфейсы ROS 2 для env_manager, описывают сообщения о состоянии среды, сервисы по конфигурации/загрузке/включению/выгрузке среды
-    - `gz_environment` - конкретный экземпляр `env_interface` для симулятора Gazebo
-    - `planning_scene_manager` - управление сценой планирования для MoveIt (среда для MoveIt)
-    - `rbs_gym` - модуль обучения с подреплением: управление процесом обучения, формирование сред симуляции, управление пространствами действий(actions) и восприятия(observation), управление задачами, утилиты
-
-- `rbs_bringup` - пакет для запуска разных launch сценариев: симуляция, реальный робот, разные конфигурации оборудования (multi-robot)
-- `rbs_bt_executor` - модуль запуска деревьев поведения на Behavior Tree CPP v4
-- `rbs_interface` - пакет для связывания деревьев со скилл-серверами (объединить с - `rbs_bt_executor`)
-- `rbs_perception` - модуль машинного восприятия, где реализованы разные версии
-- `rbs_simulation` - модели для симуляции (к удалить или объединить с env_manager/rbs_gym)
-- `rbs_skill_interfaces` - общеиспользуемые (common) интерфейсы для взаимодействия с Серверами Навыков и Деревом поведения (специфичные интерфейсы размещаются в пакетах Серверов Навыков)
-- `rbs_skill_servers` - пакеты Серверов Навыков (упразднить, для каждого Сервера Навыков свой пакет)
-- `rbs_task_planner` - планировщик задач на базе PDDL
-- `rbs_utils` - работа с конфигом, содержащим позиции захвата для деталей
-- `rbss_objectdetection` - Сервер Навыка обнаружения объектов с помощью YOLOv8

doc/ru/add_new_robot.md

@@ -0,0 +1,112 @@
+# Инструкция по добавлению нового робота в фреймворк Robossembler ROS 2
+
+Прежде всего необходимо скачать пакет робота, содержащий файлы `xacro` или `urdf`, а также файлы геометрии робота в формате `.stl`, `.dae`, `.obj` и других.
+
+Перед началом работы важно ознакомиться с основными аспектами формата [xacro](https://github.com/ros/xacro/wiki). Этот формат позволяет переиспользовать существующие фрагменты URDF-описания робота, что упрощает создание и модификацию описания.
+
+### Шаги по добавлению нового робота:
+
+1. **Установка пакета робота**  
+   После установки пакета робота создайте файл `xacro_args.yaml` в директории `{description_package}/config/`. В этом файле необходимо указать аргументы для преобразования xacro-файла в URDF.
+
+2. **Настройка запуска**  
+   Отредактируйте файл [`rbs_bringup.launch.py`](../../rbs_bringup/launch/rbs_bringup.launch.py), указав параметры для запуска робота.
+
+   Пример стандартной реализации:
+   ```python
+   main_script = IncludeLaunchDescription(
+       PythonLaunchDescriptionSource(
+           [
+               PathJoinSubstitution(
+                   [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"]
+               )
+           ]
+       ),
+       launch_arguments={
+           "with_gripper": "true",
+           "gripper_name": "rbs_gripper",
+           "robot_type": "rbs_arm",
+           "description_package": "rbs_arm",
+           "description_file": "rbs_arm_modular.xacro",
+           "robot_name": "rbs_arm",
+           "use_moveit": "false",
+           "moveit_config_package": "rbs_arm",
+           "moveit_config_file": "rbs_arm.srdf.xacro",
+           "use_sim_time": "true",
+           "hardware": "gazebo",
+           "use_controllers": "true",
+           "scene_config_file": "",
+           "base_link_name": "base_link",
+           "ee_link_name": "gripper_grasp_point",
+           "control_space": "task",
+           "control_strategy": "position",
+       }.items(),
+   )
+   ```
+   Здесь выполняется запуск другого launch-файла с указанными аргументами. Ниже приводится описание каждого аргумента.
+
+### Описание параметров:
+
+- **`with_gripper`**  
+  Указывает, есть ли на роботе захватное устройство (гриппер). Если значение `true`, будет настроен и запущен `gripper_controller`.
+
+- **`gripper_name`**  
+  Используется как ключевое слово для указания линков и джоинтов, относящихся к грипперу. Также применяется в xacro-аргументах.
+
+- **`robot_type`**  
+  Обозначает группу роботов одного типа. Например, все роботы с разными именами, но одинаковой конструкцией.
+
+- **`description_package`**  
+  Пакет, содержащий описание URDF робота. Обязательный параметр. Используется для определения пути к файлу описания и конфигурации контроллеров.
+
+- **`description_file`**  
+  Имя файла описания, который должен находиться в `{description_package}/urdf/`.
+
+- **`robot_name`**  
+  Уникальное имя робота, которое позволяет отличить его от других в сцене.
+
+- **`use_moveit`**  
+  Указывает, нужно ли использовать [MoveIt 2](https://moveit.picknik.ai/humble/index.html). Для базовых перемещений MoveIt 2 не обязателен, но для работы с препятствиями рекомендуется его включить.
+
+- **`moveit_config_package`**  
+  Имя пакета конфигурации MoveIt 2, который генерируется на основе `{description_package}`. Обязательный параметр, если используется MoveIt 2.
+
+- **`moveit_config_file`**  
+  Файл запуска MoveIt 2, находящийся в `{moveit_config_package}/launch/`.
+
+- **`use_sim_time`**  
+  Обязательный параметр при работе в симуляции. Обеспечивает синхронизацию времени с симулятором.
+
+- **`hardware`**  
+  Указывает интерфейс для управления роботом. Например, `gazebo`. Используется в основном в xacro-файлах.
+
+- **`use_controllers`**  
+  Указывает, нужно ли использовать стандартные контроллеры. Если значение `false`, робот не сможет двигаться. Влияет на запуск файла [control.launch.py](../../rbs_bringup/launch/control.launch.py).
+
+- **`scene_config_file`**  
+  Файл конфигурации сцены в формате YAML. Пример можно найти [здесь](../../env_manager/rbs_runtime/config/default-scene-config.yaml). Обратите внимание на соответствие количества степеней свободы робота.
+
+- **`base_link_name`**  
+  Имя базового линка, от которого начинается робот. Важно для настройки навыков. Рекомендуется свериться с URDF.
+
+- **`ee_link_name`**  
+  Имя конечного линка (энд-эффектора), где заканчивается робот. Этот параметр также настраивается на основе URDF.
+
+- **`control_space`**  
+  Указывает, в каком пространстве робот будет управляться.  
+  Возможные значения:  
+  - `task` — управление в пространстве задач (например, с использованием позиций и ориентации в рабочем пространстве).  
+  - `joint` — управление в пространстве суставов (например, с использованием угловых значений для каждого сустава робота).  
+  Значение по умолчанию: `task`.
+
+- **`control_strategy`**  
+  Указывает стратегию управления роботом.  
+  Возможные значения:  
+  - `position` — управление положением, когда задаются желаемые позиции для суставов или рабочей точки.  
+  - `velocity` — управление скоростью, когда задаются желаемые скорости движения.  
+  - `effort` — управление усилием, когда задаются моменты или силы, прикладываемые к суставам.  
+  Значение по умолчанию: `position`.
+
+- **`interactive`**  
+  Указывает, нужно ли запускать контроллер `motion_control_handle`.  
+  Значение по умолчанию: `true`.  

doc/ru/index.md

@@ -0,0 +1,18 @@
+# Robossembler ROS 2 Packages
+
+- **`env_manager`** - менеджер виртуальных сред:
+    - **`env_manager`** - управление объектами в сцене симуляции Gazebo.
+    - **`env_manager_interfaces`** - ROS 2 интерфейсы для конфигурации, загрузки, активации и выгрузки сред.
+    - **`rbs_gym`** - модуль обучения с подкреплением: управление обучением, создание симуляционных сред, управление пространствами действий и наблюдений, утилиты.
+    - **`rbs_runtime`** - запуск основного рантайма с использованием `env_manager`.
+
+- **`rbs_bringup`** - запуск сценариев: симуляция, реальный робот, многороботные конфигурации.
+- **`rbs_bt_executor`** - выполнение деревьев поведения с Behavior Tree CPP v4.
+- **`rbs_interface`** - интерфейс для связи деревьев поведения со скилл-серверами (рекомендуется объединить с `rbs_bt_executor`).
+- **`rbs_perception`** - модуль машинного зрения с различными версиями.
+- **`rbs_simulation`** - модели для симуляции (рекомендуется объединить с `env_manager` или `rbs_gym`).
+- **`rbs_skill_interfaces`** - общие интерфейсы для взаимодействия с скилл-серверами и деревьями поведения.
+- **`rbs_skill_servers`** - пакеты для скилл-серверов (рекомендуется заменить на индивидуальные пакеты для каждого сервера).
+- **`rbs_task_planner`** - планировщик задач на основе PDDL.
+- **`rbs_utils`** - утилиты для работы с конфигурациями, содержащими позиции захвата.
+- **`rbss_objectdetection`** - скилл-сервер для обнаружения объектов с YOLOv8.

doc/ru/installation.md

@@ -0,0 +1,32 @@
+# Инструкция по установке фреймворка
+
+Первым делом необходимо установить [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html). Рекомендуется минимальная установка.
+
+Вторым делом надо собрать [`ros2_control`](https://github.com/ros-controls/ros2_control) из исходников из этого [форка](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers) стоит отметить, что также существует альтернативная установка с использованием [`vsctool`](https://github.com/dirk-thomas/vcstool) который поставляется с базовыми пакетами ROS2.
+
+Если устанавливать через `vcstool` тогда необходимые пакеты будут клонированы в тоже рабочее пространство, что и сам фреймворк. Сама команда будет выглядеть так
+```sh
+vcs import . < robossembler-ros2/repos/all-deps.repos
+```
+
+Заодно можно выполнить команду для установки всех требуемых библиотек Python
+```shell
+pip insatll -r robossembler-ros2/repos/requirements.txt
+```
+
+При этом команду надо выполнять в директории `{robossembler_ws}/src/`
+
+Более четкая последовательность команд кому лень:
+```sh
+cd 
+mkdir -p robossembler-ros2/src && cd robossembler-ros2
+git clone git clone https://seed.robossembler.org/z46gtVRpXaXrGQM7Fxiqu7pLy7kip.git robossembler-ros2
+# Или если вы предпочитаете radicle
+rad clone rad:z46gtVRpXaXrGQM7Fxiqu7pLy7kip
+cd src
+vcs import . < robossembler-ros2/repos/all-deps.repos
+pip insatll -r robossembler-ros2/repos/requirements.txt
+cd ..
+rosdep install --from-paths src -y --ignore-src
+colcon build --symlink-install
+```

env_manager/env_manager/env_manager/models/configs/terrain.py

@@ -27,4 +27,7 @@ class TerrainData:
     spawn_position: tuple[float, float, float] = field(default=(0, 0, 0))
     spawn_quat_xyzw: tuple[float, float, float, float] = field(default=(0, 0, 0, 1))
     size: tuple[float, float] = field(default=(1.5, 1.5))
+    ambient: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0))
+    specular: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0))
+    diffuse: tuple[float, float, float, float] = field(default=(0.8, 0.8, 0.8, 1.0))
     model_rollouts_num: int = field(default=1)

env_manager/env_manager/env_manager/models/terrains/ground.py

@@ -20,6 +20,9 @@ class Ground(model_wrapper.ModelWrapper):
         size (tuple[float, float], optional): The size of the ground plane. Defaults to (1.5, 1.5).
         collision_thickness (float, optional): The thickness of the collision surface. Defaults to 0.05.
         friction (float, optional): The friction coefficient for the ground surface. Defaults to 5.0.
+        ambient (tuple[float, float, float, float], optional): The ambient color of the material. Defaults to (0.8, 0.8, 0.8, 1.0).
+        specular (tuple[float, float, float, float], optional): The specular color of the material. Defaults to (0.8, 0.8, 0.8, 1.0).
+        diffuse (tuple[float, float, float, float], optional): The diffuse color of the material. Defaults to (0.8, 0.8, 0.8, 1.0).
         **kwargs: Additional keyword arguments for future extensions.
 
     Raises:
@@ -35,6 +38,9 @@ class Ground(model_wrapper.ModelWrapper):
         size: tuple[float, float] = (1.5, 1.5),
         collision_thickness=0.05,
         friction: float = 5.0,
+        ambient: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
+        specular: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
+        diffuse: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0),
         **kwargs,
     ):
         # Get a unique model name
@@ -43,7 +49,7 @@ class Ground(model_wrapper.ModelWrapper):
         # Initial pose
         initial_pose = scenario_core.Pose(position, orientation)
 
-        # Create SDF string for the model
+        # Create SDF string for the model with the provided material properties
         sdf = f"""<sdf version="1.9">
             <model name="{model_name}">
                 <static>true</static>
@@ -75,9 +81,9 @@ class Ground(model_wrapper.ModelWrapper):
                             </plane>
                         </geometry>
                         <material>
-                            <ambient>0.8 0.8 0.8 1</ambient>
+                            <ambient>{' '.join(map(str, ambient))}</ambient>
-                            <diffuse>0.8 0.8 0.8 1</diffuse>
+                            <diffuse>{' '.join(map(str, diffuse))}</diffuse>
-                            <specular>0.8 0.8 0.8 1</specular>
+                            <specular>{' '.join(map(str, specular))}</specular>
                         </material>
                     </visual>
                 </link>

env_manager/env_manager/env_manager/scene/scene.py

@@ -573,6 +573,9 @@ class Scene:
             position=self.terrain.spawn_position,
             orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw),
             size=self.terrain.size,
+            ambient=self.terrain.ambient,
+            diffuse=self.terrain.diffuse,
+            specular=self.terrain.specular,
         )
 
         # Enable contact detection

env_manager/rbs_gym/launch/evaluate.launch.py

@@ -65,7 +65,7 @@ def launch_setup(context, *args, **kwargs):
     launch_simulation = LaunchConfiguration("launch_sim")
 
     initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
+        get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml'
     )
 
     single_robot_setup = IncludeLaunchDescription(
@@ -176,7 +176,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
+            default_value="controllers_gazebosim.yaml",
             description="YAML file with the controllers configuration.",
         )
     )

env_manager/rbs_gym/launch/optimize.launch.py

@@ -70,7 +70,7 @@ def launch_setup(context, *args, **kwargs):
     launch_simulation = LaunchConfiguration("launch_sim")
 
     initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
+        get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml'
     )
 
     single_robot_setup = IncludeLaunchDescription(
@@ -195,7 +195,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
+            default_value="controllers_gazebosim.yaml",
             description="YAML file with the controllers configuration.",
         )
     )

env_manager/rbs_gym/launch/test_env.launch.py

@@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs):
     launch_simulation = LaunchConfiguration("launch_sim")
 
     initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
+        get_package_share_directory("rbs_arm"), "config", "controllers.yaml"
     )
 
     xacro_file = os.path.join(
@@ -158,7 +158,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
+            default_value="controllers_gazebosim.yaml",
             description="YAML file with the controllers configuration.",
         )
     )

env_manager/rbs_gym/launch/train.launch.py

@@ -69,7 +69,7 @@ def launch_setup(context, *args, **kwargs):
     launch_simulation = LaunchConfiguration("launch_sim")
 
     initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
+        get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml'
     )
 
     single_robot_setup = IncludeLaunchDescription(
@@ -196,7 +196,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
+            default_value="controllers_gazebosim.yaml",
             description="YAML file with the controllers configuration.",
         )
     )

env_manager/rbs_gym/rbs_gym/envs/observation/octree.py

@@ -7,7 +7,7 @@ import torch
 from rclpy.node import Node
 from sensor_msgs.msg import PointCloud2
 
-from rbs_gym.envs.utils import Tf2Listener, conversions
+from env_manager.utils import Tf2Listener, conversions
 
 
 class OctreeCreator:

env_manager/rbs_runtime/config/default-scene-config.yaml

@@ -1,100 +1,129 @@
 camera:
-- clip_color: !tuple
+  - clip_color: !tuple
-  - 0.01
+      - 0.01
-  - 1000.0
+      - 1000.0
-  clip_depth: !tuple
+    clip_depth: !tuple
-  - 0.05
+      - 0.05
-  - 10.0
+      - 10.0
-  enable: true
+    enable: true
-  height: 128
+    height: 128
-  horizontal_fov: 1.0471975511965976
+    horizontal_fov: 1.0471975511965976
-  image_format: R8G8B8
+    image_format: R8G8B8
-  name: robot_camera
+    name: robot_camera
-  noise_mean: null
+    noise_mean: null
-  noise_stddev: null
+    noise_stddev: null
-  publish_color: true
+    publish_color: true
-  publish_depth: true
+    publish_depth: true
-  publish_points: true
+    publish_points: true
-  random_pose_focal_point_z_offset: 0.0
+    random_pose_focal_point_z_offset: 0.0
-  random_pose_mode: orbit
+    random_pose_mode: orbit
-  random_pose_orbit_distance: 1.0
+    random_pose_orbit_distance: 1.0
-  random_pose_orbit_height_range: !tuple
+    random_pose_orbit_height_range: !tuple
-  - 0.1
+      - 0.1
-  - 0.7
+      - 0.7
-  random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414
+    random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414
-  random_pose_rollout_counter: 0.0
+    random_pose_rollout_counter: 0.0
-  random_pose_rollouts_num: 1
+    random_pose_rollouts_num: 1
-  random_pose_select_position_options: []
+    random_pose_select_position_options: []
-  relative_to: base_link
+    relative_to: base_link
-  spawn_position: !tuple
+    spawn_position: !tuple
-  - 0.0
+      - 0.0
-  - 0.0
+      - 0.0
-  - 1.0
+      - 1.0
-  spawn_quat_xyzw: !tuple
+    spawn_quat_xyzw: !tuple
-  - 0.0
+      - 0.0
-  - 0.70710678118
+      - 0.70710678118
-  - 0.0
+      - 0.0
-  - 0.70710678118
+      - 0.70710678118
-  type: rgbd_camera
+    type: rgbd_camera
-  update_rate: 10
+    update_rate: 10
-  vertical_fov: 1.0471975511965976
+    vertical_fov: 1.0471975511965976
-  width: 128
+    width: 128
 gravity: !tuple
-- 0.0
+  - 0.0
-- 0.0
+  - 0.0
-- -9.80665
+  - -9.80665
 gravity_std: !tuple
-- 0.0
+  - 0.0
-- 0.0
+  - 0.0
-- 0.0232
+  - 0.0232
 light:
   color: !tuple
-  - 1.0
+    - 1.0
-  - 1.0
+    - 1.0
-  - 1.0
+    - 1.0
-  - 1.0
+    - 1.0
   direction: !tuple
-  - 0.5
+    - 0.5
-  - -0.25
+    - -0.25
-  - -0.75
+    - -0.75
   distance: 1000.0
   model_rollouts_num: 1
   radius: 25.0
   random_minmax_elevation: !tuple
-  - -0.15
+    - -0.15
-  - -0.65
+    - -0.65
   type: sun
   visual: true
 objects:
-- color: null
+  - color: null
-  name: bishop
+    name: hole
-  orientation: !tuple
+    orientation: !tuple
-  - 1.0
+      - 1.0
-  - 0.0
+      - 0.0
-  - 0.0
+      - 0.0
-  - 0.0
+      - 0.0
-  position: !tuple
+    position: !tuple
-  - 0.0
+      - 0.1
-  - 1.0
+      - 0.3
-  - 0.3
+      - 0.1
-  randomize:
+    randomize:
-    count: 0
+      count: 0
-    models_rollouts_num: 0
+      models_rollouts_num: 0
-    random_color: false
+      random_color: false
-    random_model: false
+      random_model: false
-    random_orientation: false
+      random_orientation: false
-    random_pose: false
+      random_pose: false
-    random_position: false
+      random_position: false
-    random_spawn_position_segments: []
+      random_spawn_position_segments: []
-    random_spawn_position_update_workspace_centre: false
+      random_spawn_position_update_workspace_centre: false
-    random_spawn_volume: !tuple
+      random_spawn_volume: !tuple
-    - 0.5
+        - 0.5
-    - 0.5
+        - 0.5
-    - 0.5
+        - 0.5
-  relative_to: world
+    relative_to: world
-  static: false
+    static: false
-  texture: []
+    texture: []
-  type: 'model'
+    type: "model"
+  - color: null
+    name: peg
+    orientation: !tuple
+      - 1.0
+      - 0.0
+      - 0.0
+      - 0.0
+    position: !tuple
+      - 0.0
+      - 0.3
+      - 0.1
+    randomize:
+      count: 0
+      models_rollouts_num: 0
+      random_color: false
+      random_model: false
+      random_orientation: false
+      random_pose: false
+      random_position: false
+      random_spawn_position_segments: []
+      random_spawn_position_update_workspace_centre: false
+      random_spawn_volume: !tuple
+        - 0.5
+        - 0.5
+        - 0.5
+    relative_to: world
+    static: false
+    texture: []
+    type: "model"
 physics_rollouts_num: 0
 plugins:
   fts_broadcaster: false
@@ -104,13 +133,13 @@ plugins:
 robot:
   gripper_joint_positions: 0.0
   joint_positions:
-  - 0.0
+    - -1.57
-  - 0.0
+    - 0.5
-  - 0.0
+    - 3.14159
-  - 0.0
+    - 1.5
-  - 0.0
+    - 0.0
-  - 0.0
+    - 1.4
-  - 0.0
+    - 0.0
   name: rbs_arm
   randomizer:
     joint_positions: false
@@ -120,33 +149,33 @@ robot:
     joint_positions_std: 0.1
     pose: false
     spawn_volume: !tuple
-    - 1.0
+      - 1.0
-    - 1.0
+      - 1.0
-    - 0.0
+      - 0.0
   spawn_position: !tuple
-  - 0.0
+    - 0.0
-  - 0.0
+    - 0.0
-  - 0.0
+    - 0.0
   spawn_quat_xyzw: !tuple
-  - 0.0
+    - 0.0
-  - 0.0
+    - 0.0
-  - 0.0
+    - 0.0
-  - 1.0
+    - 1.0
-  urdf_string: ''
+  urdf_string: ""
   with_gripper: true
 terrain:
   model_rollouts_num: 1
   name: ground
   size: !tuple
-  - 1.5
+    - 1.5
-  - 1.5
+    - 1.5
   spawn_position: !tuple
-  - 0
+    - 0
-  - 0
+    - 0
-  - 0
+    - 0
   spawn_quat_xyzw: !tuple
-  - 0
+    - 0
-  - 0
+    - 0
-  - 0
+    - 0
-  - 1
+    - 1
   type: flat

env_manager/rbs_runtime/launch/runtime.launch.py

@@ -1,5 +1,6 @@
 import os
 
+from launch.launch_introspector import indent
 import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -14,55 +15,115 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
+from robot_builder.parser.urdf import URDF_parser
+from robot_builder.external.ros2_control import ControllerManager
+import yaml
+
 
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     robot_type = LaunchConfiguration("robot_type")
     # General arguments
     with_gripper_condition = LaunchConfiguration("with_gripper")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
+    use_moveit = LaunchConfiguration("use_moveit")
-    start_joint_controller = LaunchConfiguration("start_joint_controller")
-    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
-    launch_simulation = LaunchConfiguration("launch_sim")
-    launch_moveit = LaunchConfiguration("launch_moveit")
-    launch_task_planner = LaunchConfiguration("launch_task_planner")
-    launch_perception = LaunchConfiguration("launch_perception")
     moveit_config_package = LaunchConfiguration("moveit_config_package")
     moveit_config_file = LaunchConfiguration("moveit_config_file")
     use_sim_time = LaunchConfiguration("use_sim_time")
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
+    scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
-    hardware = LaunchConfiguration("hardware")
-    env_manager = LaunchConfiguration("env_manager")
-    gazebo_gui = LaunchConfiguration("gazebo_gui")
-    gripper_name = LaunchConfiguration("gripper_name")
 
-    initial_joint_controllers_file_path = os.path.join(
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
-        get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
+
+    control_space = LaunchConfiguration("control_space").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+
+    interactive = LaunchConfiguration("interactive").perform(context)
+
+    if not scene_config_file == "":
+        config_file = {"config_file": scene_config_file}
+    else:
+        config_file = {}
+
+    description_package_abs_path = get_package_share_directory(
+        description_package.perform(context)
+    )
+
+    simulation_controllers = os.path.join(
+        description_package_abs_path, "config", "controllers.yaml"
     )
 
     xacro_file = os.path.join(
-        get_package_share_directory(description_package.perform(context)),
+        description_package_abs_path,
         "urdf",
         description_file.perform(context),
     )
 
-    robot_description_doc = xacro.process_file(
+    xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
-        xacro_file,
+
-        mappings={
+
-            "gripper_name": gripper_name.perform(context),
+    # TODO: hide this to another place
-            "hardware": hardware.perform(context),
+    # Load xacro_args
-            "simulation_controllers": initial_joint_controllers_file_path,
+    def param_constructor(loader, node, local_vars):
-            "namespace": "",
+        value = loader.construct_scalar(node)
-        },
+        return LaunchConfiguration(value).perform(
-    )
+            local_vars.get("context", "Launch context if not defined")
+        )
+
+    def variable_constructor(loader, node, local_vars):
+        value = loader.construct_scalar(node)
+        return local_vars.get(value, f"Variable '{value}' not found")
+
+    def load_xacro_args(yaml_file, local_vars):
+        # Get valut from ros2 argument
+        yaml.add_constructor(
+            "!param", lambda loader, node: param_constructor(loader, node, local_vars)
+        )
+
+        # Get value from local variable in this code
+        # The local variable should be initialized before the loader was called
+        yaml.add_constructor(
+            "!variable",
+            lambda loader, node: variable_constructor(loader, node, local_vars),
+        )
+
+        with open(yaml_file, "r") as file:
+            return yaml.load(file, Loader=yaml.FullLoader)
+
+    mappings_data = load_xacro_args(xacro_config_file, locals())
+
+    robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
+
+    robot_description_semantic_content = ""
+
+    if use_moveit.perform(context) == "true":
+        srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
+        srdf_file = os.path.join(
+            get_package_share_directory(moveit_config_package.perform(context)),
+            "srdf",
+            moveit_config_file.perform(context),
+        )
+        srdf_mappings = load_xacro_args(srdf_config_file, locals())
+        robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
+        robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
+        control_space = "joint"
+        control_strategy = "position"
+        interactive = "false"
+
 
     robot_description_content = robot_description_doc.toprettyxml(indent="  ")
     robot_description = {"robot_description": robot_description_content}
 
-    single_robot_setup = IncludeLaunchDescription(
+    # Parse robot and configure controller's file for ControllerManager
+    robot = URDF_parser.load_string(
+        robot_description_content, ee_link_name=ee_link_name
+    )
+    ControllerManager.save_to_yaml(
+        robot, description_package_abs_path, "controllers.yaml"
+    )
+
+    rbs_robot_setup = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
                 PathJoinSubstitution(
@@ -71,37 +132,31 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments={
-            "env_manager": env_manager,
             "with_gripper": with_gripper_condition,
-            "gripper_name": gripper_name,
+            "controllers_file": simulation_controllers,
-            "controllers_file": initial_joint_controllers_file_path,
             "robot_type": robot_type,
-            # "controllers_file": controller_paramfile,
-            "cartesian_controllers": cartesian_controllers,
             "description_package": description_package,
             "description_file": description_file,
             "robot_name": robot_type,
-            "start_joint_controller": start_joint_controller,
+            "use_moveit": use_moveit,
-            "initial_joint_controller": initial_joint_controller,
-            "launch_simulation": launch_simulation,
-            "launch_moveit": launch_moveit,
-            "launch_task_planner": launch_task_planner,
-            "launch_perception": launch_perception,
             "moveit_config_package": moveit_config_package,
             "moveit_config_file": moveit_config_file,
             "use_sim_time": use_sim_time,
-            "sim_gazebo": sim_gazebo,
+            "use_controllers": "true",
-            "hardware": hardware,
-            "launch_controllers": "true",
-            "gazebo_gui": gazebo_gui,
             "robot_description": robot_description_content,
+            "robot_description_semantic": robot_description_semantic_content,
+            "base_link_name": base_link_name,
+            "ee_link_name": ee_link_name,
+            "control_space": control_space,
+            "control_strategy": control_strategy,
+            "interactive_control": interactive,
         }.items(),
     )
 
     rbs_runtime = Node(
         package="rbs_runtime",
         executable="runtime",
-        parameters=[robot_description, {"use_sim_time": True}],
+        parameters=[robot_description, config_file, {"use_sim_time": True}],
     )
 
     clock_bridge = Node(
@@ -111,9 +166,13 @@ def launch_setup(context, *args, **kwargs):
         output="screen",
     )
 
-    delay_robot_control_stack = TimerAction(period=10.0, actions=[single_robot_setup])
+    delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup])
 
-    nodes_to_start = [rbs_runtime, clock_bridge, delay_robot_control_stack]
+    nodes_to_start = [
+        rbs_runtime,
+        clock_bridge,
+        delay_robot_control_stack
+    ]
     return nodes_to_start
 
 
@@ -123,18 +182,20 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "robot_type",
             description="Type of robot by name",
-            choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            choices=[
+                "rbs_arm",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
             default_value="rbs_arm",
         )
     )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_file",
-            default_value="rbs_arm0_controllers.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_package",
@@ -157,20 +218,6 @@ def generate_launch_description():
             description="Name for robot, used to apply namespace for specific robot in multirobot setup",
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "start_joint_controller",
-            default_value="false",
-            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",
@@ -194,14 +241,6 @@ def generate_launch_description():
             This is needed for the trajectory planing in simulation.",
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "gripper_name",
-            default_value="rbs_gripper",
-            choices=["rbs_gripper", ""],
-            description="choose gripper by name (leave empty if hasn't)",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "with_gripper", default_value="true", description="With gripper or not?"
@@ -209,25 +248,7 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "sim_gazebo", default_value="true", description="Gazebo Simulation"
+            "use_moveit", default_value="false", description="Launch moveit?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "env_manager", default_value="true", description="Launch env_manager?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_sim",
-            default_value="true",
-            description="Launch simulator (Gazebo)?\
-                              Most general arg",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_moveit", default_value="false", description="Launch moveit?"
         )
     )
     declared_arguments.append(
@@ -237,39 +258,55 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_task_planner",
+            "use_controllers",
-            default_value="false",
-            description="Launch task_planner?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "cartesian_controllers",
-            default_value="true",
-            description="Load cartesian\
-                              controllers?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "hardware",
-            choices=["gazebo", "mock"],
-            default_value="gazebo",
-            description="Choose your harware_interface",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_controllers",
             default_value="true",
             description="Launch controllers?",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "gazebo_gui", default_value="true", description="Launch gazebo with gui?"
+            "scene_config_file",
+            default_value="",
+            description="Path to a scene configuration file",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ee_link_name",
+            default_value="",
+            description="End effector name of robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "base_link_name",
+            default_value="",
+            description="Base link name if robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "interactive",
+            default_value="true",
+            description="Wheter to run the motion_control_handle controller",
+        ),
+    )
 
     return LaunchDescription(
         declared_arguments + [OpaqueFunction(function=launch_setup)]

env_manager/rbs_runtime/rbs_runtime/__init__.py

@@ -4,6 +4,24 @@ import yaml
 from dacite import from_dict
 from env_manager.models.configs import SceneData
 
+from typing import Dict, Any
+from env_manager.models.configs import (
+    BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData
+)
+
+def object_factory(obj_data: Dict[str, Any]) -> ObjectData:
+    obj_type = obj_data.get('type', '')
+    
+    if obj_type == 'box':
+        return BoxObjectData(**obj_data)
+    elif obj_type == 'cylinder':
+        return CylinderObjectData(**obj_data)
+    elif obj_type == 'mesh':
+        return MeshData(**obj_data)
+    elif obj_type == 'model':
+        return ModelData(**obj_data)
+    else:
+        return ObjectData(**obj_data)
 
 def scene_config_loader(file: str | Path) -> SceneData:
     def tuple_constructor(loader, node):
@@ -15,4 +33,8 @@ def scene_config_loader(file: str | Path) -> SceneData:
 
     scene_data = from_dict(data_class=SceneData, data=scene_cfg)
 
+    scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])]
+
+    print(scene_data)
+
     return scene_data

rbs_bringup/launch/control.launch.py

@@ -0,0 +1,87 @@
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+
+# from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+
+
+def create_spawner(controller_name, namespace, condition=None, inactive=False):
+    args = [controller_name, "-c", f"{namespace}/controller_manager"]
+    if inactive:
+        args.append("--inactive")
+    return Node(
+        package="controller_manager",
+        executable="spawner",
+        namespace=namespace,
+        arguments=args,
+        condition=condition,
+    )
+
+
+def launch_setup(context, *args, **kwargs):
+    namespace = LaunchConfiguration("namespace").perform(context)
+    control_strategy = LaunchConfiguration("control_strategy").perform(context)
+    control_space = LaunchConfiguration("control_space").perform(context)
+    has_gripper = LaunchConfiguration("has_gripper").perform(context)
+    interactive_control = LaunchConfiguration("interactive_control").perform(context)
+
+    controllers_config = {
+        "joint_state_broadcaster": True,
+        "gripper_controller": has_gripper == "true",
+        "joint_trajectory_controller": control_strategy == "position"
+        and control_space == "joint",
+        "cartesian_motion_controller": control_strategy == "position"
+        and control_space == "task",
+        "cartesian_force_controller": control_strategy == "effort"
+        and control_space == "task",
+        "joint_effort_controller": control_strategy == "effort"
+        and control_space == "joint",
+        "force_torque_sensor_broadcaster": control_strategy == "effort"
+        and control_space == "task",
+        "motion_control_handle": interactive_control == "true"
+        and control_space == "task"
+        and control_strategy == "position",
+    }
+
+    nodes_to_start = []
+    for controller_name, should_start in controllers_config.items():
+        if should_start:
+            nodes_to_start.append(create_spawner(controller_name, namespace))
+
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = [
+        DeclareLaunchArgument(
+            "namespace", default_value="", description="A robot's namespace"
+        ),
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Control type: 'task' for Cartesian, 'joint' for joint space control.",
+        ),
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Control strategy: 'position', 'velocity', or 'effort'.",
+        ),
+        DeclareLaunchArgument(
+            "has_gripper",
+            default_value="true",
+            description="Whether to activate the gripper_controller.",
+        ),
+
+        DeclareLaunchArgument(
+            "interactive_control",
+            default_value="true",
+            description="Wheter to run the motion_control_handle controller",
+        ),
+    ]
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

rbs_bringup/launch/launch_env.launch.py

@@ -1,254 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    IncludeLaunchDescription,
-    OpaqueFunction
-)
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
-import os
-from ament_index_python.packages import get_package_share_directory
-from nav2_common.launch import RewrittenYaml
-
-def launch_setup(context, *args, **kwargs):
-    # Initialize Arguments
-    robot_type = LaunchConfiguration("robot_type")
-    # General arguments
-    with_gripper_condition = LaunchConfiguration("with_gripper")
-    controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
-    description_package = LaunchConfiguration("description_package")
-    description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
-    start_joint_controller = LaunchConfiguration("start_joint_controller")
-    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
-    launch_simulation = LaunchConfiguration("launch_sim")
-    launch_moveit = LaunchConfiguration("launch_moveit")
-    launch_task_planner = LaunchConfiguration("launch_task_planner")
-    launch_perception = LaunchConfiguration("launch_perception")
-    moveit_config_package = LaunchConfiguration("moveit_config_package")
-    moveit_config_file = LaunchConfiguration("moveit_config_file")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
-    hardware = LaunchConfiguration("hardware")
-    env_manager = LaunchConfiguration("env_manager")
-    launch_controllers = LaunchConfiguration("launch_controllers")
-    gazebo_gui = LaunchConfiguration("gazebo_gui")
-    gripper_name = LaunchConfiguration("gripper_name")
-
-
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
-    launch_simulation = LaunchConfiguration("launch_sim")
-
-    configured_params = RewrittenYaml(
-        source_file=os.path.join(
-            get_package_share_directory(
-                description_package.perform(context)), 
-            "config", 
-            controllers_file.perform(context)),
-        root_key=robot_name.perform(context),
-        param_rewrites={},
-        convert_types=True,
-    )
-    initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
-    )
-    controller_paramfile = configured_params.perform(context)
-    namespace = "/" + robot_name.perform(context) 
-
-    # spawner = Node(
-    #     package="gz_enviroment_python",
-    #     executable="spawner.py",
-    #     namespace=namespace
-    # )
-
-    single_robot_setup = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            PathJoinSubstitution([
-                FindPackageShare('rbs_bringup'),
-                "launch",
-                "rbs_robot.launch.py"
-            ])
-        ]),
-        launch_arguments={
-            "env_manager": env_manager,
-            "with_gripper": with_gripper_condition,
-            "gripper_name": gripper_name,
-            "controllers_file": controllers_file,
-            "robot_type": robot_type,
-            "controllers_file": initial_joint_controllers_file_path,
-            "cartesian_controllers": cartesian_controllers,
-            "description_package": description_package,
-            "description_file": description_file,
-            "robot_name": robot_name,
-            "start_joint_controller": start_joint_controller,
-            "initial_joint_controller": initial_joint_controller,
-            "launch_simulation": launch_simulation,
-            "launch_moveit": launch_moveit,
-            "launch_task_planner": launch_task_planner,
-            "launch_perception": launch_perception,
-            "moveit_config_package": moveit_config_package,
-            "moveit_config_file": moveit_config_file,
-            "use_sim_time": use_sim_time,
-            "sim_gazebo": sim_gazebo,
-            "hardware": hardware,
-            "launch_controllers": "true",
-            "gazebo_gui": gazebo_gui,
-        }.items()
-    )
-    
-    nodes_to_start = [
-        # spawner,
-        single_robot_setup
-    ]
-    return nodes_to_start
-
-
-def generate_launch_description():
-    declared_arguments = []
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_type",
-            description="Type of robot by name",
-            choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
-            default_value="rbs_arm",
-        )
-    )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_package",
-            default_value="rbs_arm",
-            description="Description package with robot URDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom description.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_file",
-            default_value="rbs_arm_modular.xacro",
-            description="URDF/XACRO description file with the robot.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_name",
-            default_value="arm0",
-            description="Name for robot, used to apply namespace for specific robot in multirobot setup",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "start_joint_controller",
-            default_value="false",
-            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="rbs_arm",
-            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom moveit config.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "moveit_config_file",
-            default_value="rbs_arm.srdf.xacro",
-            description="MoveIt SRDF/XACRO description file with the robot.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="Make MoveIt to use simulation time.\
-            This is needed for the trajectory planing in simulation.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "gripper_name",
-            default_value="rbs_gripper",
-            choices=["rbs_gripper", ""],
-            description="choose gripper by name (leave empty if hasn't)",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("with_gripper", 
-                              default_value="true", 
-                              description="With gripper or not?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("sim_gazebo", 
-                              default_value="true", 
-                              description="Gazebo Simulation")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("env_manager", 
-                              default_value="false", 
-                              description="Launch env_manager?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_sim", 
-                              default_value="true", 
-                              description="Launch simulator (Gazebo)?\
-                              Most general arg")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_moveit", 
-                              default_value="false", 
-                              description="Launch moveit?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_perception", 
-                              default_value="false", 
-                              description="Launch perception?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_task_planner", 
-                              default_value="false", 
-                              description="Launch task_planner?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("cartesian_controllers", 
-                              default_value="true", 
-                              description="Load cartesian\
-                              controllers?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("hardware", 
-                              choices=["gazebo", "mock"],
-                              default_value="gazebo", 
-                              description="Choose your harware_interface")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_controllers", 
-                              default_value="true", 
-                              description="Launch controllers?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("gazebo_gui", 
-                              default_value="true", 
-                              description="Launch gazebo with gui?")
-    )
-
-    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

rbs_bringup/launch/multi_robot.launch.py

@@ -1,292 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-)
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
-from nav2_common.launch import RewrittenYaml
-import os
-from ament_index_python import get_package_share_directory
-from rbs_launch_utils.merged_yaml import MergedYaml
-from rbs_launch_utils.launch_common import load_yaml
-import yaml
-
-def launch_setup(context, *args, **kwargs):
-    # Initialize Arguments
-    robot_type = LaunchConfiguration("robot_type")
-    # General arguments
-    with_gripper_condition = LaunchConfiguration("with_gripper")
-    controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
-    description_package = LaunchConfiguration("description_package")
-    description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
-    start_joint_controller = LaunchConfiguration("start_joint_controller")
-    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
-    launch_simulation = LaunchConfiguration("launch_sim")
-    launch_moveit = LaunchConfiguration("launch_moveit")
-    launch_task_planner = LaunchConfiguration("launch_task_planner")
-    launch_perception = LaunchConfiguration("launch_perception")
-    moveit_config_package = LaunchConfiguration("moveit_config_package")
-    moveit_config_file = LaunchConfiguration("moveit_config_file")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
-    hardware = LaunchConfiguration("hardware")
-    env_manager = LaunchConfiguration("env_manager")
-    launch_controllers = LaunchConfiguration("launch_controllers")
-    gazebo_gui = LaunchConfiguration("gazebo_gui")
-    gripper_name = LaunchConfiguration("gripper_name")
-    robots_config_file = LaunchConfiguration("robots_config_file")
-    gazebo_world_filename = LaunchConfiguration("gazebo_world_filename")
-
-    ld = []
-    ld.append(IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            PathJoinSubstitution([
-                FindPackageShare('rbs_simulation'),
-                'launch',
-                'simulation_gazebo.launch.py'
-            ])
-        ]),
-        launch_arguments={
-            'sim_gazebo': sim_gazebo,
-            'debugger': "false",
-            'launch_env_manager': "false",
-            "gazebo_world_filename": gazebo_world_filename
-        }.items(),
-        condition=IfCondition(launch_simulation)
-    ))
-    scene_file = robots_config_file.perform(context)
-    robots = load_yaml("rbs_bringup", "config/" + scene_file)
-
-    description_package = description_package.perform(context)
-    controllers_file = controllers_file.perform(context)
-    config = MergedYaml(context, 
-                         os.path.join(get_package_share_directory(description_package), "config", controllers_file),
-                         root_keys=[i["name"] for i in robots["scene_config"]],
-                         param_rewrites={},
-                         convert_types=False,
-                         ).merge_yamls()
-    for robot in robots["scene_config"]:
-        namespace: str = "/" + robot["name"]
-        ld.append(IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([
-                PathJoinSubstitution([
-                    FindPackageShare('rbs_bringup'),
-                    "launch",
-                    "rbs_robot.launch.py"
-                ])
-            ]),
-            launch_arguments={
-                "env_manager": env_manager,
-                "with_gripper": with_gripper_condition,
-                "gripper_name": gripper_name,
-                "controllers_file": controllers_file,
-                "robot_type": robot["type"],
-                "controllers_file": config,
-                "cartesian_controllers": cartesian_controllers,
-                "description_package": description_package,
-                "description_file": description_file,
-                "robot_name": robot["name"],
-                "start_joint_controller": start_joint_controller,
-                "initial_joint_controller": initial_joint_controller,
-                "launch_simulation": launch_simulation,
-                "launch_moveit": launch_moveit,
-                "launch_task_planner": launch_task_planner,
-                "launch_perception": launch_perception,
-                "moveit_config_package": moveit_config_package,
-                "moveit_config_file": moveit_config_file,
-                "use_sim_time": use_sim_time,
-                "sim_gazebo": sim_gazebo,
-                "hardware": hardware,
-                "launch_controllers": launch_controllers,
-                "gazebo_gui": gazebo_gui,
-                "namespace": namespace,
-                "x": str(robot["pose"]["position"]["x"]),
-                "y": str(robot["pose"]["position"]["y"]),
-                "z": str(robot["pose"]["position"]["z"]),
-                "roll": str(robot["pose"]["orientation"]["x"]),
-                "pitch": str(robot["pose"]["orientation"]["y"]),
-                "yaw": str(robot["pose"]["orientation"]["z"]),
-            }.items()
-        ))
-
-        gz_spawner = Node(
-            package='ros_gz_sim',
-            executable='create',
-            arguments=[
-                '-name', robot_name,
-                # '-x', str(robot["pose"]["position"]["x"]),
-                # '-y', str(robot["pose"]["position"]["y"]),
-                # '-z', str(robot["pose"]["position"]["z"]),
-                # '-R', str(robot["pose"]["orientation"]["x"]),
-                # '-P', str(robot["pose"]["orientation"]["y"]),
-                # '-Y', str(robot["pose"]["orientation"]["z"]),
-                '-topic', namespace + '/robot_description'],
-            output='screen',
-            condition=IfCondition(sim_gazebo))
-        ld.append(gz_spawner)
-
-    return ld
-
-
-def generate_launch_description():
-    declared_arguments = []
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_type",
-            description="Type of robot by name",
-            choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
-            default_value="rbs_arm",
-        )
-    )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_file",
-            default_value="rbs_arm_controllers_gazebosim.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_package",
-            default_value="rbs_arm",
-            description="Description package with robot URDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom description.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_file",
-            default_value="rbs_arm_modular.xacro",
-            description="URDF/XACRO description file with the robot.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_name",
-            default_value="arm0",
-            description="Name for robot, used to apply namespace for specific robot in multirobot setup",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "start_joint_controller",
-            default_value="false",
-            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="rbs_arm",
-            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom moveit config.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "moveit_config_file",
-            default_value="rbs_arm.srdf.xacro",
-            description="MoveIt SRDF/XACRO description file with the robot.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="Make MoveIt to use simulation time.\
-            This is needed for the trajectory planing in simulation.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "gripper_name",
-            default_value="rbs_gripper",
-            choices=["rbs_gripper", ""],
-            description="choose gripper by name (leave empty if hasn't)",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("with_gripper", 
-                              default_value="true", 
-                              description="With gripper or not?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("sim_gazebo", 
-                              default_value="true", 
-                              description="Gazebo Simulation")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("env_manager", 
-                              default_value="false", 
-                              description="Launch env_manager?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_sim", 
-                              default_value="true", 
-                              description="Launch simulator (Gazebo)?\
-                              Most general arg")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_moveit", 
-                              default_value="false", 
-                              description="Launch moveit?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_perception", 
-                              default_value="false", 
-                              description="Launch perception?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_task_planner", 
-                              default_value="false", 
-                              description="Launch task_planner?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("cartesian_controllers", 
-                              default_value="true", 
-                              description="Load cartesian\
-                              controllers?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("hardware", 
-                              choices=["gazebo", "mock"],
-                              default_value="gazebo", 
-                              description="Choose your harware_interface")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("launch_controllers", 
-                              default_value="true", 
-                              description="Launch controllers?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("gazebo_gui", 
-                              default_value="true", 
-                              description="Launch gazebo with gui?")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("gazebo_world_filename", 
-                              default_value="asm2.sdf", 
-                              description="Filename of Gazebo world file to launch")
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument("robots_config_file", 
-                              default_value="roboclone.yaml", 
-                              description="Filename for config file with robots in scene")
-    )
-
-    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

rbs_bringup/launch/rbs_bringup.launch.py

@@ -0,0 +1,53 @@
+from launch import LaunchDescription
+from launch.actions import (
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (
+    PathJoinSubstitution,
+)
+from launch_ros.substitutions import FindPackageShare
+
+
+def launch_setup(context, *args, **kwargs):
+    main_script = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [FindPackageShare("rbs_runtime"), "launch", "runtime.launch.py"]
+                )
+            ]
+        ),
+        launch_arguments={
+            "with_gripper": "true",
+            "robot_type": "rbs_arm",
+            "description_package": "rbs_arm",
+            "description_file": "rbs_arm_modular.xacro",
+            "robot_name": "rbs_arm",
+            "use_moveit": "true",
+            "moveit_config_package": "rbs_arm",
+            "moveit_config_file": "rbs_arm.srdf.xacro",
+            "use_sim_time": "true",
+            "use_controllers": "true",
+            "scene_config_file": "",
+            "base_link_name": "base_link",
+            "ee_link_name": "gripper_grasp_point",
+            "control_space": "task",
+            "control_strategy": "position",
+            "interactive": "false"
+        }.items(),
+    )
+
+    nodes_to_start = [
+        main_script,
+    ]
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

rbs_bringup/launch/rbs_robot.launch.py

@@ -21,33 +21,15 @@ from launch_ros.substitutions import FindPackageShare
 
 
 def launch_setup(context, *args, **kwargs):
-    # Initialize Arguments
     robot_type = LaunchConfiguration("robot_type")
-    # General arguments
-    launch_rviz = LaunchConfiguration("launch_rviz")
     with_gripper_condition = LaunchConfiguration("with_gripper")
-    controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
     robot_name = LaunchConfiguration("robot_name")
-    start_joint_controller = LaunchConfiguration("start_joint_controller")
+    use_moveit = LaunchConfiguration("use_moveit")
-    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
-    launch_moveit = LaunchConfiguration("launch_moveit")
-    launch_task_planner = LaunchConfiguration("launch_task_planner")
-    launch_perception = LaunchConfiguration("launch_perception")
     moveit_config_package = LaunchConfiguration("moveit_config_package")
-    moveit_config_file = LaunchConfiguration("moveit_config_file")
     use_sim_time = LaunchConfiguration("use_sim_time")
-    hardware = LaunchConfiguration("hardware")
+    use_controllers = LaunchConfiguration("use_controllers")
-    launch_controllers = LaunchConfiguration("launch_controllers")
-    gripper_name = LaunchConfiguration("gripper_name")
-    x_pos = LaunchConfiguration("x")
-    y_pos = LaunchConfiguration("y")
-    z_pos = LaunchConfiguration("z")
-    roll = LaunchConfiguration("roll")
-    pitch = LaunchConfiguration("pitch")
-    yaw = LaunchConfiguration("yaw")
     namespace = LaunchConfiguration("namespace")
     multi_robot = LaunchConfiguration("multi_robot")
     robot_name = robot_name.perform(context)
@@ -55,83 +37,34 @@ def launch_setup(context, *args, **kwargs):
     robot_type = robot_type.perform(context)
     description_package = description_package.perform(context)
     description_file = description_file.perform(context)
-    controllers_file = controllers_file.perform(context)
     multi_robot = multi_robot.perform(context)
-    robot_description = LaunchConfiguration("robot_description")
+    robot_description_content = LaunchConfiguration("robot_description")
+    robot_description_semantic_content = LaunchConfiguration(
+        "robot_description_semantic"
+    )
+    control_space = LaunchConfiguration("control_space")
+    control_strategy = LaunchConfiguration("control_strategy")
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
 
     remappings = []
     if multi_robot == "true":
         remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
 
-    controllers_file = os.path.join(
+    robot_description = {
-        get_package_share_directory(description_package), "config", controllers_file
+        "robot_description": robot_description_content.perform(context)
-    )
-
-    if not robot_description.perform(context):
-        xacro_file = os.path.join(
-            get_package_share_directory(description_package), "urdf", description_file
-        )
-        robot_description_doc = xacro.process_file(
-            xacro_file,
-            mappings={
-                "gripper_name": gripper_name.perform(context),
-                "hardware": hardware.perform(context),
-                "simulation_controllers": controllers_file,
-                "namespace": namespace,
-                "x": x_pos.perform(context),
-                "y": y_pos.perform(context),
-                "z": z_pos.perform(context),
-                "roll": roll.perform(context),
-                "pitch": pitch.perform(context),
-                "yaw": yaw.perform(context),
-            },
-        )
-
-        robot_description_content = robot_description_doc.toprettyxml(indent="  ")
-    else:
-        robot_description_content = robot_description.perform(context)
-
-    robot_description = {"robot_description": robot_description_content}
-
-    robot_description_semantic_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare(moveit_config_package),
-                    "config/moveit",
-                    "rbs_arm.srdf.xacro",
-                ]
-            ),
-            " ",
-            "name:=",
-            robot_type,
-            " ",
-            "with_gripper:=",
-            with_gripper_condition,
-            " ",
-            "gripper_name:=",
-            gripper_name,
-            " ",
-        ]
-    )
-
-    robot_description_semantic = {
-        "robot_description_semantic": robot_description_semantic_content
     }
-    robot_description_kinematics = PathJoinSubstitution(
+
-        [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
+    robot_description_kinematics_filepath = os.path.join(
+        get_package_share_directory(moveit_config_package.perform(context)),
+        "config",
+        "kinematics.yaml",
     )
 
-    # kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
+    # robot_description_kinematics = {
+    #     "robot_description_kinematics": robot_description_kinematics
+    # }
 
-    robot_description_kinematics = {
-        "robot_description_kinematics": robot_description_kinematics
-    }
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
-    )
     robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
@@ -141,32 +74,12 @@ def launch_setup(context, *args, **kwargs):
         parameters=[{"use_sim_time": use_sim_time}, robot_description],
     )
 
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
-    )
-
-    rviz = Node(
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        output="log",
-        namespace=namespace,
-        arguments=["-d", rviz_config_file],
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ],
-        condition=IfCondition(launch_rviz),
-    )
-
     control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
                 PathJoinSubstitution(
                     [
-                        FindPackageShare(description_package),
+                        FindPackageShare("rbs_bringup"),
                         "launch",
                         "control.launch.py",
                     ]
@@ -174,12 +87,12 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments={
-            "control_space": "task",
+            "control_space": control_space,
-            "control_strategy": "position",
+            "control_strategy": control_strategy,
-            "has_gripper": "true",
+            "has_gripper": with_gripper_condition,
             "namespace": namespace,
         }.items(),
-        condition=IfCondition(launch_controllers),
+        condition=IfCondition(use_controllers),
     )
 
     moveit = IncludeLaunchDescription(
@@ -195,16 +108,17 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments={
-            "robot_description": robot_description_content,
             "moveit_config_package": moveit_config_package,
-            "moveit_config_file": moveit_config_file,
+            # "moveit_config_file": moveit_config_file,
             "use_sim_time": use_sim_time,
             "tf_prefix": robot_name,
             "with_gripper": with_gripper_condition,
+            "robot_description": robot_description_content,
             "robot_description_semantic": robot_description_semantic_content,
+            "robot_description_kinematics": robot_description_kinematics_filepath,
             "namespace": namespace,
         }.items(),
-        condition=IfCondition(launch_moveit),
+        condition=IfCondition(use_moveit),
     )
 
     skills = IncludeLaunchDescription(
@@ -222,287 +136,222 @@ def launch_setup(context, *args, **kwargs):
         launch_arguments={
             "robot_description": robot_description_content,
             "robot_description_semantic": robot_description_semantic_content,
-            "robot_description_kinematics": robot_description_kinematics,
+            "robot_description_kinematics": robot_description_kinematics_filepath,
             "use_sim_time": use_sim_time,
             "with_gripper_condition": with_gripper_condition,
             "namespace": namespace,
+            "use_moveit": use_moveit,
+            "ee_link_name": ee_link_name,
+            "base_link_name": base_link_name,
         }.items(),
     )
 
-    task_planner = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("rbs_task_planner"),
-                        "launch",
-                        "task_planner.launch.py",
-                    ]
-                )
-            ]
-        ),
-        launch_arguments={
-            # TBD
-        }.items(),
-        condition=IfCondition(launch_task_planner),
-    )
-
-    perception = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("rbs_perception"),
-                        "launch",
-                        "perception.launch.py",
-                    ]
-                )
-            ]
-        ),
-        launch_arguments={
-            # TBD
-        }.items(),
-        condition=IfCondition(launch_perception),
-    )
-
-    asm_config = Node(
-        package="rbs_utils",
-        namespace=namespace,
-        executable="assembly_config_service.py",
-    )
-
     nodes_to_start = [
         robot_state_publisher,
         control,
         moveit,
         skills,
-        asm_config,
-        # task_planner,
-        # perception,
-        rviz,
     ]
     return nodes_to_start
 
 
 def generate_launch_description():
     declared_arguments = []
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_type",
-            description="Type of robot by name",
+            description="Type of robot to launch, specified by name.",
-            choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            choices=[
+                "rbs_arm",
+                "ar4",
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+            ],
             default_value="rbs_arm",
         )
     )
-    # General arguments
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="rbs_arm0_controllers.yaml",
+            default_value="controllers.yaml",
-            description="YAML file with the controllers configuration.",
+            description="YAML file containing configuration settings for the controllers.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_package",
             default_value="rbs_arm",
-            description="Description package with robot URDF/XACRO files. Usually the argument \
+            description="Package containing the robot's URDF/XACRO description files. Can be set to use a custom description.",
-        is not set, it enables use of a custom description.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_file",
             default_value="rbs_arm_modular.xacro",
-            description="URDF/XACRO description file with the robot.",
+            description="URDF/XACRO file describing the robot's model.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_name",
             default_value="arm0",
-            description="tf_prefix of the joint names, useful for \
+            description="Prefix for joint names to support multi-robot setups. If changed, ensure joint names in controllers configuration are updated accordingly.",
-        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="rbs_arm",
-            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+            description="Package containing the MoveIt configuration files (SRDF/XACRO) for the robot. Can be customized.",
-        is not set, it enables use of a custom moveit config.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_file",
             default_value="rbs_arm.srdf.xacro",
-            description="MoveIt SRDF/XACRO description file with the robot.",
+            description="MoveIt SRDF/XACRO file for robot configuration.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_sim_time",
             default_value="true",
-            description="Make MoveIt to use simulation time.\
+            description="Enables simulation time in MoveIt, needed for trajectory planning in simulation.",
-            This is needed for the trajectory planing in simulation.",
+        )
+    )
+    # declared_arguments.append(
+    #     DeclareLaunchArgument(
+    #         "gripper_name",
+    #         default_value="",
+    #         choices=["rbs_gripper", ""],
+    #         description="Specify gripper name (leave empty if none).",
+    #     )
+    # )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "with_gripper",
+            default_value="true",
+            description="Specify if the robot includes a gripper.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "gripper_name",
+            "use_moveit",
+            default_value="true",
+            description="Specify if MoveIt should be launched.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_controllers",
+            default_value="true",
+            description="Specify if controllers should be launched.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "namespace",
             default_value="",
-            choices=["rbs_gripper", ""],
+            description="ROS 2 namespace for the robot.",
-            description="choose gripper by name (leave empty if hasn't)",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "with_gripper", default_value="true", description="With gripper or not?"
+            "x",
+            default_value="0.0",
+            description="X-coordinate for the robot's position in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_rviz", default_value="false", description="Launch RViz?"
+            "y",
+            default_value="0.0",
+            description="Y-coordinate for the robot's position in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "sim_gazebo", default_value="true", description="Gazebo Simulation"
+            "z",
+            default_value="0.0",
+            description="Z-coordinate for the robot's position in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_sim",
+            "roll",
-            default_value="true",
+            default_value="0.0",
-            description="Launch simulator (Gazebo)?\
+            description="Roll orientation of the robot in the world.",
-                              Most general arg",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_moveit", default_value="true", description="Launch moveit?"
+            "pitch",
+            default_value="0.0",
+            description="Pitch orientation of the robot in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_perception", default_value="false", description="Launch perception?"
+            "yaw",
+            default_value="0.0",
+            description="Yaw orientation of the robot in the world.",
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_task_planner",
-            default_value="false",
-            description="Launch task_planner?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "cartesian_controllers",
-            default_value="false",
-            description="Load cartesian\
-                              controllers?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "hardware",
-            choices=["gazebo", "mock"],
-            default_value="gazebo",
-            description="Choose your harware_interface",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_controllers",
-            default_value="true",
-            description="Launch controllers?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "gazebo_gui", default_value="false", description="Launch gazebo with gui?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "namespace", default_value="", description="The ROS2 namespace of a robot"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "x", default_value="0.0", description="Position of robot in world by X"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "y", default_value="0.0", description="Position of robot in world by Y"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "z", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "roll", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "pitch", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "yaw", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "roll", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "pitch", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "yaw", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-
     declared_arguments.append(
         DeclareLaunchArgument(
             "multi_robot",
             default_value="false",
-            description="Flag if you use multi robot setup",
+            description="Specify if the setup is for multiple robots.",
         )
     )
-
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_description",
             default_value="",
-            description="Robot description that override internal robot desctioption",
+            description="Custom robot description that overrides the internal default.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description_semantic",
+            default_value="",
+            description="Custom semantic robot description (SRDF) to override the default.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ee_link_name",
+            default_value="",
+            description="End effector name of robot arm",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "base_link_name",
+            default_value="",
+            description="Base link name if robot arm",
         )
     )
 

rbs_bringup/launch/single_robot.launch.py

@@ -1,301 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-)
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-
-def launch_setup(context, *args, **kwargs):
-    # Initialize Arguments
-    robot_type = LaunchConfiguration("robot_type")
-    # General arguments
-    with_gripper_condition = LaunchConfiguration("with_gripper")
-    controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
-    description_package = LaunchConfiguration("description_package")
-    description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
-    start_joint_controller = LaunchConfiguration("start_joint_controller")
-    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
-    launch_simulation = LaunchConfiguration("launch_sim")
-    launch_moveit = LaunchConfiguration("launch_moveit")
-    launch_task_planner = LaunchConfiguration("launch_task_planner")
-    launch_perception = LaunchConfiguration("launch_perception")
-    moveit_config_package = LaunchConfiguration("moveit_config_package")
-    moveit_config_file = LaunchConfiguration("moveit_config_file")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
-    hardware = LaunchConfiguration("hardware")
-    env_manager = LaunchConfiguration("env_manager")
-    launch_controllers = LaunchConfiguration("launch_controllers")
-    gazebo_gui = LaunchConfiguration("gazebo_gui")
-    gripper_name = LaunchConfiguration("gripper_name")
-
-    sim_gazebo = LaunchConfiguration("sim_gazebo")
-    launch_simulation = LaunchConfiguration("launch_sim")
-    simulation = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("rbs_simulation"),
-                        "launch",
-                        "simulation_gazebo.launch.py",
-                    ]
-                )
-            ]
-        ),
-        launch_arguments={
-            "sim_gazebo": sim_gazebo,
-            "debugger": "false",
-            "launch_env_manager": env_manager,
-            "gazebo_world_filename": "asm2.sdf",
-        }.items(),
-        condition=IfCondition(launch_simulation),
-    )
-
-    # FIXME: namespaces
-    # configured_params = RewrittenYaml(
-    #     source_file=os.path.join(
-    #         get_package_share_directory(
-    #             description_package.perform(context)),
-    #         "config",
-    #         controllers_file.perform(context)),
-    #     root_key=robot_name.perform(context),
-    #     param_rewrites={},
-    #     convert_types=True,
-    # )
-
-    initial_joint_controllers_file_path = os.path.join(
-        get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
-    )
-
-    # controller_paramfile = configured_params.perform(context)
-    # controller_paramfile = PathJoinSubstitution([
-    #     FindPackageShare(robot_type), "config", "rbs_arm0_controllers.yaml"
-    # ])
-    # namespace = "/" + robot_name.perform(context)
-    namespace = ""
-
-    gz_spawner = Node(
-        package="ros_gz_sim",
-        executable="create",
-        # prefix=['gdbserver localhost:1234'],
-        arguments=[
-            "-name",
-            robot_name.perform(context),
-            "-topic",
-            namespace + "/robot_description",
-        ],
-        output="screen",
-        condition=IfCondition(sim_gazebo),
-    )
-
-    single_robot_setup = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
-                )
-            ]
-        ),
-        launch_arguments={
-            "env_manager": env_manager,
-            "with_gripper": with_gripper_condition,
-            "gripper_name": gripper_name,
-            "controllers_file": initial_joint_controllers_file_path,
-            "robot_type": robot_type,
-            # "controllers_file": controller_paramfile,
-            "cartesian_controllers": cartesian_controllers,
-            "description_package": description_package,
-            "description_file": description_file,
-            "robot_name": robot_type,
-            "start_joint_controller": start_joint_controller,
-            "initial_joint_controller": initial_joint_controller,
-            "launch_simulation": launch_simulation,
-            "launch_moveit": launch_moveit,
-            "launch_task_planner": launch_task_planner,
-            "launch_perception": launch_perception,
-            "moveit_config_package": moveit_config_package,
-            "moveit_config_file": moveit_config_file,
-            "use_sim_time": use_sim_time,
-            "sim_gazebo": sim_gazebo,
-            "hardware": hardware,
-            "launch_controllers": "true",
-            "gazebo_gui": gazebo_gui,
-            # "x": "0.5",
-            # "y": "0.5",
-            # "z": "0.5"
-        }.items(),
-    )
-
-    nodes_to_start = [simulation, gz_spawner, single_robot_setup]
-    return nodes_to_start
-
-
-def generate_launch_description():
-    declared_arguments = []
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_type",
-            description="Type of robot by name",
-            choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
-            default_value="rbs_arm",
-        )
-    )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_file",
-            default_value="rbs_arm0_controllers.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_package",
-            default_value="rbs_arm",
-            description="Description package with robot URDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom description.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_file",
-            default_value="rbs_arm_modular.xacro",
-            description="URDF/XACRO description file with the robot.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_name",
-            default_value="arm0",
-            description="Name for robot, used to apply namespace for specific robot in multirobot setup",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "start_joint_controller",
-            default_value="false",
-            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="rbs_arm",
-            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom moveit config.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "moveit_config_file",
-            default_value="rbs_arm.srdf.xacro",
-            description="MoveIt SRDF/XACRO description file with the robot.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="Make MoveIt to use simulation time.\
-            This is needed for the trajectory planing in simulation.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "gripper_name",
-            default_value="rbs_gripper",
-            choices=["rbs_gripper", ""],
-            description="choose gripper by name (leave empty if hasn't)",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "with_gripper", default_value="true", description="With gripper or not?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "sim_gazebo", default_value="true", description="Gazebo Simulation"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "env_manager", default_value="true", description="Launch env_manager?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_sim",
-            default_value="true",
-            description="Launch simulator (Gazebo)?\
-                              Most general arg",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_moveit", default_value="false", description="Launch moveit?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_perception", default_value="false", description="Launch perception?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_task_planner",
-            default_value="false",
-            description="Launch task_planner?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "cartesian_controllers",
-            default_value="true",
-            description="Load cartesian\
-                              controllers?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "hardware",
-            choices=["gazebo", "mock"],
-            default_value="gazebo",
-            description="Choose your harware_interface",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_controllers",
-            default_value="true",
-            description="Launch controllers?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "gazebo_gui", default_value="true", description="Launch gazebo with gui?"
-        )
-    )
-
-    return LaunchDescription(
-        declared_arguments + [OpaqueFunction(function=launch_setup)]
-    )

rbs_bt_executor/src/MoveToPose.cpp

@@ -22,6 +22,7 @@ public:
   bool setGoal(RosActionNode::Goal& goal) override {
     getInput("robot_name", goal.robot_name);
     getInput("pose", goal.target_pose);
+    goal.duration = 2.0;
     return true;
   }
 

rbs_bt_executor/src/MoveToPoseArray.cpp

@@ -34,6 +34,7 @@ public:
     goal.target_pose = m_pose_vec->poses[0];
     goal.end_effector_velocity = 0.3;
     goal.end_effector_acceleration = 0.3;
+    goal.duration = 2.0;
     m_pose_vec->poses.erase(m_pose_vec->poses.begin());
 
     setOutput("pose_vec_out", m_pose_vec);

rbs_skill_interfaces/action/MoveitSendJointStates.action

@@ -6,7 +6,7 @@ rbs_skill_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (join
 float64[] joint_values
 float32 joints_velocity_scaling_factor
 float32 joints_acceleration_scaling_factor
-float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
+float32 duration #if this action cannot be completed within this time period it should be considered failed.
 ---
 #result definition
 bool success

rbs_skill_interfaces/action/MoveitSendPose.action

@@ -11,7 +11,7 @@ geometry_msgs/Pose target_pose
 string robot_name
 float32 end_effector_velocity
 float32 end_effector_acceleration
-float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
+float32 duration #if this action cannot be completed within this time period it should be considered failed.
 ---
 #result definition
 bool success

rbs_skill_servers/CMakeLists.txt

@@ -1,12 +1,10 @@
 cmake_minimum_required(VERSION 3.8)
 project(rbs_skill_servers)
 
-# Default to C99
+# Set default standards
 if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
-
-# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
@@ -15,7 +13,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
+set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS TRUE)
+
+# Find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
@@ -30,126 +30,89 @@ find_package(rbs_skill_interfaces REQUIRED)
 find_package(rmw REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_msgs REQUIRED)
-find_package(tinyxml2_vendor REQUIRED)
-find_package(TinyXML2 REQUIRED)
 find_package(Eigen3 3.3 REQUIRED)
 find_package(rbs_utils REQUIRED)
-# find_package(moveit_servo REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
 
-# Default to Fortress
+set(DEPS
-set(SDF_VER 12)
-
-# If the user didn't specify a GZ distribution, pick the one matching the ROS
-# distribution according to REP 2000
-if(NOT DEFINED ENV{GZ_VERSION} AND DEFINED ENV{ROS_DISTRO})
-  if("$ENV{ROS_DISTRO}" STREQUAL "humble")
-    set(ENV{GZ_VERSION} "fortress")
-  endif()
-endif()
-
-# Find libsdformat matching the picked GZ distribution
-if("$ENV{GZ_VERSION}" STREQUAL "fortress")
-  find_package(sdformat12 REQUIRED)
-  set(SDF_VER ${sdformat12_VERSION_MAJOR})
-  message(STATUS "Compiling against Gazebo Fortress (libSDFormat 12)")
-elseif("$ENV{GZ_VERSION}" STREQUAL "garden")
-  find_package(sdformat13 REQUIRED)
-  set(SDF_VER ${sdformat13_VERSION_MAJOR})
-  message(STATUS "Compiling against Gazebo Garden (libSDFormat 13)")
-  # No GZ distribution specified, find any version of libsdformat we can
-else()
-  foreach(major RANGE 13 9)
-    find_package(sdformat${major} QUIET)
-    if(sdformat${major}_FOUND)
-      # Next `find_package` call will be a noop
-      set(SDF_VER ${major})
-      message(STATUS "Compiling against libSDFormat ${major}")
-      break()
-    endif()
-  endforeach()
-endif()
-
-include_directories(SYSTEM ${TINYXML2_INCLUDE_DIR})
-
-link_directories(${TINYXML2_LIBRARY_DIRS})
-
-set(deps
     rclcpp
     rclcpp_action
     moveit_core
     moveit_ros_planning
     moveit_ros_planning_interface
+    control_msgs
+    controller_manager_msgs
     moveit_msgs
-    # moveit_servo
     geometry_msgs
     tf2_ros
     rclcpp_components
     rbs_skill_interfaces
     tf2_eigen
     tf2_msgs
-    tinyxml2_vendor
     geometric_shapes
-    sdformat${SDF_VER})
+)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-# -- GripperActionServer --
-add_library(gripper_action_server SHARED src/gripper_control_action_server.cpp)
-target_include_directories(
-  gripper_action_server
-  PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-          $<INSTALL_INTERFACE:include>)
-target_compile_definitions(gripper_action_server
-                           PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")
-ament_target_dependencies(gripper_action_server ${deps})
-rclcpp_components_register_node(
-  gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer"
-  EXECUTABLE gripper_control_action_server)
-
-# -- PickPlacePoseLoader --
-# add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)
-# target_include_directories(
-#   pick_place_pose_loader
-#   PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-#           $<INSTALL_INTERFACE:include>)
-# target_compile_definitions(pick_place_pose_loader
-#                            PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
-# ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3 
-#                           rbs_utils)
-# rclcpp_components_register_node(
-#   pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
-#   EXECUTABLE pick_place_pose_loader_service_server)
-
-# -- MoveitActionServers -- 
-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})
-
-add_executable(move_topose_action_server src/move_topose_action_server.cpp)
-ament_target_dependencies(move_topose_action_server ${deps})
-
-add_executable(move_cartesian_path_action_server
-               src/move_cartesian_path_action_server.cpp)
-ament_target_dependencies(move_cartesian_path_action_server ${deps})
 
 
-# add_executable(servo_action_server
+macro(add_ros2_component target_name source_file plugin_name executable_name)
-#                src/moveit_servo_skill_server.cpp)
+  add_library(${target_name} SHARED ${source_file})
-# ament_target_dependencies(servo_action_server ${deps})
+  target_include_directories(${target_name}
-#
+    PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+            $<INSTALL_INTERFACE:include>)
+  target_compile_definitions(${target_name} PRIVATE "${target_name}_BUILDING_DLL")
+  ament_target_dependencies(${target_name} ${DEPS})
+  rclcpp_components_register_node(${target_name} PLUGIN ${plugin_name} EXECUTABLE ${executable_name})
+endmacro()
+
+# Add components
+add_ros2_component(gripper_action_server
+    src/gripper_command.cpp
+    "rbs_skill_actions::GripperControlActionServer"
+    gripper_command)
+
+add_ros2_component(cartesian_move_to_pose
+    src/mtp_cart.cpp
+    "rbs_skill_actions::CartesianMoveToPose"
+    mtp_cart)
+
+add_ros2_component(move_to_joint_states
+    src/mtjs_jtc.cpp
+    "rbs_skill_actions::MoveToJointStateActionServer"
+    mtjs_jtc)
+
+add_ros2_component(move_to_pose
+    src/mtp_jtc.cpp
+    "rbs_skill_actions::MoveToPose"
+    mtp_jtc)
+
+add_ros2_component(move_to_pose_cartesian
+    src/mtp_jtc_cart.cpp
+    "rbs_skill_actions::MoveToPoseJtcCartesian"
+    mtp_jtc_cart)
+
+add_ros2_component(move_to_pose_moveit
+    src/mtp_moveit.cpp
+    "rbs_skill_actions::MoveitMtp"
+    mtp_moveit)
+
+add_ros2_component(move_to_pose_moveit_cartesian
+    src/mtp_moveit_cart.cpp
+    "rbs_skill_actions::MoveitMtpCart"
+    mtp_moveit_cart)
+
+# Install directories and targets
 install(DIRECTORY include/ DESTINATION include)
-
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
 
 install(
-  TARGETS move_topose_action_server
+  TARGETS 
           gripper_action_server
-          move_to_joint_states_action_server
+          cartesian_move_to_pose
-          move_cartesian_path_action_server
+          move_to_joint_states
-          # servo_action_server
+          move_to_pose
+          move_to_pose_cartesian
+          move_to_pose_moveit_cartesian
+          move_to_pose_moveit
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME})

rbs_skill_servers/config/gripperPositions.yaml

@@ -1,73 +0,0 @@
-box1:
-  PreGraspPose: [0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
-  GraspPose: [0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose: [0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose2: [0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
-  PrePlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
-  PlacePose: [-0.4500, -0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
-  PostPlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
-
-box2:
-  PreGraspPose: [0.0, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
-  GraspPose: [0.0, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose: [0.0, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose2: [0.0, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
-  PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
-  PlacePose: [-0.4500, 0.0, 0.46, 1.0, 0.0, 0.0, 0.0]
-  PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
-
-box3:
-  PreGraspPose: [-0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
-  GraspPose: [-0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose: [-0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose2: [-0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
-  PrePlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
-  PlacePose: [-0.4500, 0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
-  PostPlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
-
-box4:
-  PreGraspPose: [0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
-  GraspPose: [0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose: [0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose2: [0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
-  PrePlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
-  PlacePose: [-0.4500, 0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
-  PostPlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
-
-box5:
-  PreGraspPose: [0.0, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
-  GraspPose: [0.0, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose: [0.0, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose2: [0.0, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
-  PrePlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
-  PlacePose: [-0.4500, -0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
-  PostPlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
-
-box6:
-  PreGraspPose: [-0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
-  GraspPose: [-0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose: [-0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
-  PostGraspPose2: [-0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
-  PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
-  PlacePose: [-0.4500, 0.0, 0.56, 1.0, 0.0, 0.0, 0.0]
-  PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
-
-
-pre_place_joint_states:
-  -  0.11030024152330242
-  -  0.8306162497371689
-  - -1.7680363334650195
-  - -0.6262361658856159
-  -  1.4713289344704141
-  - -0.11066274809566562
-
-
-
-scene_objects:
-  - box1
-  - box2
-  - box3
-  - box4
-  - box5
-  - box6
-  - last

rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp

@@ -0,0 +1,456 @@
+#include <algorithm>
+#include <controller_manager_msgs/srv/detail/configure_controller__struct.hpp>
+#include <controller_manager_msgs/srv/detail/list_controllers__struct.hpp>
+#include <controller_manager_msgs/srv/detail/load_controller__struct.hpp>
+#include <controller_manager_msgs/srv/detail/switch_controller__struct.hpp>
+#include <memory>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/parameter_client.hpp>
+#include <rclcpp_action/create_server.hpp>
+#include <rclcpp_action/server.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rmw/qos_profiles.h>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace rbs_skill_actions {
+
+template <typename ActionT> class SkillBase : public rclcpp::Node {
+public:
+  explicit SkillBase(const std::string &node_name,
+                     const std::string &action_server_name,
+                     const rclcpp::NodeOptions &options)
+      : Node(node_name, options) {
+
+    auto act_cbg =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    m_action_server = rclcpp_action::create_server<ActionT>(
+        this, action_server_name,
+        std::bind(&SkillBase::handle_goal, this, std::placeholders::_1,
+                  std::placeholders::_2),
+        std::bind(&SkillBase::handle_canceled, this, std::placeholders::_1),
+        std::bind(&SkillBase::handle_accepted, this, std::placeholders::_1));
+
+    m_load_controller_client =
+        this->create_client<controller_manager_msgs::srv::LoadController>(
+            "/controller_manager/load_controller");
+    m_switch_controller_client =
+        create_client<controller_manager_msgs::srv::SwitchController>(
+            "/controller_manager/switch_controller");
+    m_list_controllers_client =
+        create_client<controller_manager_msgs::srv::ListControllers>(
+            "/controller_manager/list_controllers");
+    m_configure_controller_client =
+        create_client<controller_manager_msgs::srv::ConfigureController>(
+            "/controller_manager/configure_controller");
+  }
+
+  explicit SkillBase(const std::string &node_name,
+                     const rclcpp::NodeOptions &options)
+      : SkillBase(node_name, node_name, options) {}
+
+protected:
+  std::shared_ptr<const typename ActionT::Goal> m_current_goal;
+  std::shared_ptr<typename ActionT::Result> m_current_result;
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
+      m_current_goal_handle;
+
+  bool m_interfaces_checked{false};
+
+  virtual rclcpp_action::GoalResponse
+  handle_goal(const rclcpp_action::GoalUUID &uuid,
+              std::shared_ptr<const typename ActionT::Goal> goal) {
+    RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s]",
+                goal->robot_name.c_str());
+    (void)uuid;
+
+    m_current_goal = goal;
+    m_current_result = std::make_shared<typename ActionT::Result>();
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  virtual rclcpp_action::CancelResponse handle_canceled(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
+          goal_handle) {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handle_accepted(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
+          goal_handle) {
+    auto execution_thread = [this, goal_handle]() {
+      this->execute(goal_handle);
+    };
+    std::thread{execution_thread}.detach();
+  }
+
+  void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
+                   goal_handle) {
+
+    m_current_goal_handle = goal_handle;
+
+    m_required_controllers = requiredStateControllers();
+    m_required_controllers.push_back(requiredActionController());
+    m_required_controllers.erase(std::remove_if(
+        m_required_controllers.begin(), m_required_controllers.end(),
+        [](const std::string &str) { return str.empty(); }));
+
+    if (m_required_controllers.empty()) {
+      RCLCPP_ERROR(this->get_logger(), "Required controllers is empty");
+      m_current_goal_handle->abort(m_current_result);
+      return;
+    }
+    updateControllersMap();
+
+    if (m_controllers_map.count(requiredActionController()) &&
+        m_controllers_map[requiredActionController()] == "inactive") {
+      switchController(requiredActionController());
+    } else if (m_controllers_map.count(requiredActionController()) == 0) {
+      loadConfigureSwitchExecuteController({requiredActionController()});
+    } else if (!checkParameters()) {
+      getRequirementParametersAndExecute(requiredActionController());
+    } else {
+      executeAction();
+    }
+  }
+
+  void updateControllersMap() {
+    using namespace std::chrono_literals;
+
+    RCLCPP_INFO(this->get_logger(), "Called list_controllers service to check "
+                                    "if required controllers are loaded");
+
+    auto list_controllers_request = std::make_shared<
+        controller_manager_msgs::srv::ListControllers::Request>();
+
+    auto list_controllers_future =
+        m_list_controllers_client->async_send_request(list_controllers_request);
+
+    if (list_controllers_future.wait_for(3s) != std::future_status::ready) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Timeout waiting for list_controllers response.");
+      return;
+    }
+
+    auto response = list_controllers_future.get();
+    m_controllers_map.clear();
+
+    std::unordered_set<std::string> loaded_controllers;
+    loaded_controllers.reserve(response->controller.size());
+    for (const auto &controller : response->controller) {
+      m_controllers_map[controller.name] = controller.state;
+    }
+
+    // if (m_missing_controllers.empty()) {
+    //   RCLCPP_INFO(this->get_logger(), "All required controllers are
+    //   loaded."); m_interfaces_checked = true; return true;
+    // } else {
+    //   RCLCPP_WARN(this->get_logger(), "Missing required controllers:");
+    //   for (const auto &missing_controller : m_missing_controllers) {
+    //     RCLCPP_WARN(this->get_logger(), "\t- %s",
+    //     missing_controller.c_str());
+    //   }
+    //   m_interfaces_checked = false;
+    //   return false;
+    // }
+  }
+
+  void loadConfigureSwitchExecuteController(
+      const std::vector<std::string> &controller_names) {
+    for (auto &controller_name : controller_names) {
+      RCLCPP_WARN(this->get_logger(), "Attempting to load controller [%s]",
+                  controller_name.c_str());
+      auto load_request = std::make_shared<
+          controller_manager_msgs::srv::LoadController::Request>();
+      load_request->name = controller_name;
+
+      auto load_future = m_load_controller_client->async_send_request(
+          load_request,
+          [this, controller_name](
+              rclcpp::Client<controller_manager_msgs::srv::LoadController>::
+                  SharedFuture future) {
+            if (future.get()->ok) {
+              RCLCPP_INFO(this->get_logger(),
+                          "Controller %s successfully loaded",
+                          controller_name.c_str());
+              configureController(controller_name);
+            } else {
+              RCLCPP_ERROR(this->get_logger(), "Failed to load controller %s",
+                           controller_name.c_str());
+              m_current_goal_handle->abort(m_current_result);
+            }
+          });
+    }
+  }
+
+  void configureController(const std::string &controller_name) {
+    auto configure_request = std::make_shared<
+        controller_manager_msgs::srv::ConfigureController::Request>();
+    configure_request->name = controller_name;
+
+    auto configure_future = m_configure_controller_client->async_send_request(
+        configure_request,
+        [this, controller_name](
+            rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::
+                SharedFuture future) {
+          if (future.get()->ok) {
+            RCLCPP_INFO(this->get_logger(),
+                        "Controller %s successfully configured",
+                        controller_name.c_str());
+            switchController(controller_name);
+          } else {
+            RCLCPP_ERROR(this->get_logger(),
+                         "Failed to configure controller %s",
+                         controller_name.c_str());
+            m_current_goal_handle->abort(m_current_result);
+          }
+        });
+  }
+
+  void switchController(const std::string &controller_name) {
+
+    detectResourceConflict(controller_name, [this, controller_name](
+                                                const std::vector<std::string>
+                                                    &conflicted_controllers) {
+      auto switch_request = std::make_shared<
+          controller_manager_msgs::srv::SwitchController::Request>();
+      switch_request->activate_controllers = {controller_name};
+      switch_request->deactivate_controllers = conflicted_controllers;
+      switch_request->strictness =
+          controller_manager_msgs::srv::SwitchController::Request::STRICT;
+
+      auto switch_future = m_switch_controller_client->async_send_request(
+          switch_request,
+          [this, controller_name](
+              rclcpp::Client<controller_manager_msgs::srv::SwitchController>::
+                  SharedFuture future) {
+            if (future.get()->ok) {
+              RCLCPP_INFO(this->get_logger(),
+                          "Controller %s successfully switched",
+                          controller_name.c_str());
+              if (!checkParameters()) {
+                getRequirementParametersAndExecute(controller_name);
+              } else {
+                if (controller_name == requiredActionController()) {
+                  executeAction();
+                }
+              }
+            } else {
+              RCLCPP_ERROR(this->get_logger(), "Failed to switch controller %s",
+                           controller_name.c_str());
+              m_current_goal_handle->abort(m_current_result);
+            }
+          });
+    });
+  }
+
+  bool checkParameters() {
+    std::ostringstream missing_params_stream;
+    std::ostringstream empty_params_stream;
+
+    for (const auto &name : requiredParameters()) {
+      if (!this->has_parameter(name)) {
+        missing_params_stream << "\n\t- " << name;
+        continue;
+      }
+
+      const auto &parameter = this->get_parameter(name);
+      if (isEmptyParameter(parameter)) {
+        empty_params_stream << "\n\t- " << name;
+      }
+    }
+
+    if (missing_params_stream.tellp() > 0) {
+      RCLCPP_WARN(this->get_logger(),
+                  "The following required parameters are missing:%s",
+                  missing_params_stream.str().c_str());
+    }
+
+    if (empty_params_stream.tellp() > 0) {
+      RCLCPP_WARN(this->get_logger(),
+                  "The following required parameters are empty:%s",
+                  empty_params_stream.str().c_str());
+    }
+
+    return missing_params_stream.tellp() == 0 &&
+           empty_params_stream.tellp() == 0;
+  }
+
+  bool isEmptyParameter(const rclcpp::Parameter &parameter) {
+    switch (parameter.get_type()) {
+    case rclcpp::ParameterType::PARAMETER_STRING:
+      return parameter.as_string().empty();
+    case rclcpp::ParameterType::PARAMETER_STRING_ARRAY:
+      return parameter.as_string_array().empty();
+    case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY:
+      return parameter.as_integer_array().empty();
+    case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY:
+      return parameter.as_double_array().empty();
+    default:
+      return false;
+    }
+  }
+
+  virtual std::string requiredActionController() = 0;
+
+  virtual std::vector<std::string> requiredStateControllers() { return {""}; }
+
+  virtual std::vector<std::string> requiredParameters() { return {}; }
+
+  virtual void executeAction() = 0;
+
+  void getRequirementParametersAndExecute(const std::string &controller_name) {
+
+    RCLCPP_INFO(this->get_logger(),
+                "Connected to service %s, asking for parameters:",
+                controller_name.c_str());
+
+    for (const auto &param : requiredParameters()) {
+      RCLCPP_INFO(this->get_logger(), "\t- %s", param.c_str());
+    }
+
+    auto parameter_callback =
+        [this, controller_name](
+            const std::shared_future<std::vector<rclcpp::Parameter>> &future) {
+          try {
+            auto parameters = future.get();
+            if (parameters.empty()) {
+              RCLCPP_ERROR(this->get_logger(),
+                           "No parameter found or empty array received");
+              m_current_goal_handle->abort(m_current_result);
+            } else {
+              for (const auto &param : parameters) {
+                switch (param.get_type()) {
+                case rclcpp::ParameterType::PARAMETER_BOOL:
+                  this->declare_parameter(param.get_name(), param.as_bool());
+                  break;
+                case rclcpp::ParameterType::PARAMETER_INTEGER:
+                  this->declare_parameter(param.get_name(), param.as_int());
+                  break;
+                case rclcpp::ParameterType::PARAMETER_DOUBLE:
+                  this->declare_parameter(param.get_name(), param.as_double());
+                  break;
+                case rclcpp::ParameterType::PARAMETER_STRING:
+                  this->declare_parameter(param.get_name(), param.as_string());
+                  break;
+                case rclcpp::ParameterType::PARAMETER_STRING_ARRAY:
+                  this->declare_parameter(param.get_name(),
+                                          param.as_string_array());
+                  break;
+                case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY:
+                  this->declare_parameter(param.get_name(),
+                                          param.as_integer_array());
+                  break;
+                case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY:
+                  this->declare_parameter(param.get_name(),
+                                          param.as_double_array());
+                  break;
+                default:
+                  RCLCPP_WARN(this->get_logger(),
+                              "Unsupported parameter type for parameter '%s'",
+                              param.get_name().c_str());
+                  break;
+                }
+              }
+              if (controller_name == requiredActionController()) {
+                executeAction();
+              }
+            }
+          } catch (const std::exception &e) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to get parameter: %s",
+                         e.what());
+          }
+        };
+
+    m_parameter_client =
+        std::make_shared<rclcpp::AsyncParametersClient>(this, controller_name);
+
+    auto future = m_parameter_client->get_parameters(
+        requiredParameters(), std::move(parameter_callback));
+
+    RCLCPP_INFO(this->get_logger(),
+                "Asynchronous request sent for required parameters");
+  }
+
+  void detectResourceConflict(
+      const std::string &new_controller,
+      std::function<void(const std::vector<std::string> &)> callback) {
+
+    auto list_controllers_request = std::make_shared<
+        controller_manager_msgs::srv::ListControllers::Request>();
+
+    m_list_controllers_client->async_send_request(
+        list_controllers_request,
+        [this, new_controller,
+         callback](const rclcpp::Client<
+                   controller_manager_msgs::srv::ListControllers>::SharedFuture
+                       future) {
+          std::vector<std::string> conflicted_controller;
+          auto it_new_controller = std::find_if(
+              future.get()->controller.begin(), future.get()->controller.end(),
+              [&new_controller](
+                  const controller_manager_msgs::msg::ControllerState
+                      controller) {
+                return controller.name == new_controller;
+              });
+
+          if (it_new_controller != future.get()->controller.end()) {
+            auto new_controller_interface =
+                it_new_controller->required_command_interfaces;
+            RCLCPP_INFO(this->get_logger(), "Received list of controllers");
+
+            for (const auto &controller : future.get()->controller) {
+              if (controller.state == "active") {
+                RCLCPP_INFO(this->get_logger(), "Checking controller: %s",
+                            controller.name.c_str());
+
+                bool has_conflict = std::any_of(
+                    controller.required_command_interfaces.begin(),
+                    controller.required_command_interfaces.end(),
+                    [&new_controller_interface](const std::string &interface) {
+                      return std::find(new_controller_interface.begin(),
+                                       new_controller_interface.end(),
+                                       interface) !=
+                             new_controller_interface.end();
+                    });
+
+                if (has_conflict) {
+                  conflicted_controller.push_back(controller.name);
+                  RCLCPP_WARN(this->get_logger(), "Conflicted controller: %s",
+                              controller.name.c_str());
+                }
+              }
+            }
+          } else {
+            RCLCPP_WARN(this->get_logger(),
+                        "New controller not found in loaded controllers");
+          }
+
+          callback(conflicted_controller);
+        });
+  }
+
+private:
+  rclcpp::Client<controller_manager_msgs::srv::LoadController>::SharedPtr
+      m_load_controller_client;
+  rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr
+      m_switch_controller_client;
+  rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr
+      m_list_controllers_client;
+  rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::SharedPtr
+      m_configure_controller_client;
+  rclcpp::AsyncParametersClient::SharedPtr m_parameter_client;
+  typename rclcpp_action::Server<ActionT>::SharedPtr m_action_server;
+  std::vector<std::string> m_required_controllers{};
+  std::vector<std::string> m_missing_controllers{};
+  std::unordered_map<std::string, std::string> m_controllers_map;
+};
+
+} // namespace rbs_skill_actions

rbs_skill_servers/launch/skills.launch.py

@@ -2,82 +2,98 @@ from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import ComposableNodeContainer, Node
-from rbs_launch_utils.launch_common import load_yaml
+from launch_ros.actions.composable_node_container import ComposableNode
+from rbs_launch_utils.launch_common import load_yaml, load_yaml_abs
 
 
 def launch_setup(context, *args, **kwargs):
-    robot_description_decl = LaunchConfiguration("robot_description")
+    robot_description_content = LaunchConfiguration("robot_description")
-    robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
+    robot_description_semantic_content = LaunchConfiguration("robot_description_semantic")
-    robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
+    robot_description_kinematics_filepath = LaunchConfiguration("robot_description_kinematics")
     use_sim_time = LaunchConfiguration("use_sim_time")
-    with_gripper_condition = LaunchConfiguration("with_gripper_condition")
+    use_moveit = LaunchConfiguration("use_moveit")
-    points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
+    ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
-    robot_description = {"robot_description": robot_description_decl}
+    base_link_name = LaunchConfiguration("base_link_name").perform(context)
+    # with_gripper_condition = LaunchConfiguration("with_gripper_condition")
+
+    robot_description = {"robot_description": robot_description_content}
     robot_description_semantic = {
-        "robot_description_semantic": robot_description_semantic_decl
+        "robot_description_semantic": robot_description_semantic_content
     }
     namespace = LaunchConfiguration("namespace")
 
-    points_params = load_yaml("rbs_skill_servers", "config/gripperPositions.yaml")
+    kinematics_yaml = load_yaml_abs(robot_description_kinematics_filepath.perform(context))
-
-    kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
-
     robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
 
-    move_topose_action_server = Node(
+    skills_container = ComposableNodeContainer(
-        package="rbs_skill_servers",
+        name="skills",
-        executable="move_topose_action_server",
         namespace=namespace,
-        parameters=[
+        package="rclcpp_components",
-            robot_description,
+        # prefix=['gdbserver localhost:1234'],
-            robot_description_semantic,
+        executable="component_container_mt",
-            robot_description_kinematics,
+        composable_node_descriptions=[
-            {"use_sim_time": use_sim_time},
+            ComposableNode(
-        ],
+                package="rbs_skill_servers",
-    )
+                plugin="rbs_skill_actions::MoveToJointStateActionServer",
-
+                name="mtjs_jtc",
-    gripper_control_node = Node(
+                parameters=[{"use_sim_time": use_sim_time}],
-        package="rbs_skill_servers",
+            ),
-        executable="gripper_control_action_server",
+            ComposableNode(
-        namespace=namespace,
+                package="rbs_skill_servers",
-        parameters=[
+                plugin="rbs_skill_actions::CartesianMoveToPose",
-            robot_description,
+                name="mtp_cart",
-            robot_description_semantic,
+                parameters=[
-            robot_description_kinematics,
+                    {"use_sim_time": use_sim_time},
-            {"use_sim_time": use_sim_time},
+                    {"robot_name": namespace},
-        ],
+                ],
-        condition=IfCondition(with_gripper_condition),
+            ),
-    )
+            ComposableNode(
-
+                package="rbs_skill_servers",
-    move_cartesian_path_action_server = Node(
+                plugin="rbs_skill_actions::MoveToPose",
-        package="rbs_skill_servers",
+                name="mtp_jtc",
-        executable="move_cartesian_path_action_server",
+                parameters=[
-        namespace=namespace,
+                    {"use_sim_time": use_sim_time},
-        parameters=[
+                    {"robot_name": namespace},
-            robot_description,
+                    robot_description,
-            robot_description_semantic,
+                ],
-            robot_description_kinematics,
+            ),
-            {"use_sim_time": use_sim_time},
+            ComposableNode(
-        ],
+                package="rbs_skill_servers",
-    )
+                plugin="rbs_skill_actions::MoveToPoseJtcCartesian",
-
+                name="mtp_jtc_cart",
-    cartesian_move_to_pose_action_server = Node(
+                parameters=[
-        package="rbs_skill_servers",
+                    {"use_sim_time": use_sim_time},
-        executable="move_to_pose.py",
+                    {"robot_name": namespace},
-        namespace=namespace,
+                    {"base_link": base_link_name},
-        parameters=[{"use_sim_time": use_sim_time}, {"robot_name": namespace}],
+                    {"robot_ee_link": ee_link_name},
-    )
+                    robot_description,
-
+                ],
-    move_joint_state_action_server = Node(
+            ),
-        package="rbs_skill_servers",
+            ComposableNode(
-        executable="move_to_joint_states_action_server",
+                package="rbs_skill_servers",
-        namespace=namespace,
+                plugin="rbs_skill_actions::MoveitMtp",
-        parameters=[
+                name="mtp_moveit",
-            robot_description,
+                parameters=[
-            robot_description_semantic,
+                    robot_description,
-            robot_description_kinematics,
+                    robot_description_semantic,
-            {"use_sim_time": use_sim_time},
+                    robot_description_kinematics,
+                    {"use_sim_time": use_sim_time},
+                ],
+                condition=IfCondition(use_moveit),
+            ),
+            ComposableNode(
+                package="rbs_skill_servers",
+                plugin="rbs_skill_actions::MoveitMtpCart",
+                name="mtp_moveit_cart",
+                parameters=[
+                    robot_description,
+                    robot_description_semantic,
+                    robot_description_kinematics,
+                    {"use_sim_time": use_sim_time},
+                ],
+                condition=IfCondition(use_moveit),
+            ),
         ],
     )
 
@@ -90,12 +106,7 @@ def launch_setup(context, *args, **kwargs):
 
     nodes_to_start = [
         assembly_config,
-        move_topose_action_server,
+        skills_container,
-        gripper_control_node,
-        move_cartesian_path_action_server,
-        move_joint_state_action_server,
-        cartesian_move_to_pose_action_server,
-        # grasp_pose_loader
     ]
     return nodes_to_start
 
@@ -116,15 +127,24 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument("robot_description_kinematics", default_value="")
     )
-    declared_arguments.append(DeclareLaunchArgument("use_sim_time", default_value=""))
+    declared_arguments.append(
+        DeclareLaunchArgument("use_sim_time", default_value="false")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("use_moveit", default_value="false")
+    )
     declared_arguments.append(
         DeclareLaunchArgument("with_gripper_condition", default_value="")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("points_params_filepath", default_value="")
+        DeclareLaunchArgument("namespace", default_value="")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("ee_link_name", default_value="")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("base_link_name", default_value="")
     )
-    declared_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
-
     return LaunchDescription(
         declared_arguments + [OpaqueFunction(function=launch_setup)]
     )

rbs_skill_servers/package.xml

@@ -23,6 +23,7 @@
   <depend>rbs_skill_interfaces</depend>
   <depend>tf2_eigen</depend>
   <depend>rbs_utils</depend>
+  <depend>rclcpp_components</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rbs_skill_servers/src/move_to_joint_states_action_server.cpp

@@ -1,164 +0,0 @@
-#include <cinttypes>
-#include <functional>
-#include <memory>
-#include <thread>
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/timer.hpp"
-#include "rclcpp_components/register_node_macro.hpp"
-
-// action libs
-#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/quaternion.hpp"
-#include "geometry_msgs/msg/transform.hpp"
-
-// moveit libs
-#include "moveit/move_group_interface/move_group_interface.h"
-#include "moveit/planning_interface/planning_interface.h"
-
-/*
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2_ros/transform_listener.h>
-*/
-// For Visualization
-// #include <eigen_conversions/eigen_msg.h>
-#include "moveit_msgs/action/move_group.hpp"
-#include "moveit_msgs/msg/display_robot_state.hpp"
-#include "moveit_msgs/msg/display_trajectory.hpp"
-#include "moveit_msgs/msg/robot_trajectory.hpp"
-
-namespace rbs_skill_actions {
-
-class MoveToJointStateActionServer : public rclcpp::Node {
-public:
-  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) {
-    // using namespace std::placeholders;
-    // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
-    //     this->get_node_base_interface(),
-    //     this->get_node_clock_interface(),
-    //     this->get_node_logging_interface(),
-    //     this->get_node_waitables_interface(),
-    //     "move_topose",
-    //     std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
-    //     std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
-    //     std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
-  }
-
-  void init() {
-
-    action_server_ = rclcpp_action::create_server<MoveitSendJointStates>(
-        node_->get_node_base_interface(), node_->get_node_clock_interface(),
-        node_->get_node_logging_interface(),
-        node_->get_node_waitables_interface(), "move_to_joint_states",
-        std::bind(&MoveToJointStateActionServer::goal_callback, this,
-                  std::placeholders::_1, std::placeholders::_2),
-        std::bind(&MoveToJointStateActionServer::cancel_callback, this,
-                  std::placeholders::_1),
-        std::bind(&MoveToJointStateActionServer::accepted_callback, this,
-                  std::placeholders::_1));
-  }
-
-private:
-  rclcpp::Node::SharedPtr node_;
-  rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
-
-  using ServerGoalHandle =
-      rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
-
-  rclcpp_action::GoalResponse
-  goal_callback(const rclcpp_action::GoalUUID &uuid,
-                std::shared_ptr<const MoveitSendJointStates::Goal> goal) {
-    RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]",
-                goal->robot_name.c_str());
-    (void)uuid;
-    if (false) {
-      return rclcpp_action::GoalResponse::REJECT;
-    }
-    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-  }
-
-  rclcpp_action::CancelResponse
-  cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
-    RCLCPP_INFO(this->get_logger(), "Received cancel request");
-    (void)goal_handle;
-    return rclcpp_action::CancelResponse::ACCEPT;
-  }
-
-  void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
-    using namespace std::placeholders;
-    std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1),
-                goal_handle)
-        .detach();
-  }
-
-  void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
-    RCLCPP_INFO(this->get_logger(), "Executing action goal");
-    const auto goal = goal_handle->get_goal();
-    auto result = std::make_shared<MoveitSendJointStates::Result>();
-
-    moveit::planning_interface::MoveGroupInterface move_group_interface(
-        node_, goal->robot_name);
-    move_group_interface.startStateMonitor();
-
-    std::vector<double> joint_states = goal->joint_values;
-    for (auto &joint : joint_states) {
-      RCLCPP_INFO(this->get_logger(), "Joint value %f", joint);
-    }
-
-    move_group_interface.setJointValueTarget(goal->joint_values);
-    move_group_interface.setMaxVelocityScalingFactor(
-        goal->joints_velocity_scaling_factor);
-    move_group_interface.setMaxAccelerationScalingFactor(
-        goal->joints_acceleration_scaling_factor);
-
-    moveit::planning_interface::MoveGroupInterface::Plan plan;
-    bool success = (move_group_interface.plan(plan) ==
-                    moveit::core::MoveItErrorCode::SUCCESS);
-    if (success) {
-      RCLCPP_INFO(this->get_logger(), "Planning success");
-      move_group_interface.execute(plan);
-      // move_group_interface.move();
-    } else {
-      RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
-    }
-
-    if (goal_handle->is_canceling()) {
-      goal_handle->canceled(result);
-      RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
-      return;
-    }
-
-    goal_handle->succeed(result);
-    RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
-  }
-}; // class MoveToJointStateActionServer
-
-} // namespace rbs_skill_actions
-
-int main(int argc, char **argv) {
-  rclcpp::init(argc, argv);
-  rclcpp::NodeOptions node_options;
-  node_options.automatically_declare_parameters_from_overrides(true);
-  auto node =
-      rclcpp::Node::make_shared("move_to_joint_states", "", node_options);
-
-  rbs_skill_actions::MoveToJointStateActionServer server(node);
-  std::thread run_server([&server]() {
-    rclcpp::sleep_for(std::chrono::seconds(3));
-    server.init();
-  });
-
-  rclcpp::spin(node);
-  run_server.join();
-
-  return 0;
-}

rbs_skill_servers/src/mtjs_jtc.cpp

@@ -0,0 +1,155 @@
+#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+#include <memory>
+#include <rbs_skill_interfaces/action/detail/moveit_send_joint_states__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp_action/client.hpp>
+#include <rclcpp_action/client_goal_handle.hpp>
+#include <rclcpp_action/create_client.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <string>
+#include <vector>
+
+namespace rbs_skill_actions {
+
+using MoveitSendJointStates =
+    rbs_skill_interfaces::action::MoveitSendJointStates;
+using GoalHandleMoveitSendJointStates =
+    rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
+using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
+using FollowJointTrajectoryGoalHandle =
+    rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
+
+class MoveToJointStateActionServer : public SkillBase<MoveitSendJointStates> {
+public:
+  explicit MoveToJointStateActionServer(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase<MoveitSendJointStates>("mtjs_jtc", options) {
+
+    m_joint_trajectory_client =
+        rclcpp_action::create_client<FollowJointTrajectory>(
+            this, "/joint_trajectory_controller/follow_joint_trajectory");
+
+    auto cbg =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    rclcpp::SubscriptionOptions s_options;
+    s_options.callback_group = cbg;
+
+    m_joint_state_subscriber =
+        this->create_subscription<sensor_msgs::msg::JointState>(
+            "/joint_states", 10,
+            std::bind(&MoveToJointStateActionServer::jointStateCallback, this,
+                      std::placeholders::_1),
+            s_options);
+  }
+
+protected:
+  std::string requiredActionController() override {
+    return "joint_trajectory_controller";
+  }
+
+  std::vector<std::string> requiredParameters() override { return {"joints"}; }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
+
+    auto trajectory_goal = FollowJointTrajectory::Goal();
+    auto joints = this->get_parameters(requiredParameters());
+    m_joint_names = joints.at(0).as_string_array();
+
+    trajectory_goal.trajectory.joint_names = m_joint_names;
+
+    // TODO: Better sync solution
+    while (m_current_joint_positions.empty()) {
+    }
+
+    trajectory_goal.trajectory.points = generate_trajectory(
+        m_current_joint_positions, m_current_goal->joint_values,
+        m_current_goal->duration);
+
+    auto send_goal_options =
+        rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
+    send_goal_options.result_callback =
+        [this](const FollowJointTrajectoryGoalHandle::WrappedResult
+                   &wrapped_result) {
+          if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
+            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+            m_current_goal_handle->succeed(m_current_result);
+          } else {
+            RCLCPP_ERROR(this->get_logger(), "Goal failed");
+            m_current_goal_handle->abort(m_current_result);
+          }
+        };
+
+    m_joint_trajectory_client->async_send_goal(trajectory_goal,
+                                               send_goal_options);
+  }
+
+private:
+  void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
+    if (m_joint_names.empty()) {
+      return;
+    }
+    // RCLCPP_INFO(this->get_logger(), "Called joint positions");
+
+    if (m_joint_mame_to_index.empty()) {
+      m_joint_mame_to_index.reserve(m_joint_names.size());
+      for (size_t j = 0; j < m_joint_names.size(); ++j) {
+        auto it =
+            std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
+        if (it != msg->name.end()) {
+          size_t index = std::distance(msg->name.begin(), it);
+          m_joint_mame_to_index[m_joint_names[j]] = index;
+        }
+      }
+    }
+
+    if (m_current_joint_positions.size() != m_joint_names.size()) {
+      m_current_joint_positions.resize(m_joint_names.size(), 0.0);
+    }
+
+    for (size_t j = 0; j < m_joint_names.size(); ++j) {
+      auto index_it = m_joint_mame_to_index.find(m_joint_names[j]);
+      if (index_it != m_joint_mame_to_index.end()) {
+        m_current_joint_positions[j] = msg->position[index_it->second];
+      }
+    }
+  }
+
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
+  generate_trajectory(const std::vector<double> &start_joint_values,
+                      const std::vector<double> &target_joint_values,
+                      const double duration) {
+
+    const int num_points = 100;
+    std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
+    for (int i = 0; i <= num_points; ++i) {
+      trajectory_msgs::msg::JointTrajectoryPoint point;
+      double t = static_cast<double>(i) / num_points;
+      for (size_t j = 0; j < target_joint_values.size(); ++j) {
+        double position = start_joint_values[j] +
+                          t * (target_joint_values[j] - start_joint_values[j]);
+        point.positions.push_back(position);
+      }
+      point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
+      points.push_back(point);
+    }
+    return points;
+  }
+
+  rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
+      m_joint_trajectory_client;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
+      m_joint_state_subscriber;
+
+  std::vector<double> m_current_joint_positions;
+  std::unordered_map<std::string, size_t> m_joint_mame_to_index;
+  std::vector<std::string> m_joint_names;
+};
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(
+    rbs_skill_actions::MoveToJointStateActionServer);

rbs_skill_servers/src/mtp_cart.cpp

@@ -0,0 +1,183 @@
+#include "Eigen/Dense"
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/logging.hpp>
+
+// Action libs
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+
+namespace rbs_skill_actions {
+
+using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+using GoalHandleMoveitSendPose =
+    rclcpp_action::ServerGoalHandle<MoveitSendPose>;
+
+class CartesianMoveToPose : public SkillBase<MoveitSendPose> {
+public:
+  explicit CartesianMoveToPose(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase<MoveitSendPose>("mtp_cart", options), m_current_step(0) {
+
+    // Initialize publisher for target poses
+    auto cbg =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    rclcpp::PublisherOptions p_options;
+    p_options.callback_group = cbg;
+
+    m_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>(
+        "cartesian_motion_controller/target_frame", 10, p_options);
+
+    m_current_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
+
+    rclcpp::SubscriptionOptions s_options;
+    s_options.callback_group = cbg;
+
+    m_current_pose_subscriber =
+        this->create_subscription<geometry_msgs::msg::PoseStamped>(
+            "cartesian_motion_controller/current_pose", 10,
+            std::bind(&CartesianMoveToPose::on_pose_callback, this,
+                      std::placeholders::_1),
+            s_options);
+  }
+
+protected:
+  std::string requiredActionController() override {
+    return "cartesian_motion_controller";
+  }
+
+  std::vector<std::string> requiredParameters() override {
+    return {"end_effector_link", "robot_base_link"};
+  }
+
+  void executeAction() override {
+    m_base_link = this->get_parameter("robot_base_link").as_string();
+
+    m_trajectory =
+        interpolate(m_current_pose->pose, m_current_goal->target_pose);
+    m_current_step = 0;
+
+    m_total_time = m_current_goal->duration;
+    m_step_time =
+        m_total_time / static_cast<double>(m_trajectory.size()) * 1000.0;
+
+    m_trajectory_timer = this->create_wall_timer(
+        std::chrono::milliseconds(static_cast<int>(m_step_time)),
+        [this]() { adjust_and_publish_pose(); });
+  }
+
+private:
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_publisher;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
+      m_current_pose_subscriber;
+  geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose;
+  std::string m_base_link;
+  double m_threshold_distance = 0.02;
+
+  std::vector<geometry_msgs::msg::Pose> m_trajectory;
+  size_t m_current_step;
+  rclcpp::TimerBase::SharedPtr m_trajectory_timer;
+  double m_total_time;
+  double m_step_time;
+
+  void adjust_and_publish_pose() {
+    if (m_current_step < m_trajectory.size()) {
+      publish_pose(m_trajectory[m_current_step++]);
+    } else {
+      double distance =
+          calculate_distance(m_current_pose->pose, m_current_goal->target_pose);
+      if (distance <= m_threshold_distance) {
+        m_current_result->success = true;
+        m_current_goal_handle->succeed(m_current_result);
+        RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
+        m_trajectory_timer->cancel();
+      } else {
+        publish_pose(m_trajectory.back());
+      }
+    }
+  }
+
+  double calculate_distance(const geometry_msgs::msg::Pose &p1,
+                            const geometry_msgs::msg::Pose &p2) {
+    Eigen::Vector3d pos1(p1.position.x, p1.position.y, p1.position.z);
+    Eigen::Vector3d pos2(p2.position.x, p2.position.y, p2.position.z);
+
+    double position_distance = (pos1 - pos2).norm();
+
+    Eigen::Quaterniond orient1 = normalize_orientation(p1.orientation);
+    Eigen::Quaterniond orient2 = normalize_orientation(p2.orientation);
+
+    return position_distance + orient1.angularDistance(orient2);
+  }
+
+  Eigen::Quaterniond
+  normalize_orientation(const geometry_msgs::msg::Quaternion &q) {
+    Eigen::Quaterniond orientation(q.w, q.x, q.y, q.z);
+    orientation.normalize();
+
+    if (orientation.norm() == 0) {
+      RCLCPP_FATAL(this->get_logger(),
+                   "Quaternion has zero norm; normalization failed.");
+      throw std::runtime_error("Quaternion normalization failed.");
+    }
+
+    return orientation;
+  }
+
+  std::vector<geometry_msgs::msg::Pose>
+  interpolate(const geometry_msgs::msg::Pose &start_pose,
+              const geometry_msgs::msg::Pose &end_pose) {
+    std::vector<geometry_msgs::msg::Pose> trajectory;
+    Eigen::Vector3d start_position(start_pose.position.x, start_pose.position.y,
+                                   start_pose.position.z);
+    Eigen::Vector3d end_position(end_pose.position.x, end_pose.position.y,
+                                 end_pose.position.z);
+
+    Eigen::Quaterniond start_orientation =
+        normalize_orientation(start_pose.orientation);
+    Eigen::Quaterniond end_orientation =
+        normalize_orientation(end_pose.orientation);
+
+    double distance = (end_position - start_position).norm();
+    int num_steps = static_cast<int>(distance / 0.01);
+
+    for (int i = 0; i <= num_steps; ++i) {
+      double t = static_cast<double>(i) / num_steps;
+
+      Eigen::Vector3d interpolated_position =
+          start_position + t * (end_position - start_position);
+      Eigen::Quaterniond interpolated_orientation =
+          start_orientation.slerp(t, end_orientation);
+
+      geometry_msgs::msg::Pose interpolated_pose;
+      interpolated_pose.position.x = interpolated_position.x();
+      interpolated_pose.position.y = interpolated_position.y();
+      interpolated_pose.position.z = interpolated_position.z();
+      interpolated_pose.orientation.x = interpolated_orientation.x();
+      interpolated_pose.orientation.y = interpolated_orientation.y();
+      interpolated_pose.orientation.z = interpolated_orientation.z();
+      interpolated_pose.orientation.w = interpolated_orientation.w();
+
+      trajectory.push_back(interpolated_pose);
+    }
+
+    return trajectory;
+  }
+
+  void publish_pose(const geometry_msgs::msg::Pose &pose) {
+    geometry_msgs::msg::PoseStamped tp;
+    tp.pose = pose;
+    tp.header.stamp = this->now();
+    tp.header.frame_id = m_base_link;
+    m_publisher->publish(tp);
+  }
+
+  void on_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+    m_current_pose = msg;
+  }
+};
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::CartesianMoveToPose);

rbs_skill_servers/src/mtp_jtc.cpp

@@ -0,0 +1,224 @@
+#include "Eigen/Dense"
+#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "kdl/chainiksolverpos_lma.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+#include <cstddef>
+#include <iterator>
+#include <kdl/chain.hpp>
+#include <kdl/chainiksolver.hpp>
+#include <kdl/chainiksolvervel_wdls.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/frames.hpp>
+#include <kdl/jntarray.hpp>
+#include <kdl/tree.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <memory>
+#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/parameter_client.hpp>
+#include <rclcpp_action/client.hpp>
+#include <rclcpp_action/client_goal_handle.hpp>
+#include <rclcpp_action/create_client.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <string>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <vector>
+
+namespace rbs_skill_actions {
+
+using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+using GoalHandleMoveitSendJointStates =
+    rclcpp_action::ServerGoalHandle<MoveitSendPose>;
+using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
+using FollowJointTrajectoryGoalHandle =
+    rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
+
+class MoveToPose : public SkillBase<MoveitSendPose> {
+public:
+  explicit MoveToPose(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase<MoveitSendPose>("mtp_jtc", options) {
+
+    m_joint_trajectory_client =
+        rclcpp_action::create_client<FollowJointTrajectory>(
+            this, "/joint_trajectory_controller/follow_joint_trajectory");
+
+    auto cbg =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    rclcpp::SubscriptionOptions s_options;
+    s_options.callback_group = cbg;
+
+    m_joint_state_subscriber =
+        this->create_subscription<sensor_msgs::msg::JointState>(
+            "/joint_states", 10,
+            std::bind(&MoveToPose::jointStateCallback, this,
+                      std::placeholders::_1),
+            s_options);
+    this->declare_parameter("robot_description", "");
+  }
+
+protected:
+  std::string requiredActionController() override {
+    return "joint_trajectory_controller";
+  }
+
+  std::vector<std::string> requiredParameters() override { return {"joints"}; }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
+
+    auto trajectory_goal = FollowJointTrajectory::Goal();
+    auto joints = this->get_parameters(requiredParameters());
+    m_joint_names = joints.at(0).as_string_array();
+
+    trajectory_goal.trajectory.joint_names = m_joint_names;
+
+    // TODO: Better sync solution
+    while (m_current_joint_positions.empty()) {
+    }
+
+    std::vector<double> target_joint_values;
+    solveIK(target_joint_values);
+
+    trajectory_goal.trajectory.points =
+        generate_trajectory(m_current_joint_positions, target_joint_values,
+                            m_current_goal->duration);
+
+    auto send_goal_options =
+        rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
+    send_goal_options.result_callback =
+        [this](const FollowJointTrajectoryGoalHandle::WrappedResult
+                   &wrapped_result) {
+          if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
+            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+            m_current_goal_handle->succeed(m_current_result);
+          } else {
+            RCLCPP_ERROR(this->get_logger(), "Goal failed");
+            m_current_goal_handle->abort(m_current_result);
+          }
+        };
+
+    m_joint_trajectory_client->async_send_goal(trajectory_goal,
+                                               send_goal_options);
+  }
+
+private:
+  void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
+    if (m_joint_names.empty()) {
+      return;
+    }
+    RCLCPP_WARN(this->get_logger(), "Called joint positions");
+
+    if (m_joint_mame_to_index.empty()) {
+      m_joint_mame_to_index.reserve(m_joint_names.size());
+      for (size_t j = 0; j < m_joint_names.size(); ++j) {
+        auto it =
+            std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
+        if (it != msg->name.end()) {
+          size_t index = std::distance(msg->name.begin(), it);
+          m_joint_mame_to_index[m_joint_names[j]] = index;
+        }
+      }
+    }
+
+    if (m_current_joint_positions.size() != m_joint_names.size()) {
+      m_current_joint_positions.resize(m_joint_names.size(), 0.0);
+    }
+
+    for (size_t j = 0; j < m_joint_names.size(); ++j) {
+      auto index_it = m_joint_mame_to_index.find(m_joint_names[j]);
+      if (index_it != m_joint_mame_to_index.end()) {
+        m_current_joint_positions[j] = msg->position[index_it->second];
+      }
+    }
+  }
+
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
+  generate_trajectory(const std::vector<double> &start_joint_values,
+                      const std::vector<double> &target_joint_values,
+                      const double duration) {
+
+    const int num_points = 100;
+    std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
+    for (int i = 0; i <= num_points; ++i) {
+      trajectory_msgs::msg::JointTrajectoryPoint point;
+      double t = static_cast<double>(i) / num_points;
+      for (size_t j = 0; j < target_joint_values.size(); ++j) {
+        double position = start_joint_values[j] +
+                          t * (target_joint_values[j] - start_joint_values[j]);
+        point.positions.push_back(position);
+      }
+      point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
+      points.push_back(point);
+    }
+    return points;
+  }
+
+  void solveIK(std::vector<double> &out) {
+    KDL::JntArray q_in(m_joint_names.size());
+    for (size_t i = 0; i < m_joint_names.size(); ++i) {
+      q_in(i) = m_current_joint_positions[i];
+    }
+
+    KDL::JntArray q_out(m_joint_names.size());
+
+    Eigen::Affine3d target_pose;
+    Eigen::fromMsg(m_current_goal->target_pose, target_pose);
+
+    KDL::Frame target_pose_kdl(
+        KDL::Rotation(target_pose(0, 0), target_pose(0, 1), target_pose(0, 2),
+                      target_pose(1, 0), target_pose(1, 1), target_pose(1, 2),
+                      target_pose(2, 0), target_pose(2, 1), target_pose(2, 2)),
+        KDL::Vector(target_pose.translation().x(),
+                    target_pose.translation().y(),
+                    target_pose.translation().z()));
+
+    const std::string m_base_link = "base_link";
+    const std::string m_ee_link = "gripper_grasp_point";
+
+    auto robot_description =
+        this->get_parameter("robot_description").as_string();
+
+    KDL::Tree kdl_tree;
+    if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Failed to parse KDL tree from robot description.");
+      return;
+    }
+
+    KDL::Chain kdl_chain;
+    if (!kdl_tree.getChain(m_base_link, m_ee_link, kdl_chain)) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Failed to get KDL chain from base to end-effector.");
+      return;
+    }
+
+    auto ik_solver =
+        std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_chain, 1e-5, 500);
+
+    if (ik_solver->CartToJnt(q_in, target_pose_kdl, q_out) >= 0) {
+      out.resize(q_out.rows());
+      for (size_t i = 0; i < out.size(); i++) {
+        out[i] = q_out(i);
+      }
+    } else {
+      RCLCPP_ERROR(this->get_logger(), "IK solution not found.");
+    }
+  }
+
+  rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
+      m_joint_trajectory_client;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
+      m_joint_state_subscriber;
+
+  std::vector<double> m_current_joint_positions;
+  std::unordered_map<std::string, size_t> m_joint_mame_to_index;
+  std::vector<std::string> m_joint_names;
+};
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPose);

rbs_skill_servers/src/mtp_jtc_cart.cpp

@@ -0,0 +1,324 @@
+#include "Eigen/Dense"
+#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainiksolverpos_lma.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+#include <Eigen/src/Geometry/Transform.h>
+#include <cstddef>
+#include <iterator>
+#include <kdl/chain.hpp>
+#include <kdl/chainiksolver.hpp>
+#include <kdl/chainiksolvervel_wdls.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/frames.hpp>
+#include <kdl/jntarray.hpp>
+#include <kdl/tree.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <memory>
+#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/parameter_client.hpp>
+#include <rclcpp_action/client.hpp>
+#include <rclcpp_action/client_goal_handle.hpp>
+#include <rclcpp_action/create_client.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <string>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <vector>
+
+namespace rbs_skill_actions {
+
+using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+using GoalHandleMoveitSendJointStates =
+    rclcpp_action::ServerGoalHandle<MoveitSendPose>;
+using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
+using FollowJointTrajectoryGoalHandle =
+    rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
+
+class MoveToPoseJtcCartesian : public SkillBase<MoveitSendPose> {
+public:
+  explicit MoveToPoseJtcCartesian(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase<MoveitSendPose>("mtp_jtc_cart", options) {
+
+    m_joint_trajectory_client =
+        rclcpp_action::create_client<FollowJointTrajectory>(
+            this, "/joint_trajectory_controller/follow_joint_trajectory");
+
+    auto cbg =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    rclcpp::SubscriptionOptions s_options;
+    s_options.callback_group = cbg;
+
+    m_joint_state_subscriber =
+        this->create_subscription<sensor_msgs::msg::JointState>(
+            "/joint_states", 10,
+            std::bind(&MoveToPoseJtcCartesian::jointStateCallback, this,
+                      std::placeholders::_1),
+            s_options);
+    this->declare_parameter("robot_description", "");
+    this->declare_parameter("base_link", "");
+    this->declare_parameter("robot_ee_link", "");
+
+    auto robot_description =
+        this->get_parameter("robot_description").as_string();
+    KDL::Tree kdl_tree;
+    if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Failed to obtain KDL tree from the robot description.");
+
+      throw std::runtime_error("KDL Tree initialization failed");
+    }
+
+    auto base_link = this->get_parameter("base_link").as_string();
+    auto robot_ee_link = this->get_parameter("robot_ee_link").as_string();
+    if (base_link.empty() or robot_ee_link.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Describe robot end-effector link and base link to continue");
+      throw std::runtime_error("Describe robot end-effector link and base link to continue");
+    }
+
+    if (!kdl_tree.getChain(base_link, robot_ee_link, m_kdl_chain)) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Failed to obtain KDL chain from base to end-effector.");
+      throw std::runtime_error("KDL Chain initialization failed");
+    }
+  }
+
+protected:
+  std::string requiredActionController() override {
+    return "joint_trajectory_controller";
+  }
+
+  std::vector<std::string> requiredParameters() override { return {"joints"}; }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
+
+    auto trajectory_goal = FollowJointTrajectory::Goal();
+    auto joints = this->get_parameters(requiredParameters());
+    m_joint_names = joints.at(0).as_string_array();
+
+    trajectory_goal.trajectory.joint_names = m_joint_names;
+
+    const int max_wait_iterations = 100;
+    int wait_count = 0;
+    while (m_current_joint_positions.empty() &&
+           wait_count++ < max_wait_iterations) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    if (m_current_joint_positions.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Joint positions were not received in time");
+      return;
+    }
+
+    std::vector<double> target_joint_values;
+    Eigen::Affine3d target_pose;
+    Eigen::fromMsg(m_current_goal->target_pose, target_pose);
+    if (!solveIK(target_pose, target_joint_values)) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "IK solution not found for goal position");
+      m_current_goal_handle->abort(m_current_result);
+    }
+
+    trajectory_goal.trajectory.points =
+        generate_trajectory(m_current_joint_positions, target_joint_values,
+                            m_current_goal->duration);
+
+    auto send_goal_options =
+        rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
+    send_goal_options.result_callback =
+        [this](const FollowJointTrajectoryGoalHandle::WrappedResult
+                   &wrapped_result) {
+          if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
+            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+            m_current_goal_handle->succeed(m_current_result);
+          } else {
+            RCLCPP_ERROR(this->get_logger(), "Goal failed");
+            m_current_goal_handle->abort(m_current_result);
+          }
+        };
+
+    m_joint_trajectory_client->async_send_goal(trajectory_goal,
+                                               send_goal_options);
+  }
+
+private:
+  void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
+    if (m_joint_names.empty()) {
+      return;
+    }
+    if (m_joint_name_to_index.empty()) {
+      m_joint_name_to_index.reserve(m_joint_names.size());
+      for (size_t j = 0; j < m_joint_names.size(); ++j) {
+        auto it =
+            std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
+        if (it != msg->name.end()) {
+          size_t index = std::distance(msg->name.begin(), it);
+          m_joint_name_to_index[m_joint_names[j]] = index;
+        }
+      }
+    }
+
+    if (m_current_joint_positions.size() != m_joint_names.size()) {
+      m_current_joint_positions.resize(m_joint_names.size(), 0.0);
+    }
+
+    for (size_t j = 0; j < m_joint_names.size(); ++j) {
+      auto index_it = m_joint_name_to_index.find(m_joint_names[j]);
+      if (index_it != m_joint_name_to_index.end()) {
+        m_current_joint_positions[j] = msg->position[index_it->second];
+      }
+    }
+  }
+
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
+  generate_trajectory(const std::vector<double> &start_joint_values,
+                      const std::vector<double> &target_joint_values,
+                      const double duration) {
+    const int num_points = 100;
+    std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
+
+    KDL::Frame start_pose_kdl, target_pose_kdl;
+    if (!getEndEffectorPose(start_joint_values, start_pose_kdl) ||
+        !getEndEffectorPose(target_joint_values, target_pose_kdl)) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Failed to get initial or target end-effector pose");
+      return points;
+    }
+
+    Eigen::Affine3d start_pose = KDLFrameToEigen(start_pose_kdl);
+    Eigen::Affine3d target_pose = KDLFrameToEigen(target_pose_kdl);
+
+    for (int i = 0; i <= num_points; ++i) {
+      trajectory_msgs::msg::JointTrajectoryPoint point;
+      double t = static_cast<double>(i) / num_points;
+
+      Eigen::Vector3d start_translation = start_pose.translation();
+      Eigen::Vector3d target_translation = target_pose.translation();
+      Eigen::Vector3d interpolated_translation =
+          (1.0 - t) * start_translation + t * target_translation;
+      Eigen::Translation3d interpolated_translation_transform(
+          interpolated_translation);
+
+      Eigen::Quaterniond start_quaternion(start_pose.rotation());
+      Eigen::Quaterniond target_quaternion(target_pose.rotation());
+
+      Eigen::Quaterniond interpolated_quaternion =
+          start_quaternion.slerp(t, target_quaternion);
+
+      Eigen::Affine3d interpolated_pose =
+          interpolated_translation_transform * interpolated_quaternion;
+
+      std::vector<double> ik_joint_values;
+      if (!solveIK(interpolated_pose, ik_joint_values)) {
+        RCLCPP_ERROR(this->get_logger(), "IK solution not found for point %d",
+                     i);
+        return {};
+      }
+
+      point.positions = ik_joint_values;
+      point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
+      points.push_back(point);
+    }
+    return points;
+  }
+
+  bool getEndEffectorPose(const std::vector<double> &joint_positions,
+                          KDL::Frame &end_effector_pose) {
+
+    if (joint_positions.size() != m_joint_names.size()) {
+      RCLCPP_ERROR(this->get_logger(), "Mismatch between provided joint "
+                                       "positions and expected joint names.");
+      RCLCPP_ERROR(this->get_logger(), "Joint pos size: %zu",
+                   joint_positions.size());
+
+      RCLCPP_ERROR(this->get_logger(), "Joint names size: %zu",
+                   m_joint_names.size());
+
+      return false;
+    }
+
+    std::unordered_set<std::string> available_joints(m_joint_names.begin(),
+                                                     m_joint_names.end());
+    if (available_joints.size() != m_joint_names.size()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Duplicate joints detected in joint names list.");
+      return false;
+    }
+
+    KDL::JntArray q_in(joint_positions.size());
+    for (size_t i = 0; i < joint_positions.size(); ++i) {
+      q_in(i) = joint_positions[i];
+    }
+
+    KDL::ChainFkSolverPos_recursive fk_solver(m_kdl_chain);
+    if (fk_solver.JntToCart(q_in, end_effector_pose) < 0) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Failed to compute FK for the specified joint positions.");
+      return false;
+    }
+    return true;
+  }
+
+  Eigen::Affine3d KDLFrameToEigen(const KDL::Frame &frame) {
+    Eigen::Affine3d transform;
+    for (int i = 0; i < 3; ++i) {
+      for (int j = 0; j < 3; ++j) {
+        transform(i, j) = frame.M(i, j);
+      }
+      transform(i, 3) = frame.p[i];
+    }
+    transform(3, 3) = 1.0;
+    return transform;
+  }
+
+  bool solveIK(const Eigen::Affine3d &target_pose, std::vector<double> &out) {
+    KDL::JntArray q_in(m_joint_names.size());
+    for (size_t i = 0; i < m_joint_names.size(); ++i) {
+      q_in(i) = m_current_joint_positions[i];
+    }
+
+    KDL::JntArray q_out(m_joint_names.size());
+
+    KDL::Frame target_pose_kdl(
+        KDL::Rotation(target_pose(0, 0), target_pose(0, 1), target_pose(0, 2),
+                      target_pose(1, 0), target_pose(1, 1), target_pose(1, 2),
+                      target_pose(2, 0), target_pose(2, 1), target_pose(2, 2)),
+        KDL::Vector(target_pose.translation().x(),
+                    target_pose.translation().y(),
+                    target_pose.translation().z()));
+
+    auto ik_solver =
+        std::make_unique<KDL::ChainIkSolverPos_LMA>(m_kdl_chain, 1e-5, 500);
+
+    if (ik_solver->CartToJnt(q_in, target_pose_kdl, q_out) >= 0) {
+      out.resize(q_out.rows());
+      for (size_t i = 0; i < out.size(); i++) {
+        out[i] = q_out(i);
+      }
+      return true;
+    } else {
+      RCLCPP_ERROR(this->get_logger(), "IK solution not found.");
+      return false;
+    }
+  }
+
+  rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
+      m_joint_trajectory_client;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
+      m_joint_state_subscriber;
+
+  std::vector<double> m_current_joint_positions;
+  std::unordered_map<std::string, size_t> m_joint_name_to_index;
+  std::vector<std::string> m_joint_names;
+  KDL::Chain m_kdl_chain;
+};
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPoseJtcCartesian);

rbs_skill_servers/src/mtp_moveit.cpp

@@ -0,0 +1,68 @@
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+
+namespace rbs_skill_actions {
+
+using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+
+class MoveitMtp : public SkillBase<MoveitSendPose> {
+public:
+  explicit MoveitMtp(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase("mtp_moveit", options) {}
+
+private:
+  std::string requiredActionController() override {
+    return "joint_trajectory_controller";
+  }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Executing action goal");
+
+    moveit::planning_interface::MoveGroupInterface move_group_interface(
+        this->shared_from_this(), m_current_goal->robot_name);
+    move_group_interface.startStateMonitor();
+
+    move_group_interface.setPoseTarget(m_current_goal->target_pose);
+    move_group_interface.setMaxVelocityScalingFactor(
+        m_current_goal->end_effector_velocity);
+    move_group_interface.setMaxAccelerationScalingFactor(
+        m_current_goal->end_effector_acceleration);
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    moveit::core::MoveItErrorCode plan_err_code =
+        move_group_interface.plan(plan);
+    if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) {
+      move_group_interface.plan(plan);
+    }
+    if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
+      RCLCPP_INFO(this->get_logger(), "Planning success");
+      // move_group_interface.execute(plan);
+      moveit::core::MoveItErrorCode move_err_code =
+          move_group_interface.execute(plan);
+      if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
+        m_current_result->success = true;
+        m_current_goal_handle->succeed(m_current_result);
+        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
+      } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) {
+        RCLCPP_ERROR(this->get_logger(), "Failure in move:");
+      }
+
+    } else {
+      RCLCPP_WARN_STREAM(this->get_logger(),
+                         "Failed to generate plan because "
+                             << error_code_to_string(plan_err_code));
+      m_current_goal_handle->abort(m_current_result);
+    }
+
+    if (m_current_goal_handle->is_canceling()) {
+      m_current_goal_handle->canceled(m_current_result);
+      RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
+      return;
+    }
+  }
+}; // class MoveToPoseActionServer
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtp);

rbs_skill_servers/src/mtp_moveit_cart.cpp

@@ -0,0 +1,95 @@
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+#include <rclcpp/logging.hpp>
+
+namespace rbs_skill_actions {
+
+using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+
+class MoveitMtpCart : public SkillBase<MoveitSendPose> {
+public:
+  explicit MoveitMtpCart(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase("mtp_moveit_cart", options) {}
+
+private:
+  std::string requiredActionController() override {
+    return "joint_trajectory_controller";
+  }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Executing action goal");
+
+    moveit::planning_interface::MoveGroupInterface move_group_interface(
+        this->shared_from_this(), m_current_goal->robot_name);
+    move_group_interface.startStateMonitor();
+
+    moveit::core::RobotState start_state(
+        *move_group_interface.getCurrentState());
+
+    std::vector<geometry_msgs::msg::Pose> waypoints;
+    auto current_pose = move_group_interface.getCurrentPose();
+    geometry_msgs::msg::Pose target_pose = m_current_goal->target_pose;
+    waypoints.push_back(target_pose);
+
+    RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
+                target_pose.position.x, target_pose.position.y,
+                target_pose.position.z);
+
+    moveit_msgs::msg::RobotTrajectory trajectory;
+    const double jump_threshold = 0.0;
+    const double eef_step = 0.001;
+    double fraction = move_group_interface.computeCartesianPath(
+        waypoints, eef_step, jump_threshold, trajectory);
+
+    robot_trajectory::RobotTrajectory rt(
+        move_group_interface.getCurrentState()->getRobotModel(),
+        m_current_goal->robot_name);
+
+    rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
+
+    trajectory_processing::TimeOptimalTrajectoryGeneration tp;
+
+    bool su = tp.computeTimeStamps(rt);
+    if (!su) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to compute timestamp of trajectory");
+      m_current_goal_handle->abort(m_current_result);
+    }
+
+    rt.getRobotTrajectoryMsg(trajectory);
+
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    plan.trajectory_ = trajectory;
+
+    if (fraction > 0) {
+      RCLCPP_INFO(this->get_logger(), "Planning success");
+      moveit::core::MoveItErrorCode execute_err_code =
+          move_group_interface.execute(plan);
+      if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
+        m_current_result->success = true;
+        m_current_goal_handle->succeed(m_current_result);
+        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
+      } else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
+        RCLCPP_ERROR(this->get_logger(), "Failure in move:");
+      }
+
+      // move_group_interface.move();
+    } else {
+      RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
+      m_current_goal_handle->abort(m_current_result);
+    }
+
+    if (m_current_goal_handle->is_canceling()) {
+      m_current_goal_handle->canceled(m_current_result);
+      RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
+      return;
+    }
+  }
+}; // class MoveToPoseActionServer
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtpCart);

rbs_skill_servers/src/mtp_servo.cpp

@@ -1,3 +1,4 @@
+// FIXME: This code currently not used
 #include <functional>
 #include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
 #include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>

rbs_utils/rbs_utils/package.xml

@@ -21,7 +21,7 @@
   <depend>rbs_utils_interfaces</depend>
   <depend>dynmsg</depend>
   <depend>sensor_msgs</depend>
-  <depend>nlohmann_json</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>rosbag2_cpp</depend>
   
   <test_depend>ament_lint_auto</test_depend>

repos/all-deps.repos

@@ -0,0 +1,37 @@
+repositories:
+  rbs_arm:
+    type: git
+    url: https://github.com/solid-sinusoid/rbs-arm.git
+    version: main
+  robot_builder:
+    type: git
+    url: https://github.com/solid-sinusoid/robot-builder.git
+    version: main
+  rbs_gripper:
+    type: git
+    url: https://github.com/solid-sinusoid/rbs-gripper.git
+    version: main
+  behavior_tree:
+    type: git
+    url: https://github.com/BehaviorTree/BehaviorTree.ROS2.git
+    version: humble
+  dynamic_message_introspection:
+    type: git
+    url: https://github.com/osrf/dynamic_message_introspection.git
+    version: main
+  robot_builder:
+    type: git
+    url: https://github.com/solid-sinusoid/robot-builder.git
+    version: main
+  cartesian_controllers:
+    type: git
+    url: https://github.com/solid-sinusoid/cartesian_controllers.git
+    version: gazebo-simulation
+  ros2_control:
+    type: git
+    url: https://github.com/solid-sinusoid/ros2_control.git
+    version: gz-ros2-cartesian-controllers
+  gz_ros2_control:
+    type: git
+    url: https://github.com/solid-sinusoid/gz_ros2_control.git
+    version: fts-sensor

repos/requirements.txt

@@ -0,0 +1,29 @@
+trimesh
+pcg-gazebo
+loguru
+# markupsafe==2.0.1
+# Jinja2==2.6
+
+numpy==1.24.0
+flask==3.0.3
+Jinja2>=3.1.2
+docutils<0.18,>=0.15
+markdown-it-py<3.0.0,>=1.0.0
+# setuptools-scm
+
+dacite>=1.8.1
+gymnasium==0.29.1
+numpy>=1.24.0
+open3d>=0.18.0
+scipy>=1.14.1
+tensorflow>=2.17.0
+torch>=2.4.1
+torchvision>=0.19.1
+trimesh>=4.4.9
+wandb>=0.18.6
+
+# Packages from devpi custom repository
+scenario @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/63e/86b0583c22e52/scenario-1.4.0-cp310-cp310-linux_x86_64.whl#sha256=63e86b0583c22e52c299b9a25e74a26178fc7dbc423f661bf34e4dd26543d11f
+gym-gz @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/539/1f448c1391486/gym_gz-1.4.0-py3-none-any.whl#sha256=5391f448c13914860e2a65904f6d0fac7eba71daaa3327d8399d7ae516471a68
+gym-gz-models @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/f42/4784934183e88/gym_gz_models-1.2.0-cp310-cp310-linux_x86_64.whl#sha256=f424784934183e88541c703c789315ba6118661bd221a4bf9135c6a0ee012e96
+rbs_asstss_library @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/165/dd2476d9dc455/rbs_assets_library-0.1.0-py2.py3-none-any.whl#sha256=165dd2476d9dc455b425a8c8d3e09a9a9541d54c1f900a3a0e63526c6118de6d