Merge branch 'main' into 109-nix-jazzy
This commit is contained in:
commit
ec2cebe5f1
46 changed files with 2696 additions and 1844 deletions
69
Dockerfile
69
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 \
|
||||
gnupg
|
||||
curl \
|
||||
wget
|
||||
|
||||
RUN pip install vcstool
|
||||
|
||||
WORKDIR /libs
|
||||
RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\
|
||||
tar -xf v3.11.3.tar.gz &&\
|
||||
cd json-3.11.3 &&\
|
||||
mkdir build &&\
|
||||
cd build &&\
|
||||
cmake .. &&\
|
||||
make &&\
|
||||
make install
|
||||
# WORKDIR /libs
|
||||
# RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\
|
||||
# tar -xf v3.11.3.tar.gz &&\
|
||||
# cd json-3.11.3 &&\
|
||||
# mkdir build &&\
|
||||
# cd build &&\
|
||||
# cmake .. &&\
|
||||
# make &&\
|
||||
# make install
|
||||
|
||||
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 .
|
||||
RUN add-apt-repository universe
|
||||
RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
|
||||
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
|
||||
|
||||
|
||||
RUN apt-get update && apt-get upgrade && apt-get install -y ros-humble-ros-base
|
||||
|
||||
# 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 apt update && rosdep update && \
|
||||
RUN pip install vcstool uv
|
||||
|
||||
# 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}
|
||||
|
|
|
|||
110
doc/en/add_new_robot.md
Normal file
110
doc/en/add_new_robot.md
Normal file
|
|
@ -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`.
|
||||
18
doc/en/index.md
Normal file
18
doc/en/index.md
Normal file
|
|
@ -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.
|
||||
20
doc/index.md
20
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
|
||||
112
doc/ru/add_new_robot.md
Normal file
112
doc/ru/add_new_robot.md
Normal file
|
|
@ -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`.
|
||||
18
doc/ru/index.md
Normal file
18
doc/ru/index.md
Normal file
|
|
@ -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.
|
||||
32
doc/ru/installation.md
Normal file
32
doc/ru/installation.md
Normal file
|
|
@ -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
|
||||
```
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
<ambient>{' '.join(map(str, ambient))}</ambient>
|
||||
<diffuse>{' '.join(map(str, diffuse))}</diffuse>
|
||||
<specular>{' '.join(map(str, specular))}</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.",
|
||||
)
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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.",
|
||||
)
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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.",
|
||||
)
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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.",
|
||||
)
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -1,100 +1,129 @@
|
|||
camera:
|
||||
- clip_color: !tuple
|
||||
- 0.01
|
||||
- 1000.0
|
||||
clip_depth: !tuple
|
||||
- 0.05
|
||||
- 10.0
|
||||
enable: true
|
||||
height: 128
|
||||
horizontal_fov: 1.0471975511965976
|
||||
image_format: R8G8B8
|
||||
name: robot_camera
|
||||
noise_mean: null
|
||||
noise_stddev: null
|
||||
publish_color: true
|
||||
publish_depth: true
|
||||
publish_points: true
|
||||
random_pose_focal_point_z_offset: 0.0
|
||||
random_pose_mode: orbit
|
||||
random_pose_orbit_distance: 1.0
|
||||
random_pose_orbit_height_range: !tuple
|
||||
- 0.1
|
||||
- 0.7
|
||||
random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414
|
||||
random_pose_rollout_counter: 0.0
|
||||
random_pose_rollouts_num: 1
|
||||
random_pose_select_position_options: []
|
||||
relative_to: base_link
|
||||
spawn_position: !tuple
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
spawn_quat_xyzw: !tuple
|
||||
- 0.0
|
||||
- 0.70710678118
|
||||
- 0.0
|
||||
- 0.70710678118
|
||||
type: rgbd_camera
|
||||
update_rate: 10
|
||||
vertical_fov: 1.0471975511965976
|
||||
width: 128
|
||||
- clip_color: !tuple
|
||||
- 0.01
|
||||
- 1000.0
|
||||
clip_depth: !tuple
|
||||
- 0.05
|
||||
- 10.0
|
||||
enable: true
|
||||
height: 128
|
||||
horizontal_fov: 1.0471975511965976
|
||||
image_format: R8G8B8
|
||||
name: robot_camera
|
||||
noise_mean: null
|
||||
noise_stddev: null
|
||||
publish_color: true
|
||||
publish_depth: true
|
||||
publish_points: true
|
||||
random_pose_focal_point_z_offset: 0.0
|
||||
random_pose_mode: orbit
|
||||
random_pose_orbit_distance: 1.0
|
||||
random_pose_orbit_height_range: !tuple
|
||||
- 0.1
|
||||
- 0.7
|
||||
random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414
|
||||
random_pose_rollout_counter: 0.0
|
||||
random_pose_rollouts_num: 1
|
||||
random_pose_select_position_options: []
|
||||
relative_to: base_link
|
||||
spawn_position: !tuple
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
spawn_quat_xyzw: !tuple
|
||||
- 0.0
|
||||
- 0.70710678118
|
||||
- 0.0
|
||||
- 0.70710678118
|
||||
type: rgbd_camera
|
||||
update_rate: 10
|
||||
vertical_fov: 1.0471975511965976
|
||||
width: 128
|
||||
gravity: !tuple
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -9.80665
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -9.80665
|
||||
gravity_std: !tuple
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0232
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 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.25
|
||||
- -0.75
|
||||
- 0.5
|
||||
- -0.25
|
||||
- -0.75
|
||||
distance: 1000.0
|
||||
model_rollouts_num: 1
|
||||
radius: 25.0
|
||||
random_minmax_elevation: !tuple
|
||||
- -0.15
|
||||
- -0.65
|
||||
- -0.15
|
||||
- -0.65
|
||||
type: sun
|
||||
visual: true
|
||||
objects:
|
||||
- color: null
|
||||
name: bishop
|
||||
orientation: !tuple
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
position: !tuple
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 0.3
|
||||
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'
|
||||
- color: null
|
||||
name: hole
|
||||
orientation: !tuple
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
position: !tuple
|
||||
- 0.1
|
||||
- 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"
|
||||
- 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
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- -1.57
|
||||
- 0.5
|
||||
- 3.14159
|
||||
- 1.5
|
||||
- 0.0
|
||||
- 1.4
|
||||
- 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
|
||||
- 0.0
|
||||
- 1.0
|
||||
- 1.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
|
||||
- 1.0
|
||||
urdf_string: ''
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
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
|
||||
- 1
|
||||
- 0
|
||||
- 0
|
||||
- 0
|
||||
- 1
|
||||
type: flat
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
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")
|
||||
use_moveit = LaunchConfiguration("use_moveit")
|
||||
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
||||
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
||||
hardware = LaunchConfiguration("hardware")
|
||||
env_manager = LaunchConfiguration("env_manager")
|
||||
gazebo_gui = LaunchConfiguration("gazebo_gui")
|
||||
gripper_name = LaunchConfiguration("gripper_name")
|
||||
scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
|
||||
|
||||
initial_joint_controllers_file_path = os.path.join(
|
||||
get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
|
||||
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
||||
base_link_name = LaunchConfiguration("base_link_name").perform(context)
|
||||
|
||||
control_space = LaunchConfiguration("control_space").perform(context)
|
||||
control_strategy = LaunchConfiguration("control_strategy").perform(context)
|
||||
|
||||
interactive = LaunchConfiguration("interactive").perform(context)
|
||||
|
||||
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_file,
|
||||
mappings={
|
||||
"gripper_name": gripper_name.perform(context),
|
||||
"hardware": hardware.perform(context),
|
||||
"simulation_controllers": initial_joint_controllers_file_path,
|
||||
"namespace": "",
|
||||
},
|
||||
)
|
||||
xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
|
||||
|
||||
|
||||
# TODO: hide this to another place
|
||||
# Load xacro_args
|
||||
def param_constructor(loader, node, local_vars):
|
||||
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": initial_joint_controllers_file_path,
|
||||
"controllers_file": simulation_controllers,
|
||||
"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,
|
||||
"use_moveit": use_moveit,
|
||||
"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,
|
||||
"use_controllers": "true",
|
||||
"robot_description": robot_description_content,
|
||||
"robot_description_semantic": robot_description_semantic_content,
|
||||
"base_link_name": base_link_name,
|
||||
"ee_link_name": ee_link_name,
|
||||
"control_space": control_space,
|
||||
"control_strategy": control_strategy,
|
||||
"interactive_control": interactive,
|
||||
}.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"
|
||||
)
|
||||
)
|
||||
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?"
|
||||
"use_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",
|
||||
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",
|
||||
"use_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)]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
87
rbs_bringup/launch/control.launch.py
Normal file
87
rbs_bringup/launch/control.launch.py
Normal file
|
|
@ -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)]
|
||||
)
|
||||
|
|
@ -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)])
|
||||
|
|
@ -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)])
|
||||
53
rbs_bringup/launch/rbs_bringup.launch.py
Normal file
53
rbs_bringup/launch/rbs_bringup.launch.py
Normal file
|
|
@ -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)]
|
||||
)
|
||||
|
|
@ -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")
|
||||
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
|
||||
launch_moveit = LaunchConfiguration("launch_moveit")
|
||||
launch_task_planner = LaunchConfiguration("launch_task_planner")
|
||||
launch_perception = LaunchConfiguration("launch_perception")
|
||||
use_moveit = LaunchConfiguration("use_moveit")
|
||||
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
||||
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
hardware = LaunchConfiguration("hardware")
|
||||
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")
|
||||
use_controllers = LaunchConfiguration("use_controllers")
|
||||
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(
|
||||
get_package_share_directory(description_package), "config", controllers_file
|
||||
)
|
||||
|
||||
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 = {
|
||||
"robot_description": robot_description_content.perform(context)
|
||||
}
|
||||
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_strategy": "position",
|
||||
"has_gripper": "true",
|
||||
"control_space": control_space,
|
||||
"control_strategy": control_strategy,
|
||||
"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",
|
||||
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
||||
description="Type of robot to launch, specified by name.",
|
||||
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.",
|
||||
default_value="controllers.yaml",
|
||||
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 \
|
||||
is not set, it enables use of a custom description.",
|
||||
description="Package containing the robot's URDF/XACRO description files. Can be set to use 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 \
|
||||
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.",
|
||||
description="Prefix for joint names to support multi-robot setups. If changed, ensure joint names in controllers configuration are updated accordingly.",
|
||||
)
|
||||
)
|
||||
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.",
|
||||
description="Package containing the MoveIt configuration files (SRDF/XACRO) for the robot. Can be customized.",
|
||||
)
|
||||
)
|
||||
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.\
|
||||
This is needed for the trajectory planing in simulation.",
|
||||
description="Enables simulation time in MoveIt, needed for trajectory planning 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="choose gripper by name (leave empty if hasn't)",
|
||||
description="ROS 2 namespace for the robot.",
|
||||
)
|
||||
)
|
||||
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",
|
||||
default_value="true",
|
||||
description="Launch simulator (Gazebo)?\
|
||||
Most general arg",
|
||||
"roll",
|
||||
default_value="0.0",
|
||||
description="Roll orientation of the robot in the world.",
|
||||
)
|
||||
)
|
||||
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",
|
||||
)
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -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)]
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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(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
|
||||
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
|
||||
# src/moveit_servo_skill_server.cpp)
|
||||
# ament_target_dependencies(servo_action_server ${deps})
|
||||
#
|
||||
macro(add_ros2_component target_name source_file plugin_name executable_name)
|
||||
add_library(${target_name} SHARED ${source_file})
|
||||
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
|
||||
move_cartesian_path_action_server
|
||||
# servo_action_server
|
||||
cartesian_move_to_pose
|
||||
move_to_joint_states
|
||||
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})
|
||||
|
|
|
|||
|
|
@ -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
|
||||
0
rbs_skill_servers/config/skills.yaml
Normal file
0
rbs_skill_servers/config/skills.yaml
Normal file
456
rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp
Normal file
456
rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp
Normal file
|
|
@ -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 ¶meter = 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 ¶meter) {
|
||||
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 ¶m : 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 ¶m : 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
|
||||
|
|
@ -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 rbs_launch_utils.launch_common import load_yaml
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
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_semantic_decl = LaunchConfiguration("robot_description_semantic")
|
||||
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
|
||||
robot_description_content = LaunchConfiguration("robot_description")
|
||||
robot_description_semantic_content = LaunchConfiguration("robot_description_semantic")
|
||||
robot_description_kinematics_filepath = LaunchConfiguration("robot_description_kinematics")
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
|
||||
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
|
||||
robot_description = {"robot_description": robot_description_decl}
|
||||
use_moveit = LaunchConfiguration("use_moveit")
|
||||
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
||||
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("rbs_arm", "config/kinematics.yaml")
|
||||
|
||||
kinematics_yaml = load_yaml_abs(robot_description_kinematics_filepath.perform(context))
|
||||
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
|
||||
|
||||
move_topose_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_topose_action_server",
|
||||
skills_container = ComposableNodeContainer(
|
||||
name="skills",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
],
|
||||
)
|
||||
|
||||
gripper_control_node = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="gripper_control_action_server",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
],
|
||||
condition=IfCondition(with_gripper_condition),
|
||||
)
|
||||
|
||||
move_cartesian_path_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_cartesian_path_action_server",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
],
|
||||
)
|
||||
|
||||
cartesian_move_to_pose_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_to_pose.py",
|
||||
namespace=namespace,
|
||||
parameters=[{"use_sim_time": use_sim_time}, {"robot_name": namespace}],
|
||||
)
|
||||
|
||||
move_joint_state_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_to_joint_states_action_server",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
package="rclcpp_components",
|
||||
# prefix=['gdbserver localhost:1234'],
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package="rbs_skill_servers",
|
||||
plugin="rbs_skill_actions::MoveToJointStateActionServer",
|
||||
name="mtjs_jtc",
|
||||
parameters=[{"use_sim_time": use_sim_time}],
|
||||
),
|
||||
ComposableNode(
|
||||
package="rbs_skill_servers",
|
||||
plugin="rbs_skill_actions::CartesianMoveToPose",
|
||||
name="mtp_cart",
|
||||
parameters=[
|
||||
{"use_sim_time": use_sim_time},
|
||||
{"robot_name": namespace},
|
||||
],
|
||||
),
|
||||
ComposableNode(
|
||||
package="rbs_skill_servers",
|
||||
plugin="rbs_skill_actions::MoveToPose",
|
||||
name="mtp_jtc",
|
||||
parameters=[
|
||||
{"use_sim_time": use_sim_time},
|
||||
{"robot_name": namespace},
|
||||
robot_description,
|
||||
],
|
||||
),
|
||||
ComposableNode(
|
||||
package="rbs_skill_servers",
|
||||
plugin="rbs_skill_actions::MoveToPoseJtcCartesian",
|
||||
name="mtp_jtc_cart",
|
||||
parameters=[
|
||||
{"use_sim_time": use_sim_time},
|
||||
{"robot_name": namespace},
|
||||
{"base_link": base_link_name},
|
||||
{"robot_ee_link": ee_link_name},
|
||||
robot_description,
|
||||
],
|
||||
),
|
||||
ComposableNode(
|
||||
package="rbs_skill_servers",
|
||||
plugin="rbs_skill_actions::MoveitMtp",
|
||||
name="mtp_moveit",
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
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,
|
||||
gripper_control_node,
|
||||
move_cartesian_path_action_server,
|
||||
move_joint_state_action_server,
|
||||
cartesian_move_to_pose_action_server,
|
||||
# grasp_pose_loader
|
||||
skills_container,
|
||||
]
|
||||
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)]
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
155
rbs_skill_servers/src/mtjs_jtc.cpp
Normal file
155
rbs_skill_servers/src/mtjs_jtc.cpp
Normal file
|
|
@ -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);
|
||||
183
rbs_skill_servers/src/mtp_cart.cpp
Normal file
183
rbs_skill_servers/src/mtp_cart.cpp
Normal file
|
|
@ -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);
|
||||
224
rbs_skill_servers/src/mtp_jtc.cpp
Normal file
224
rbs_skill_servers/src/mtp_jtc.cpp
Normal file
|
|
@ -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);
|
||||
324
rbs_skill_servers/src/mtp_jtc_cart.cpp
Normal file
324
rbs_skill_servers/src/mtp_jtc_cart.cpp
Normal file
|
|
@ -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);
|
||||
68
rbs_skill_servers/src/mtp_moveit.cpp
Normal file
68
rbs_skill_servers/src/mtp_moveit.cpp
Normal file
|
|
@ -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);
|
||||
95
rbs_skill_servers/src/mtp_moveit_cart.cpp
Normal file
95
rbs_skill_servers/src/mtp_moveit_cart.cpp
Normal file
|
|
@ -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);
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
37
repos/all-deps.repos
Normal file
37
repos/all-deps.repos
Normal file
|
|
@ -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
|
||||
29
repos/requirements.txt
Normal file
29
repos/requirements.txt
Normal file
|
|
@ -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
|
||||
Loading…
Add table
Add a link
Reference in a new issue