├── .docker ├── README.md ├── build_source.sh └── source │ └── Dockerfile ├── .dockerignore ├── .github └── workflows │ ├── build_docker_image.yml │ └── industrial_ci.yml ├── .gitignore ├── LICENSE ├── README.md ├── crane_plus-not-released.rolling.repos ├── crane_plus ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── crane_plus_control ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ └── crane_plus_controllers.yaml ├── crane_plus_hardware.xml ├── include │ └── crane_plus_control │ │ ├── crane_plus_driver.hpp │ │ ├── crane_plus_hardware.hpp │ │ └── visibility_control.h ├── launch │ └── crane_plus_control.launch.py ├── package.xml └── src │ ├── crane_plus_driver.cpp │ └── crane_plus_hardware.cpp ├── crane_plus_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── crane_plus_description │ ├── __init__.py │ └── robot_description_loader.py ├── launch │ ├── display.launch.py │ └── display.rviz ├── meshes │ ├── collision │ │ ├── body.stl │ │ ├── hand.stl │ │ ├── link1.stl │ │ ├── link2.stl │ │ ├── link3.stl │ │ └── link4.stl │ └── visual │ │ ├── body.stl │ │ ├── hand.stl │ │ ├── link1.stl │ │ ├── link2.stl │ │ ├── link3.stl │ │ └── link4.stl ├── package.xml ├── test │ ├── dummy_controllers.yaml │ └── test_robot_description_loader.py └── urdf │ ├── camera.xacro │ ├── camera_stand.xacro │ ├── crane_plus.gazebo.xacro │ ├── crane_plus.gazebo_ros2_control.xacro │ ├── crane_plus.ros2_control.xacro │ ├── crane_plus.urdf.xacro │ └── crane_plus.xacro ├── crane_plus_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── aruco_markers.pdf ├── config │ └── camera_info.yaml ├── launch │ ├── camera_example.launch.py │ ├── camera_example.rviz │ ├── demo.launch.py │ └── example.launch.py ├── package.xml └── src │ ├── aruco_detection.cpp │ ├── color_detection.cpp │ ├── gripper_control.cpp │ ├── joint_values.cpp │ ├── pick_and_place.cpp │ ├── pick_and_place_tf.cpp │ └── pose_groupstate.cpp ├── crane_plus_gazebo ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── gui │ └── gui.config ├── launch │ ├── crane_plus_with_aruco_cube.launch.py │ ├── crane_plus_with_red_cube.launch.py │ └── crane_plus_with_table.launch.py ├── models │ └── aruco_cube_0 │ │ ├── meshes │ │ ├── aruco_cube_0.dae │ │ └── aruco_cube_0.png │ │ ├── model.config │ │ └── model.sdf ├── package.xml └── worlds │ ├── table.sdf │ ├── table_with_aruco_cube.sdf │ └── table_with_red_cube.sdf └── crane_plus_moveit_config ├── .setup_assistant ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config ├── crane_plus.srdf ├── joint_limits.yaml ├── kinematics.yaml ├── moveit.rviz ├── moveit_controllers.yaml ├── ompl_planning.yaml └── pilz_cartesian_limits.yaml ├── launch ├── run_move_group.launch.py └── setup_assistant.launch.py └── package.xml /.docker/README.md: -------------------------------------------------------------------------------- 1 | # Dockerを使用する 2 | 3 | ## Dockerイメージを使用する 4 | 5 | https://github.com/rt-net/crane_plus/pkgs/container/crane_plus 6 | にDockerイメージ`ghcr.io/rt-net/crane_plus:$ROS_DISTRO` 7 | をアップロードしています。 8 | tagにはROSのディストリビューションを指定してください。 9 | 10 | Jazzyディストリビューションのイメージをダウンロードする場合は次のコマンドを実行します。 11 | 12 | ```sh 13 | $ docker pull ghcr.io/rt-net/crane_plus:jazzy 14 | ``` 15 | 16 | ### ノードの起動 17 | 18 | GUIアプリケーションを起動するため 19 | [osrf/rocker](https://github.com/osrf/rocker) 20 | を使用します 21 | 22 | rockerのオプションには、 23 | ホストのネットワーク環境を使用するための`--net=host`と 24 | [ネットワーク使用時のエラー](https://github.com/osrf/rocker/issues/13) 25 | を回避するための`--privileged`を与えます 26 | 27 | またCRANE+ V2実機を動かす場合は、[crane_plus_control/README.md](../crane_plus_control/README.md) 28 | を参考に、USB通信ポートのアクセス権限を変更してください。 29 | 30 | ```sh 31 | # rockerのインストール 32 | $ sudo apt install python3-rocker 33 | 34 | $ rocker --x11 --net=host --privileged \ 35 | --volume /dev/shm:/dev/shm \ 36 | -- ghcr.io/rt-net/crane_plus:$ROS_DISTRO \ 37 | ros2 launch crane_plus_examples demo.launch.py 38 | ``` 39 | 40 | ### パッケージの再ビルド 41 | 42 | ホストに作成したワークスペースをDockerコンテナにマウントすることで、 43 | ホスト環境でのファイル編集内容をコンテナ内に反映できます 44 | 45 | ```sh 46 | # ワークスペース作成 47 | $ mkdir -p ~/crane_ws/src 48 | # パッケージをクローン 49 | $ git clone https://github.com/rt-net/crane_plus.git ~/crane_ws/src/crane_plus 50 | 51 | # パッケージをビルド 52 | $ rocker --x11 --net=host --privileged \ 53 | --volume ~/crane_ws:/root/overlay_ws \ 54 | -- ghcr.io/rt-net/crane_plus:$ROS_DISTRO \ 55 | colcon build --symlink-install 56 | 57 | # ノードを起動 58 | $ rocker --x11 --net=host --privileged \ 59 | --volume /dev/shm:/dev/shm \ 60 | --volume ~/crane_ws:/root/overlay_ws \ 61 | -- ghcr.io/rt-net/crane_plus:$ROS_DISTRO \ 62 | ros2 launch crane_plus_examples demo.launch.py 63 | ``` 64 | 65 | ## Dockerイメージをビルドする 66 | 67 | `./build_source.sh $ROS_DISTRO`を実行してイメージを作成します。 68 | 69 | ```sh 70 | # jazzyディストリビューションのイメージを作成する 71 | $ cd crane_plus/.docker 72 | $ ./build_source.sh jazzy 73 | ... 74 | Successfully tagged crane_plus:jazzy 75 | ``` 76 | -------------------------------------------------------------------------------- /.docker/build_source.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -e 4 | 5 | if [ $# -eq 0 ]; then 6 | echo "Please set ROS_DISTRO to the argument." 7 | echo "e.g. ./build_source.sh jazzy" 8 | fi 9 | ROS_DISTRO=$1 10 | 11 | DOCKER_OPTION="" 12 | if [ $# -ge 2 ]; then 13 | DOCKER_OPTION=$2 14 | fi 15 | 16 | COLCON_OPTION="" 17 | if [ $# -ge 3 ]; then 18 | COLCON_OPTION=$3 19 | fi 20 | 21 | # Move to the package route directory 22 | cd $(dirname $0)/../ 23 | docker build $DOCKER_OPTION -t crane_plus:$ROS_DISTRO -f .docker/source/Dockerfile . \ 24 | --build-arg ROS_DISTRO=$ROS_DISTRO \ 25 | --build-arg COLCON_OPTION=$COLCON_OPTION 26 | -------------------------------------------------------------------------------- /.docker/source/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO="jazzy" 2 | FROM osrf/ros:${ROS_DISTRO}-desktop 3 | ENV OVERLAY_WS /root/overlay_ws 4 | WORKDIR $OVERLAY_WS/src 5 | 6 | # Copy source files 7 | COPY . crane_plus 8 | 9 | # Install depnedencies 10 | RUN apt-get update && apt-get -y dist-upgrade && rosdep update && \ 11 | rosdep install -iy --from-paths . && \ 12 | # https://github.com/ros-planning/moveit2/issues/1094 13 | apt install -y ros-${ROS_DISTRO}-backward-ros && \ 14 | rm -rf /var/lib/apt/lists/ 15 | 16 | # Build packages 17 | ARG COLCON_OPTION="" 18 | RUN cd $OVERLAY_WS && \ 19 | . /opt/ros/${ROS_DISTRO}/setup.sh && \ 20 | colcon build --symlink-install $COLCON_OPTION 21 | 22 | # Download gazebo models by sparse checkout 23 | RUN mkdir -p /root/.gazebo/models && \ 24 | cd /root/.gazebo/models && \ 25 | git init . && \ 26 | git config core.sparsecheckout true && \ 27 | echo "ground_plane" >> .git/info/sparse-checkout && \ 28 | echo "sun" >> .git/info/sparse-checkout && \ 29 | echo "table" >> .git/info/sparse-checkout && \ 30 | git remote add origin https://github.com/osrf/gazebo_models && \ 31 | git pull origin master && \ 32 | rm -rf .git 33 | 34 | # Edit entrypoint to source overlay setup file 35 | WORKDIR $OVERLAY_WS 36 | RUN sed --in-place --expression \ 37 | '$i if [ -e $OVERLAY_WS/install/setup.bash ]; then\n\tsource "$OVERLAY_WS/install/setup.bash" \nfi' \ 38 | /ros_entrypoint.sh 39 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | .docker 2 | .github 3 | .git 4 | *.md -------------------------------------------------------------------------------- /.github/workflows/build_docker_image.yml: -------------------------------------------------------------------------------- 1 | name: Create and publish a Docker image 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | workflow_dispatch: 8 | 9 | env: 10 | ROS_DISTRO: jazzy 11 | REGISTRY: ghcr.io 12 | 13 | jobs: 14 | build-and-push-image: 15 | runs-on: ubuntu-latest 16 | permissions: 17 | contents: read 18 | packages: write 19 | 20 | steps: 21 | - name: Checkout repository 22 | uses: actions/checkout@v3 23 | 24 | - name: Login to GitHub Container Registry 25 | uses: docker/login-action@v2 26 | with: 27 | registry: ${{ env.REGISTRY }} 28 | username: ${{ github.actor }} 29 | password: ${{ secrets.GITHUB_TOKEN }} 30 | 31 | - name: Build and push Docker image 32 | uses: docker/build-push-action@v3 33 | with: 34 | file: .docker/source/Dockerfile 35 | build-args: ROS_DISTRO=${{ env.ROS_DISTRO }} 36 | push: true 37 | tags: ${{ env.REGISTRY }}/${{ github.repository }}:${{ env.ROS_DISTRO }} 38 | -------------------------------------------------------------------------------- /.github/workflows/industrial_ci.yml: -------------------------------------------------------------------------------- 1 | name: industrial_ci 2 | 3 | on: 4 | push: 5 | paths-ignore: 6 | - 'docs/**' 7 | - '**.md' 8 | pull_request: 9 | paths-ignore: 10 | - 'docs/**' 11 | - '**.md' 12 | schedule: 13 | - cron: "0 2 * * 0" # Weekly on Sundays at 02:00 14 | 15 | jobs: 16 | industrial_ci: 17 | strategy: 18 | fail-fast: false 19 | matrix: 20 | env: 21 | - { ROS_DISTRO: jazzy, ROS_REPO: main } 22 | - { ROS_DISTRO: rolling, ROS_REPO: main, UPSTREAM_WORKSPACE: crane_plus-not-released.rolling.repos } 23 | runs-on: ubuntu-latest 24 | steps: 25 | - uses: actions/checkout@v4 26 | - uses: "ros-industrial/industrial_ci@master" 27 | env: ${{ matrix.env }} 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | .pytest_cache 59 | .vscode -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # crane_plus 2 | 3 | [![industrial_ci](https://github.com/rt-net/crane_plus/workflows/industrial_ci/badge.svg?branch=master)](https://github.com/rt-net/crane_plus/actions?query=workflow%3Aindustrial_ci+branch%3Amaster) 4 | 5 | ROS 2 package suite of CRANE+ V2. 6 | 7 | 8 | 9 | ## Table of Contents 10 | 11 | - [Supported ROS 2 distributions](#supported-ROS-2-distributions) 12 | - [Requirements](#requirements) 13 | - [Installation](#installation) 14 | - [Binary installation](#binary-installation) 15 | - [Source Build](#source-build) 16 | - [Quick Start](#quick-start) 17 | - [Packages](#packages) 18 | - [License](#license) 19 | - [Disclaimer](#disclaimer) 20 | 21 | ## Supported ROS 2 distributions 22 | 23 | - [Foxy](https://github.com/rt-net/crane_plus/tree/foxy-devel) 24 | - [Humble](https://github.com/rt-net/crane_plus/tree/humble-devel) 25 | - Jazzy 26 | ## Requirements 27 | 28 | - CRANE+ V2 29 | - [Product Introduction](https://rt-net.jp/products/cranev2/) 30 | - [Web Shop](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1348_1&products_id=3626&language=ja) 31 | - Linux OS 32 | - Ubuntu 24.04 33 | - ROS 34 | - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/Installation.html) 35 | 36 | ## Installation 37 | 38 | ### Docker images 39 | 40 | ビルド済みのパッケージ含むDocker imageを用意してます。 41 | 詳細は[.docker/README.md](./.docker/README.md)を参照してください。 42 | 43 | ### Binary installation 44 | 45 | ```sh 46 | $ sudo apt update 47 | $ sudo apt install ros-$ROS_DISTRO-crane-plus 48 | ``` 49 | 50 | ### Source Build 51 | 52 | ```sh 53 | # Download crane_plus repository 54 | $ mkdir -p ~/ros2_ws/src 55 | $ cd ~/ros2_ws/src 56 | $ git clone -b $ROS_DISTRO https://github.com/rt-net/crane_plus.git 57 | 58 | # Install dependencies 59 | $ rosdep install -r -y -i --from-paths . 60 | 61 | # Build & Install 62 | $ cd ~/ros2_ws 63 | $ colcon build --symlink-install 64 | $ source ~/ros2_ws/install/setup.bash 65 | ``` 66 | 67 | ## Quick Start 68 | 69 | ```sh 70 | # Connect CRANE+ V2 to PC, then 71 | $ source ~/ros2_ws/install/setup.bash 72 | $ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 73 | 74 | # Terminal 2 75 | $ source ~/ros2_ws/install/setup.bash 76 | $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' 77 | 78 | # Press [Ctrl-c] to terminate. 79 | ``` 80 | 81 | 82 | 83 | 詳細は[crane_plus_examples](./crane_plus_examples/README.md) 84 | を参照してください。 85 | 86 | ## Packages 87 | 88 | - crane_plus_control 89 | - [README](./crane_plus_control/README.md) 90 | - CRANE+ V2を制御するパッケージです 91 | - USB通信ポートの設定方法をREAMDEに記載してます 92 | - crane_plus_description 93 | - [README](./crane_plus_description/README.md) 94 | - CRANE+ V2のモデルデータ（xacro）を定義するパッケージです 95 | - crane_plus_examples 96 | - [README](./crane_plus_examples/README.md) 97 | - CRANE+ V2のサンプルコード集です 98 | - crane_plus_gazebo 99 | - [README](./crane_plus_gazebo/README.md) 100 | - CRANE+ V2のGazeboシミュレーションパッケージです 101 | - crane_plus_moveit_config 102 | - [README](./crane_plus_moveit_config/README.md) 103 | - CRANE+ V2の`moveit2`設定ファイルです 104 | 105 | ## License 106 | 107 | このリポジトリはApache 2.0ライセンスの元、公開されています。 108 | ライセンスについては[LICENSE](./LICENSE)を参照ください。 109 | 110 | サーボモータのAX-12Aに関するCADモデルの使用については、ROBOTIS社より使用許諾を受けています。 111 | CRANE+ V2に使用されているROBOTIS社の部品類にかかる著作権、商標権、その他の知的財産権は、ROBOTIS社に帰属します。 112 | 113 | We have obtained permission from ROBOTIS Co., Ltd. to use CAD models relating to servo motors AX-12A. The proprietary rights relating to any components or parts manufactured by ROBOTIS and used in this product, including but not limited to copyrights, trademarks, and other intellectual property rights, shall remain vested in ROBOTIS. 114 | 115 | ## Disclaimer 116 | 117 | 本ソフトウェアはApache 2.0ライセンスで、「AS IS」（現状有姿のまま）で提供しています。本ソフトウェアに関する無償サポートはありません。 118 | 119 | 当該製品および当ソフトウェアの使用中に生じたいかなる損害も株式会社アールティでは一切の責任を負いかねます。 ユーザー自身で作成されたプログラムに適切な制限動作が備わっていない場合、本体の損傷や、本体が周囲や作業者に接触、あるいは衝突し、思わぬ重大事故が発生する危険があります。 ユーザーの責任において十分に安全に注意した上でご使用下さい。 120 | 121 | -------------------------------------------------------------------------------- /crane_plus-not-released.rolling.repos: -------------------------------------------------------------------------------- 1 | - git: 2 | uri: https://github.com/ros-controls/gz_ros2_control.git 3 | local-name: gz_ros2_control 4 | version: rolling -------------------------------------------------------------------------------- /crane_plus/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package crane_plus 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-01-31) 6 | ------------------ 7 | 8 | 2.0.1 (2023-09-08) 9 | ------------------ 10 | 11 | 2.0.0 (2023-08-01) 12 | ------------------ 13 | * Feature/support humble (`#58 `_) 14 | * Contributors: Shota Aoki, YusukeKato 15 | 16 | 1.1.0 (2022-08-16) 17 | ------------------ 18 | * Update author tags (`#49 `_) 19 | * Add crane_plus_ignition dependency (`#48 `_) 20 | * Rename CRANE+V2 to CRANE+ V2 (`#44 `_) 21 | * Contributors: Shota Aoki 22 | 23 | 1.0.0 (2022-06-22) 24 | ------------------ 25 | * パッケージバージョン表記の更新 (`#40 `_) 26 | * Contributors: Atsushi Kuwagata 27 | 28 | 0.1.0 (2020-11-11) 29 | ------------------ 30 | * Update maintainer and author in package.xml (`#23 `_) 31 | * Update for release (`#21 `_) 32 | * Contributors: Shota Aoki 33 | -------------------------------------------------------------------------------- /crane_plus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crane_plus) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /crane_plus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crane_plus 5 | 3.0.0 6 | ROS 2 package suite of CRANE+ V2 7 | RT Corporation 8 | Apache License 2.0 9 | 10 | Shota Aoki 11 | Atsushi Kuwagata 12 | Yusuke Kato 13 | 14 | https://rt-net.jp/products/cranev2/ 15 | https://github.com/rt-net/crane_plus/issues 16 | https://github.com/rt-net/crane_plus 17 | 18 | ament_cmake 19 | 20 | crane_plus_control 21 | crane_plus_description 22 | crane_plus_examples 23 | crane_plus_gazebo 24 | crane_plus_moveit_config 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /crane_plus_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package crane_plus_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-01-31) 6 | ------------------ 7 | 8 | 2.0.1 (2023-09-08) 9 | ------------------ 10 | 11 | 2.0.0 (2023-08-01) 12 | ------------------ 13 | * Feature/support humble (`#58 `_) 14 | * Contributors: Shota Aoki, YusukeKato 15 | 16 | 1.1.0 (2022-08-16) 17 | ------------------ 18 | * Update author tags (`#49 `_) 19 | * Rename CRANE+V2 to CRANE+ V2 (`#44 `_) 20 | * Contributors: Shota Aoki 21 | 22 | 1.0.0 (2022-06-22) 23 | ------------------ 24 | * パッケージバージョン表記の更新 (`#40 `_) 25 | * xacroファイルの読み込みを一元化するためのPythonスクリプトを追加 (`#36 `_) 26 | * GripperActionControllerに関するコメントを削除 (`#37 `_) 27 | * hardware_interfaceのパラメータをxacro引数から変更する (`#35 `_) 28 | * Use new ros2 control interface (`#27 `_) 29 | * Contributors: Atsushi Kuwagata, Shota Aoki 30 | 31 | 0.1.0 (2020-11-11) 32 | ------------------ 33 | * Fix READMEs (`#25 `_) 34 | * Update control error log (`#24 `_) 35 | * Update maintainer and author in package.xml (`#23 `_) 36 | * Update for release (`#21 `_) 37 | * Update crane_plus_control (`#16 `_) 38 | * Add joint_values example and pick_and_place example (`#15 `_) 39 | * Fix launch files (`#12 `_) 40 | * Use joint_trajectory_controller for gripper control (`#11 `_) 41 | * Refactor to pass ament_lint check (`#9 `_) 42 | * Revert "Use new joint handles (`#4 `_)" (`#7 `_) 43 | * Use new joint handles (`#4 `_) 44 | * Add crane plus control (`#3 `_) 45 | * Contributors: Shota Aoki 46 | -------------------------------------------------------------------------------- /crane_plus_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crane_plus_control) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(dynamixel_sdk REQUIRED) 21 | find_package(hardware_interface REQUIRED) 22 | find_package(pluginlib REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | 25 | ## COMPILE 26 | set(LIBRARY_NAME "crane_plus_hardware") 27 | add_library( 28 | ${LIBRARY_NAME} 29 | SHARED 30 | src/crane_plus_hardware.cpp src/crane_plus_driver.cpp 31 | ) 32 | target_include_directories( 33 | ${LIBRARY_NAME} 34 | PRIVATE 35 | include 36 | ) 37 | ament_target_dependencies( 38 | ${LIBRARY_NAME} 39 | dynamixel_sdk 40 | hardware_interface 41 | pluginlib 42 | rclcpp 43 | ) 44 | 45 | pluginlib_export_plugin_description_file(hardware_interface crane_plus_hardware.xml) 46 | 47 | # INSTALL 48 | install( 49 | TARGETS ${LIBRARY_NAME} 50 | DESTINATION lib 51 | ) 52 | install( 53 | DIRECTORY include/ 54 | DESTINATION include 55 | ) 56 | 57 | install(DIRECTORY 58 | launch 59 | config 60 | DESTINATION share/${PROJECT_NAME}/ 61 | ) 62 | 63 | if(BUILD_TESTING) 64 | find_package(ament_lint_auto REQUIRED) 65 | ament_lint_auto_find_test_dependencies() 66 | endif() 67 | 68 | ## EXPORTS 69 | ament_export_include_directories( 70 | include 71 | ) 72 | ament_export_libraries( 73 | ${LIBRARY_NAME} 74 | ) 75 | ament_export_dependencies( 76 | dynamixel_sdk 77 | hardware_interface 78 | pluginlib 79 | rclcpp 80 | ) 81 | 82 | ament_package() 83 | -------------------------------------------------------------------------------- /crane_plus_control/README.md: -------------------------------------------------------------------------------- 1 | # crane_plus_control 2 | 3 | このパッケージは[ros2_control](https://github.com/ros-controls/ros2_control) 4 | をベースにした、CRANE+ V2 のコントローラパッケージです。 5 | 6 | ## ros2_control関連ファイル 7 | 8 | - `crane_plus_control::CranePlusHardware (crane_plus_hardware)` 9 | - 本パッケージがエクスポートする[Hardware Components](https://ros-controls.github.io/control.ros.org/getting_started.html#hardware-components)です 10 | - CRANE+ V2実機と通信します 11 | - [crane_plus_description/urdf/crane_plus.ros2_control.xacro](../crane_plus_description/urdf/crane_plus.ros2_control.xacro)から読み込まれます 12 | - [launch/crane_plus_control.launch.py](./launch/crane_plus_control.launch.py) 13 | - [Controller Manager](https://ros-controls.github.io/control.ros.org/getting_started.html#controller-manager)とコントローラを起動するlaunchファイルです 14 | - [config/crane_plus_controllers.yaml](./config/crane_plus_controllers.yaml) 15 | - Controller Managerのパラメータファイルです 16 | 17 | ## 実機のセットアップ 18 | 19 | `crane_plus_hardware`がCRANE+ V2実機と通信するために、 20 | PCとCRANE+ V2の設定が必要です。 21 | 22 | **正しく設定できていない場合、CRANE+ V2が動作しない、振動する、などの不安定な動きをするため注意してください** 23 | 24 | ### USB通信ポートの設定 25 | 26 | `crane_plus_hardware`はUSB通信ポート（`/dev/ttyUSB*`）を経由してCRANE+ V2と通信します。 27 | 28 | 次のコマンドでアクセス権限を変更します。 29 | 30 | ```sh 31 | # /dev/ttyUSB0を使用する場合 32 | $ sudo chmod 666 /dev/ttyUSB0 33 | ``` 34 | 35 | ### latency_timerの設定 36 | 37 | CRANE+ V2を100 Hz周期で制御するためには、 38 | USB通信ポートとサーボモータの設定を変更します。 39 | 40 | 下記のコマンドを実行してUSB通信ポートの`latency_timer`を変更します。 41 | 42 | 参考資料：https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#usb-latency-setting 43 | 44 | ```sh 45 | # /dev/ttyUSB0を使用する場合 46 | 47 | # rootに切り替える 48 | $ sudo su 49 | ``` 50 | 51 | ```txt 52 | # echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer 53 | # cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer 54 | 1 55 | # exit 56 | ``` 57 | 58 | ### Return Delay Timeの設定 59 | 60 | CRANE+ V2に搭載されているサーボモータ[Dynamixel AX-12A](https://emanual.robotis.com/docs/en/dxl/ax/ax-12a/) 61 | には`Return Delay Time`というパラメータがあります。 62 | 63 | デフォルトは250がセットされており、 64 | サーボモータが`Instruction Packet`を受信してから`Status Packet`を送信するまでに`500 usec`の遅れがあります。 65 | 66 | [Dynamixel Wizard 2](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/) 67 | を使用して`Retrun Delay Time`を小さくすると、制御周期が早くなります。 68 | 69 | ![Setting Return Delay Time](https://rt-net.github.io/images/crane-plus/setting_return_delay_time.png) 70 | 71 | ## ノードの起動 72 | 73 | `crane_plus_control.launch.py`を実行すると、`Controller Manager`ノードが起動し、 74 | 以下のコントローラが読み込まれます。 75 | 76 | - crane_plus_joint_state_broadcaster (`joint_state_broadcaster/JointStateBroadcaster`) 77 | - crane_plus_arm_controller (`joint_trajectory_controller/JointTrajectoryController`) 78 | - crane_plus_gripper_controller (`joint_trajectory_controller/JointTrajectoryController`) 79 | 80 | ノードが起動した後、 81 | 次のコマンドでジョイント角度情報（`joint_states`）を表示できます 82 | 83 | ```sh 84 | $ ros2 topic echo /joint_states 85 | ``` 86 | 87 | ## Controller Managerのパラメータ 88 | 89 | `Controller Manager`のパラメータは 90 | [config/crane_plus_controllers.yaml](./config/crane_plus_controllers.yaml) 91 | で設定しています。 92 | 93 | ```yaml 94 | controller_manager: 95 | ros__parameters: 96 | update_rate: 100 # Hz 97 | 98 | crane_plus_arm_controller: 99 | type: joint_trajectory_controller/JointTrajectoryController 100 | crane_plus_gripper_controller: 101 | type: joint_trajectory_controller/JointTrajectoryController 102 | joint_state_broadcaster: 103 | type: joint_state_broadcaster/JointStateBroadcaster 104 | ``` 105 | 106 | ### 制御周期 107 | 108 | `update_rate`は制御周期を設定します。 109 | 110 | CRANE+ V2に使用しているサーボモータの仕様により、 111 | 100 Hz以上の周期で制御できません。 112 | 113 | ### コントローラ 114 | 115 | CRANE+ V2の腕の制御用に`crane_plus_arm_controller`を、 116 | グリッパの制御用に`crane_plus_gripper_controller`を設定しています。 117 | 118 | ## crane_plus_hardwareのパラメータ 119 | 120 | `crane_plus_hardware`のパラメータは 121 | [crane_plus_description/urdf/crane_plus.urdf.xacro](../crane_plus_description/urdf/crane_plus.urdf.xacro) 122 | で設定しています。 123 | 124 | ```xml 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | ``` 134 | 135 | ### USB通信ポート 136 | 137 | `port_name`はCRANE+ V2との通信に使用するUSB通信ポートを設定します。 138 | 139 | ### ボーレート 140 | 141 | `baudrate`はCRANE+ V2に搭載したDynamixelとの通信ボーレートを設定します。 142 | 143 | デフォルト値にはDynamixel AX-12Aの最高ボーレートである`1000000` (1 Mbps)を設定しています。 144 | 145 | ### 通信タイムアウト 146 | 147 | `timeout_seconds`は通信タイムアウト時間（秒）を設定します。 148 | 149 | `crane_plus_hardware`は、一定時間（デフォルト5秒間）通信に失敗し続けると、 150 | read/write動作を停止します。 151 | USBケーブルや電源ケーブルが抜けた場合等に有効です。 152 | 153 | ### サーボパラメータ 154 | 155 | `read_velocities`、`read_loads`、`read_voltages`、`read_temperatures` 156 | は、サーボの回転速度、電圧、負荷、温度を読み取るためのパラメータです。 157 | 158 | `1`をセットすると、サーボパラメータを読み取ります。 159 | 160 | これらのパラメータを読み取ると通信データ量が増加するため、制御周期が100 Hzより低下します。 161 | 162 | 読み取ったパラメータは`dynamic_joint_states`トピックとしてパブリッシュされます。 163 | 164 | ```sh 165 | $ ros2 topic echo /dynamic_joint_states 166 | ``` 167 | 168 | --- 169 | 170 | [back to top](#crane_plus_control) 171 | -------------------------------------------------------------------------------- /crane_plus_control/config/crane_plus_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | crane_plus_arm_controller: 6 | type: joint_trajectory_controller/JointTrajectoryController 7 | crane_plus_gripper_controller: 8 | type: joint_trajectory_controller/JointTrajectoryController 9 | joint_state_broadcaster: 10 | type: joint_state_broadcaster/JointStateBroadcaster 11 | 12 | crane_plus_arm_controller: 13 | ros__parameters: 14 | joints: 15 | - crane_plus_joint1 16 | - crane_plus_joint2 17 | - crane_plus_joint3 18 | - crane_plus_joint4 19 | 20 | command_interfaces: 21 | - position 22 | 23 | state_interfaces: 24 | - position 25 | 26 | crane_plus_gripper_controller: 27 | ros__parameters: 28 | joints: 29 | - crane_plus_joint_hand 30 | 31 | command_interfaces: 32 | - position 33 | 34 | state_interfaces: 35 | - position 36 | -------------------------------------------------------------------------------- /crane_plus_control/crane_plus_hardware.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A CRANE+ V2 hardware plugin for the ros2_control. 7 | 8 | 9 | -------------------------------------------------------------------------------- /crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CRANE_PLUS_CONTROL__CRANE_PLUS_DRIVER_HPP_ 16 | #define CRANE_PLUS_CONTROL__CRANE_PLUS_DRIVER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | class CranePlusDriver 24 | { 25 | public: 26 | CranePlusDriver(const std::string port_name, const int baudrate, std::vector id_list); 27 | ~CranePlusDriver(); 28 | 29 | bool open_port(void); 30 | void close_port(void); 31 | std::string get_last_error_log(void); 32 | 33 | bool torque_enable(const bool enable); 34 | bool write_goal_joint_positions(const std::vector & goal_positions); 35 | bool write_moving_speed_rpm(const uint8_t dxl_id, const double speed_rpm); 36 | bool write_moving_speed_rpm_all(const double speed_rpm); 37 | bool read_present_joint_positions(std::vector & joint_positions); 38 | bool read_present_joint_speeds(std::vector & joint_speeds); 39 | bool read_present_joint_loads(std::vector & joint_loads); 40 | bool read_present_joint_voltages(std::vector & joint_voltages); 41 | bool read_present_joint_temperatures(std::vector & joint_temperatures); 42 | 43 | private: 44 | std::shared_ptr dxl_port_handler_; 45 | std::shared_ptr dxl_packet_handler_; 46 | int baudrate_; 47 | std::vector id_list_; 48 | std::string last_error_log_; 49 | 50 | bool read_byte_data_from_each_joints(const uint16_t address, std::vector & buffer); 51 | bool read_word_data_from_each_joints(const uint16_t address, std::vector & buffer); 52 | bool parse_dxl_error( 53 | const std::string func_name, const uint8_t dxl_id, 54 | const int dxl_comm_result, const uint8_t dxl_packet_error); 55 | double dxl_pos_to_radian(const uint16_t position); 56 | uint16_t radian_to_dxl_pos(const double position); 57 | double dxl_speed_to_rps(const uint16_t speed); 58 | double dxl_load_to_percent(const uint16_t load); 59 | double dxl_voltage_to_actual_voltage(const uint8_t voltage); 60 | }; 61 | 62 | #endif // CRANE_PLUS_CONTROL__CRANE_PLUS_DRIVER_HPP_ 63 | -------------------------------------------------------------------------------- /crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ 17 | #define CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "crane_plus_control/crane_plus_driver.hpp" 24 | #include "crane_plus_control/visibility_control.h" 25 | #include "hardware_interface/handle.hpp" 26 | #include "hardware_interface/hardware_info.hpp" 27 | #include "hardware_interface/system_interface.hpp" 28 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 29 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 30 | #include "rclcpp/macros.hpp" 31 | #include "rclcpp/rclcpp.hpp" 32 | #include "rclcpp_lifecycle/state.hpp" 33 | 34 | using hardware_interface::return_type; 35 | using hardware_interface::CallbackReturn; 36 | 37 | namespace crane_plus_control 38 | { 39 | class CranePlusHardware : public 40 | hardware_interface::SystemInterface 41 | { 42 | public: 43 | RCLCPP_SHARED_PTR_DEFINITIONS(CranePlusHardware) 44 | 45 | CRANE_PLUS_CONTROL_PUBLIC 46 | ~CranePlusHardware(); 47 | 48 | CRANE_PLUS_CONTROL_PUBLIC 49 | CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; 50 | 51 | CRANE_PLUS_CONTROL_PUBLIC 52 | std::vector export_state_interfaces() override; 53 | 54 | CRANE_PLUS_CONTROL_PUBLIC 55 | std::vector export_command_interfaces() override; 56 | 57 | CRANE_PLUS_CONTROL_PUBLIC 58 | CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 59 | 60 | CRANE_PLUS_CONTROL_PUBLIC 61 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 62 | 63 | CRANE_PLUS_CONTROL_PUBLIC 64 | return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; 65 | 66 | CRANE_PLUS_CONTROL_PUBLIC 67 | return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; 68 | 69 | private: 70 | bool communication_timeout(); 71 | 72 | std::shared_ptr driver_; 73 | double timeout_seconds_; 74 | bool read_velocities_; 75 | bool read_loads_; 76 | bool read_voltages_; 77 | bool read_temperatures_; 78 | 79 | std::vector hw_position_commands_; 80 | std::vector hw_position_states_; 81 | std::vector hw_velocity_states_; 82 | std::vector hw_load_states_; 83 | std::vector hw_voltage_states_; 84 | std::vector hw_temperature_states_; 85 | 86 | rclcpp::Clock steady_clock_; 87 | rclcpp::Time prev_comm_timestamp_; 88 | bool timeout_has_printed_; 89 | }; 90 | } // namespace crane_plus_control 91 | 92 | #endif // CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ 93 | -------------------------------------------------------------------------------- /crane_plus_control/include/crane_plus_control/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #ifndef CRANE_PLUS_CONTROL__VISIBILITY_CONTROL_H_ 23 | #define CRANE_PLUS_CONTROL__VISIBILITY_CONTROL_H_ 24 | 25 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 26 | // https://gcc.gnu.org/wiki/Visibility 27 | 28 | #if defined _WIN32 || defined __CYGWIN__ 29 | #ifdef __GNUC__ 30 | #define CRANE_PLUS_CONTROL_EXPORT __attribute__((dllexport)) 31 | #define CRANE_PLUS_CONTROL_IMPORT __attribute__((dllimport)) 32 | #else 33 | #define CRANE_PLUS_CONTROL_EXPORT __declspec(dllexport) 34 | #define CRANE_PLUS_CONTROL_IMPORT __declspec(dllimport) 35 | #endif 36 | #ifdef CRANE_PLUS_CONTROL_BUILDING_DLL 37 | #define CRANE_PLUS_CONTROL_PUBLIC CRANE_PLUS_CONTROL_EXPORT 38 | #else 39 | #define CRANE_PLUS_CONTROL_PUBLIC CRANE_PLUS_CONTROL_IMPORT 40 | #endif 41 | #define CRANE_PLUS_CONTROL_PUBLIC_TYPE CRANE_PLUS_CONTROL_PUBLIC 42 | #define CRANE_PLUS_CONTROL_LOCAL 43 | #else 44 | #define CRANE_PLUS_CONTROL_EXPORT __attribute__((visibility("default"))) 45 | #define CRANE_PLUS_CONTROL_IMPORT 46 | #if __GNUC__ >= 4 47 | #define CRANE_PLUS_CONTROL_PUBLIC __attribute__((visibility("default"))) 48 | #define CRANE_PLUS_CONTROL_LOCAL __attribute__((visibility("hidden"))) 49 | #else 50 | #define CRANE_PLUS_CONTROL_PUBLIC 51 | #define CRANE_PLUS_CONTROL_LOCAL 52 | #endif 53 | #define CRANE_PLUS_CONTROL_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // CRANE_PLUS_CONTROL__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /crane_plus_control/launch/crane_plus_control.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | from launch import LaunchDescription 19 | from launch.actions import DeclareLaunchArgument 20 | from launch.substitutions import LaunchConfiguration 21 | from launch_ros.actions import Node 22 | 23 | 24 | def generate_launch_description(): 25 | declare_loaded_description = DeclareLaunchArgument( 26 | 'loaded_description', 27 | default_value='', 28 | description='Set robot_description text. \ 29 | It is recommended to use RobotDescriptionLoader() in crane_plus_description.', 30 | ) 31 | 32 | crane_plus_controllers = os.path.join( 33 | get_package_share_directory('crane_plus_control'), 34 | 'config', 35 | 'crane_plus_controllers.yaml', 36 | ) 37 | 38 | controller_manager = Node( 39 | package='controller_manager', 40 | executable='ros2_control_node', 41 | output='screen', 42 | parameters=[ 43 | {'robot_description': LaunchConfiguration('loaded_description')}, 44 | crane_plus_controllers, 45 | ], 46 | ) 47 | 48 | spawn_joint_state_controller = Node( 49 | package='controller_manager', 50 | executable='spawner', 51 | output='screen', 52 | arguments=['joint_state_broadcaster'], 53 | ) 54 | 55 | spawn_arm_controller = Node( 56 | package='controller_manager', 57 | executable='spawner', 58 | output='screen', 59 | arguments=['crane_plus_arm_controller'], 60 | ) 61 | 62 | spawn_gripper_controller = Node( 63 | package='controller_manager', 64 | executable='spawner', 65 | output='screen', 66 | arguments=['crane_plus_gripper_controller'], 67 | ) 68 | 69 | return LaunchDescription( 70 | [ 71 | declare_loaded_description, 72 | controller_manager, 73 | spawn_joint_state_controller, 74 | spawn_arm_controller, 75 | spawn_gripper_controller, 76 | ] 77 | ) 78 | -------------------------------------------------------------------------------- /crane_plus_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crane_plus_control 5 | 3.0.0 6 | CRANE+ V2 control package 7 | RT Corporation 8 | Apache License 2.0 9 | 10 | Shota Aoki 11 | Atsushi Kuwagata 12 | Yusuke Kato 13 | 14 | ament_cmake 15 | 16 | controller_manager 17 | crane_plus_description 18 | dynamixel_sdk 19 | hardware_interface 20 | pluginlib 21 | rclcpp 22 | ros2_controllers 23 | ros2controlcli 24 | xacro 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /crane_plus_control/src/crane_plus_hardware.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "crane_plus_control/crane_plus_hardware.hpp" 22 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 23 | 24 | 25 | namespace crane_plus_control 26 | { 27 | 28 | CranePlusHardware::~CranePlusHardware() 29 | { 30 | driver_->torque_enable(false); 31 | driver_->close_port(); 32 | } 33 | 34 | CallbackReturn CranePlusHardware::on_init( 35 | const hardware_interface::HardwareInfo & info) 36 | { 37 | if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { 38 | return CallbackReturn::ERROR; 39 | } 40 | 41 | // Get parameters from URDF 42 | // Initialize member variables 43 | std::string port_name = info_.hardware_parameters["port_name"]; 44 | int baudrate = std::stoi(info_.hardware_parameters["baudrate"]); 45 | timeout_seconds_ = std::stod(info_.hardware_parameters["timeout_seconds"]); 46 | read_velocities_ = std::stoi(info_.hardware_parameters["read_velocities"]); 47 | read_loads_ = std::stoi(info_.hardware_parameters["read_loads"]); 48 | read_voltages_ = std::stoi(info_.hardware_parameters["read_voltages"]); 49 | read_temperatures_ = std::stoi(info_.hardware_parameters["read_temperatures"]); 50 | 51 | std::vector dxl_id_list; 52 | for (auto joint : info_.joints) { 53 | if (joint.parameters["dxl_id"] != "") { 54 | dxl_id_list.push_back(std::stoi(joint.parameters["dxl_id"])); 55 | } else { 56 | RCLCPP_ERROR( 57 | rclcpp::get_logger("CranePlusHardware"), 58 | "Joint '%s' does not have 'dxl_id' parameter.", 59 | joint.name.c_str()); 60 | return CallbackReturn::ERROR; 61 | } 62 | } 63 | 64 | hw_position_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 65 | hw_position_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 66 | hw_velocity_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 67 | hw_load_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 68 | hw_voltage_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 69 | hw_temperature_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 70 | 71 | // Open a crane_plus_driver 72 | driver_ = std::make_shared(port_name, baudrate, dxl_id_list); 73 | if (!driver_->open_port()) { 74 | RCLCPP_ERROR( 75 | rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); 76 | return CallbackReturn::ERROR; 77 | } 78 | if (!driver_->torque_enable(false)) { 79 | RCLCPP_ERROR( 80 | rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); 81 | return CallbackReturn::ERROR; 82 | } 83 | 84 | // Verify that the interface required by CranePlusHardware is set in the URDF. 85 | for (const hardware_interface::ComponentInfo & joint : info_.joints) { 86 | if (joint.command_interfaces.size() != 1) { 87 | RCLCPP_ERROR( 88 | rclcpp::get_logger("CranePlusHardware"), 89 | "Joint '%s' has %lu command interfaces found. 1 expected.", 90 | joint.name.c_str(), joint.command_interfaces.size()); 91 | return CallbackReturn::ERROR; 92 | } 93 | 94 | if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { 95 | RCLCPP_ERROR( 96 | rclcpp::get_logger("CranePlusHardware"), 97 | "Joint '%s' have %s command interfaces found. '%s' expected.", 98 | joint.name.c_str(), joint.command_interfaces[0].name.c_str(), 99 | hardware_interface::HW_IF_POSITION); 100 | return CallbackReturn::ERROR; 101 | } 102 | } 103 | 104 | steady_clock_ = rclcpp::Clock(RCL_STEADY_TIME); 105 | 106 | 107 | return CallbackReturn::SUCCESS; 108 | } 109 | 110 | std::vector 111 | CranePlusHardware::export_state_interfaces() 112 | { 113 | std::vector state_interfaces; 114 | for (uint i = 0; i < info_.joints.size(); i++) { 115 | state_interfaces.emplace_back( 116 | hardware_interface::StateInterface( 117 | info_.joints[i].name, hardware_interface::HW_IF_POSITION, 118 | &hw_position_states_[i]) 119 | ); 120 | 121 | state_interfaces.emplace_back( 122 | hardware_interface::StateInterface( 123 | info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, 124 | &hw_velocity_states_[i]) 125 | ); 126 | 127 | state_interfaces.emplace_back( 128 | hardware_interface::StateInterface( 129 | info_.joints[i].name, "load", 130 | &hw_load_states_[i]) 131 | ); 132 | state_interfaces.emplace_back( 133 | hardware_interface::StateInterface( 134 | info_.joints[i].name, "voltage", 135 | &hw_voltage_states_[i]) 136 | ); 137 | state_interfaces.emplace_back( 138 | hardware_interface::StateInterface( 139 | info_.joints[i].name, "temperature", 140 | &hw_temperature_states_[i]) 141 | ); 142 | } 143 | 144 | return state_interfaces; 145 | } 146 | 147 | std::vector 148 | CranePlusHardware::export_command_interfaces() 149 | { 150 | std::vector command_interfaces; 151 | for (uint i = 0; i < info_.joints.size(); i++) { 152 | command_interfaces.emplace_back( 153 | hardware_interface::CommandInterface( 154 | info_.joints[i].name, hardware_interface::HW_IF_POSITION, 155 | &hw_position_commands_[i]) 156 | ); 157 | } 158 | 159 | return command_interfaces; 160 | } 161 | 162 | CallbackReturn CranePlusHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) 163 | { 164 | if (!driver_->torque_enable(false)) { 165 | RCLCPP_ERROR( 166 | rclcpp::get_logger("CranePlusHardware"), 167 | driver_->get_last_error_log().c_str()); 168 | return CallbackReturn::ERROR; 169 | } 170 | // Set current timestamp to disable the communication timeout. 171 | prev_comm_timestamp_ = steady_clock_.now(); 172 | timeout_has_printed_ = false; 173 | 174 | // Set current joint positions to hw_position_commands. 175 | read(prev_comm_timestamp_, rclcpp::Duration::from_seconds(0)); 176 | for (uint i = 0; i < hw_position_commands_.size(); i++) { 177 | hw_position_commands_[i] = hw_position_states_[i]; 178 | } 179 | 180 | return CallbackReturn::SUCCESS; 181 | } 182 | 183 | CallbackReturn CranePlusHardware::on_deactivate( 184 | const rclcpp_lifecycle::State & /*previous_state*/) 185 | { 186 | driver_->torque_enable(false); 187 | 188 | return CallbackReturn::SUCCESS; 189 | } 190 | 191 | return_type CranePlusHardware::read( 192 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 193 | { 194 | if (communication_timeout()) { 195 | if (!timeout_has_printed_) { 196 | RCLCPP_ERROR( 197 | rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); 198 | timeout_has_printed_ = true; 199 | } 200 | return return_type::ERROR; 201 | } 202 | 203 | std::vector joint_positions; 204 | if (!driver_->read_present_joint_positions(joint_positions)) { 205 | RCLCPP_ERROR( 206 | rclcpp::get_logger("CranePlusHardware"), 207 | driver_->get_last_error_log().c_str()); 208 | return return_type::ERROR; 209 | } else { 210 | for (uint i = 0; i < hw_position_states_.size(); ++i) { 211 | hw_position_states_[i] = joint_positions[i]; 212 | } 213 | } 214 | 215 | // Reading joint speeds, loads, voltages or temperatures 216 | // causes a decrease of the communication rate. 217 | if (read_velocities_) { 218 | std::vector joint_speeds; 219 | if (driver_->read_present_joint_speeds(joint_speeds)) { 220 | for (uint i = 0; i < hw_velocity_states_.size(); ++i) { 221 | hw_velocity_states_[i] = joint_speeds[i]; 222 | } 223 | } 224 | } 225 | 226 | if (read_loads_) { 227 | std::vector joint_loads; 228 | if (driver_->read_present_joint_loads(joint_loads)) { 229 | for (uint i = 0; i < hw_load_states_.size(); ++i) { 230 | hw_load_states_[i] = joint_loads[i]; 231 | } 232 | } 233 | } 234 | 235 | if (read_voltages_) { 236 | std::vector joint_voltages; 237 | if (driver_->read_present_joint_voltages(joint_voltages)) { 238 | for (uint i = 0; i < hw_voltage_states_.size(); ++i) { 239 | hw_voltage_states_[i] = joint_voltages[i]; 240 | } 241 | } 242 | } 243 | 244 | 245 | if (read_temperatures_) { 246 | std::vector joint_temperatures; 247 | if (driver_->read_present_joint_temperatures(joint_temperatures)) { 248 | for (uint i = 0; i < hw_temperature_states_.size(); ++i) { 249 | hw_temperature_states_[i] = joint_temperatures[i]; 250 | } 251 | } 252 | } 253 | 254 | prev_comm_timestamp_ = steady_clock_.now(); 255 | return return_type::OK; 256 | } 257 | 258 | return_type CranePlusHardware::write( 259 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 260 | { 261 | if (communication_timeout()) { 262 | if (!timeout_has_printed_) { 263 | RCLCPP_ERROR( 264 | rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); 265 | timeout_has_printed_ = true; 266 | } 267 | return return_type::ERROR; 268 | } 269 | 270 | if (!driver_->write_goal_joint_positions(hw_position_commands_)) { 271 | RCLCPP_ERROR( 272 | rclcpp::get_logger("CranePlusHardware"), 273 | driver_->get_last_error_log().c_str()); 274 | return return_type::ERROR; 275 | } 276 | 277 | prev_comm_timestamp_ = steady_clock_.now(); 278 | return return_type::OK; 279 | } 280 | 281 | bool CranePlusHardware::communication_timeout() 282 | { 283 | if (steady_clock_.now().seconds() - prev_comm_timestamp_.seconds() >= timeout_seconds_) { 284 | return true; 285 | } else { 286 | return false; 287 | } 288 | } 289 | 290 | } // namespace crane_plus_control 291 | 292 | #include "pluginlib/class_list_macros.hpp" 293 | 294 | PLUGINLIB_EXPORT_CLASS( 295 | crane_plus_control::CranePlusHardware, 296 | hardware_interface::SystemInterface 297 | ) 298 | -------------------------------------------------------------------------------- /crane_plus_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package crane_plus_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-01-31) 6 | ------------------ 7 | * Mock components対応 (`#86 `_) 8 | * Jazzy対応 (`#78 `_) 9 | * Gazebo環境にカメラを追加 (`#83 `_) 10 | * テスト実行時はダミーのパラメータファイルを読み込めるようにdescription_loaderを修正 (`#73 `_) 11 | * Contributors: Kuwamai, YusukeKato, mizonon 12 | 13 | 2.0.1 (2023-09-08) 14 | ------------------ 15 | * テスト実行時はダミーのパラメータファイルを読み込めるようにdescription_loaderを修正 (`#73 `_) 16 | * Contributors: YusukeKato 17 | 18 | 2.0.0 (2023-08-01) 19 | ------------------ 20 | * カメラサンプルのHumble対応 (`#64 `_) 21 | * Feature/support humble (`#58 `_) 22 | * Contributors: Kuwamai, Shota Aoki, YusukeKato 23 | 24 | 1.1.0 (2022-08-16) 25 | ------------------ 26 | * Update author tags (`#49 `_) 27 | * effort limitをサーボのカタログ値に変更 (`#46 `_) 28 | * Rename CRANE+V2 to CRANE+ V2 (`#44 `_) 29 | * Contributors: Atsushi Kuwagata, Shota Aoki 30 | 31 | 1.0.0 (2022-06-22) 32 | ------------------ 33 | * パッケージバージョン表記の更新 (`#40 `_) 34 | * crane_plus_ignition パッケージを追加し、Gazeboの使用を非推奨にする (`#38 `_) 35 | * xacroファイルの読み込みを一元化するためのPythonスクリプトを追加 (`#36 `_) 36 | * hardware_interfaceのパラメータをxacro引数から変更する (`#35 `_) 37 | * Update crane_plus_gazebo (`#30 `_) 38 | * Use new ros2 control interface (`#27 `_) 39 | * Contributors: Atsushi Kuwagata, Shota Aoki 40 | 41 | 0.1.0 (2020-11-11) 42 | ------------------ 43 | * Update maintainer and author in package.xml (`#23 `_) 44 | * update package.xml (`#17 `_) 45 | * Update crane_plus_description (`#10 `_) 46 | * Refactor to pass ament_lint check (`#9 `_) 47 | * Update mesh (STL) files. (`#6 `_) 48 | * Add crane_plus_gazebo (`#5 `_) 49 | * Update xacro (`#1 `_) 50 | * First commit -m 51 | * Contributors: Shota Aoki 52 | -------------------------------------------------------------------------------- /crane_plus_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crane_plus_description) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(ament_cmake_python REQUIRED) 6 | 7 | install(DIRECTORY 8 | meshes 9 | urdf 10 | launch 11 | DESTINATION share/${PROJECT_NAME}/ 12 | ) 13 | 14 | ament_python_install_package(${PROJECT_NAME}) 15 | install(PROGRAMS 16 | ${PROJECT_NAME}/robot_description_loader.py 17 | DESTINATION lib/${PROJECT_NAME}) 18 | 19 | if(BUILD_TESTING) 20 | find_package(ament_lint_auto REQUIRED) 21 | ament_lint_auto_find_test_dependencies() 22 | 23 | find_package(ament_cmake_pytest REQUIRED) 24 | ament_add_pytest_test(test_robot_description_loader test/test_robot_description_loader.py) 25 | endif() 26 | 27 | ament_package() 28 | -------------------------------------------------------------------------------- /crane_plus_description/README.md: -------------------------------------------------------------------------------- 1 | # crane_plus_description 2 | 3 | このパッケージはCRANE+ V2のモデルデータ(xacro)を所持しています。 4 | 5 | ## display robot model 6 | 7 | 下記のコマンドを実行して、`robot_state_publisher`、`joint_state_publisher`、`rviz2`を起動します。 8 | 9 | CRANE+ V2のモデルが表示されるので、xacroファイルのデバッグに役立ちます。 10 | 11 | ```sh 12 | $ ros2 launch crane_plus_description display.launch.py 13 | ``` 14 | 15 | Webカメラ搭載モデルの場合は、下記のコマンドを実行してください。 16 | 17 | ```sh 18 | $ ros2 launch crane_plus_description display.launch.py use_camera:=true 19 | ``` 20 | 21 | ![display.launch.py](https://rt-net.github.io/images/crane-plus/display_launch.png) 22 | 23 | ## configure servo angle limits 24 | 25 | CRANE+ V2の実機を動かす場合は、 26 | 事前にサーボモータ内部の角度リミット（`CW Angle Limit`、`CCW Angle Limit`）を設定してください。 27 | 28 | CRANE+ V2に搭載されているサーボモータはROBOTISのAX-12Aのため、 29 | [Dynamixel Wizard 2](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/) 30 | を使用して角度リミットを設定できます。 31 | 32 | ![dynamixel wizard2](https://rt-net.github.io/images/crane-plus/dynamixel_wizard2.png) 33 | 34 | [crane_plus.urdf.xacro](./urdf/crane_plus.urdf.xacro)には、 35 | 下記のように各関節の角度リミットが定義されています。 36 | この角度リミットに近い値をサーボモータに設定することを推奨します。 37 | 38 | ```xml 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | ``` 53 | -------------------------------------------------------------------------------- /crane_plus_description/crane_plus_description/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/crane_plus_description/__init__.py -------------------------------------------------------------------------------- /crane_plus_description/crane_plus_description/robot_description_loader.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | from launch.substitutions import Command 19 | 20 | 21 | class RobotDescriptionLoader(): 22 | 23 | def __init__(self): 24 | self.robot_description_path = os.path.join( 25 | get_package_share_directory('crane_plus_description'), 26 | 'urdf', 27 | 'crane_plus.urdf.xacro') 28 | self.port_name = '/dev/ttyUSB0' 29 | self.use_gazebo = 'false' 30 | self.use_camera = 'false' 31 | self.use_mock_components = 'false' 32 | self.gz_control_config_package = '' 33 | self.gz_control_config_file_path = '' 34 | 35 | def load(self): 36 | return Command([ 37 | 'xacro ', 38 | self.robot_description_path, 39 | ' port_name:=', self.port_name, 40 | ' use_gazebo:=', self.use_gazebo, 41 | ' use_camera:=', self.use_camera, 42 | ' use_mock_components:=', self.use_mock_components, 43 | ' gz_control_config_package:=', self.gz_control_config_package, 44 | ' gz_control_config_file_path:=', self.gz_control_config_file_path 45 | ]) 46 | -------------------------------------------------------------------------------- /crane_plus_description/launch/display.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | from ament_index_python.packages import get_package_share_directory 17 | from crane_plus_description.robot_description_loader import RobotDescriptionLoader 18 | from launch import LaunchDescription 19 | from launch.actions import DeclareLaunchArgument 20 | from launch.substitutions import LaunchConfiguration 21 | from launch_ros.actions import Node 22 | 23 | 24 | def generate_launch_description(): 25 | declare_use_camera = DeclareLaunchArgument( 26 | 'use_camera', 27 | default_value='false', 28 | description='Set true to attach the camera model.', 29 | ) 30 | 31 | description_loader = RobotDescriptionLoader() 32 | description_loader.use_camera = LaunchConfiguration('use_camera') 33 | 34 | rsp = Node( 35 | package='robot_state_publisher', 36 | executable='robot_state_publisher', 37 | output='both', 38 | parameters=[{'robot_description': description_loader.load()}], 39 | ) 40 | jsp = Node( 41 | package='joint_state_publisher_gui', 42 | executable='joint_state_publisher_gui', 43 | output='screen', 44 | ) 45 | 46 | rviz_config_file = ( 47 | get_package_share_directory('crane_plus_description') + '/launch/display.rviz' 48 | ) 49 | rviz_node = Node( 50 | name='rviz2', 51 | package='rviz2', 52 | executable='rviz2', 53 | output='log', 54 | arguments=['-d', rviz_config_file], 55 | ) 56 | 57 | return LaunchDescription( 58 | [ 59 | declare_use_camera, 60 | rsp, 61 | jsp, 62 | rviz_node, 63 | ] 64 | ) 65 | -------------------------------------------------------------------------------- /crane_plus_description/launch/display.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 775 9 | - Class: rviz_common/Selection 10 | Name: Selection 11 | - Class: rviz_common/Tool Properties 12 | Expanded: 13 | - /2D Goal Pose1 14 | - /Publish Point1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz_common/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | Visualization Manager: 23 | Class: "" 24 | Displays: 25 | - Alpha: 0.5 26 | Cell Size: 1 27 | Class: rviz_default_plugins/Grid 28 | Color: 160; 160; 164 29 | Enabled: true 30 | Line Style: 31 | Line Width: 0.029999999329447746 32 | Value: Lines 33 | Name: Grid 34 | Normal Cell Count: 0 35 | Offset: 36 | X: 0 37 | Y: 0 38 | Z: 0 39 | Plane: XY 40 | Plane Cell Count: 10 41 | Reference Frame: 42 | Value: true 43 | - Alpha: 1 44 | Class: rviz_default_plugins/RobotModel 45 | Collision Enabled: false 46 | Description File: "" 47 | Description Source: Topic 48 | Description Topic: 49 | Depth: 5 50 | Durability Policy: Volatile 51 | History Policy: Keep Last 52 | Reliability Policy: Reliable 53 | Value: /robot_description 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | camera_color_frame: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | camera_color_optical_frame: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | camera_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | camera_stand_link1: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | camera_stand_link2: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | camera_stand_link3: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | camera_stand_link4: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | camera_stand_link5: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | crane_plus_base: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | crane_plus_link1: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | crane_plus_link2: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | crane_plus_link3: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | crane_plus_link4: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | crane_plus_link_hand: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | world: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Mass Properties: 138 | Inertia: false 139 | Mass: false 140 | Name: RobotModel 141 | TF Prefix: "" 142 | Update Interval: 0 143 | Value: true 144 | Visual Enabled: true 145 | - Class: rviz_default_plugins/TF 146 | Enabled: true 147 | Filter (blacklist): "" 148 | Filter (whitelist): "" 149 | Frame Timeout: 15 150 | Frames: 151 | All Enabled: true 152 | base_link: 153 | Value: true 154 | camera_color_frame: 155 | Value: true 156 | camera_color_optical_frame: 157 | Value: true 158 | camera_link: 159 | Value: true 160 | camera_stand_link1: 161 | Value: true 162 | camera_stand_link2: 163 | Value: true 164 | camera_stand_link3: 165 | Value: true 166 | camera_stand_link4: 167 | Value: true 168 | camera_stand_link5: 169 | Value: true 170 | crane_plus_base: 171 | Value: true 172 | crane_plus_link1: 173 | Value: true 174 | crane_plus_link2: 175 | Value: true 176 | crane_plus_link3: 177 | Value: true 178 | crane_plus_link4: 179 | Value: true 180 | crane_plus_link_hand: 181 | Value: true 182 | world: 183 | Value: true 184 | Marker Scale: 0.30000001192092896 185 | Name: TF 186 | Show Arrows: false 187 | Show Axes: true 188 | Show Names: true 189 | Tree: 190 | world: 191 | base_link: 192 | crane_plus_base: 193 | camera_link: 194 | camera_color_frame: 195 | camera_color_optical_frame: 196 | {} 197 | camera_stand_link1: 198 | camera_stand_link2: 199 | camera_stand_link3: 200 | {} 201 | camera_stand_link4: 202 | {} 203 | camera_stand_link5: 204 | {} 205 | crane_plus_link1: 206 | crane_plus_link2: 207 | crane_plus_link3: 208 | crane_plus_link4: 209 | crane_plus_link_hand: 210 | {} 211 | Update Interval: 0 212 | Value: true 213 | Enabled: true 214 | Global Options: 215 | Background Color: 48; 48; 48 216 | Fixed Frame: base_link 217 | Frame Rate: 30 218 | Name: root 219 | Tools: 220 | - Class: rviz_default_plugins/Interact 221 | Hide Inactive Objects: true 222 | - Class: rviz_default_plugins/MoveCamera 223 | - Class: rviz_default_plugins/Select 224 | - Class: rviz_default_plugins/FocusCamera 225 | - Class: rviz_default_plugins/Measure 226 | Line color: 128; 128; 0 227 | - Class: rviz_default_plugins/SetInitialPose 228 | Covariance x: 0.25 229 | Covariance y: 0.25 230 | Covariance yaw: 0.06853891909122467 231 | Topic: 232 | Depth: 5 233 | Durability Policy: Volatile 234 | History Policy: Keep Last 235 | Reliability Policy: Reliable 236 | Value: /initialpose 237 | - Class: rviz_default_plugins/SetGoal 238 | Topic: 239 | Depth: 5 240 | Durability Policy: Volatile 241 | History Policy: Keep Last 242 | Reliability Policy: Reliable 243 | Value: /goal_pose 244 | - Class: rviz_default_plugins/PublishPoint 245 | Single click: true 246 | Topic: 247 | Depth: 5 248 | Durability Policy: Volatile 249 | History Policy: Keep Last 250 | Reliability Policy: Reliable 251 | Value: /clicked_point 252 | Transformation: 253 | Current: 254 | Class: rviz_default_plugins/TF 255 | Value: true 256 | Views: 257 | Current: 258 | Class: rviz_default_plugins/Orbit 259 | Distance: 0.800000011920929 260 | Enable Stereo Rendering: 261 | Stereo Eye Separation: 0.05999999865889549 262 | Stereo Focal Distance: 1 263 | Swap Stereo Eyes: false 264 | Value: false 265 | Focal Point: 266 | X: 0 267 | Y: 0 268 | Z: 0.20000000298023224 269 | Focal Shape Fixed Size: true 270 | Focal Shape Size: 0.05000000074505806 271 | Invert Z Axis: false 272 | Name: Current View 273 | Near Clip Distance: 0.009999999776482582 274 | Pitch: 0.5 275 | Target Frame: 276 | Value: Orbit (rviz) 277 | Yaw: 0.5 278 | Saved: ~ 279 | Window Geometry: 280 | Displays: 281 | collapsed: false 282 | Height: 1011 283 | Hide Left Dock: false 284 | Hide Right Dock: false 285 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000395fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000395000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000395fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000395000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c90000039500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 286 | Selection: 287 | collapsed: false 288 | Tool Properties: 289 | collapsed: false 290 | Views: 291 | collapsed: false 292 | Width: 1850 293 | X: 70 294 | Y: 32 295 | -------------------------------------------------------------------------------- /crane_plus_description/meshes/collision/body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/collision/body.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/collision/hand.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/collision/link1.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/collision/link2.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/collision/link3.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/collision/link4.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/visual/body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/visual/body.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/visual/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/visual/hand.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/visual/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/visual/link1.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/visual/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/visual/link2.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/visual/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/visual/link3.stl -------------------------------------------------------------------------------- /crane_plus_description/meshes/visual/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_description/meshes/visual/link4.stl -------------------------------------------------------------------------------- /crane_plus_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crane_plus_description 5 | 3.0.0 6 | CRANE+ V2 description package 7 | RT Corporation 8 | Apache License 2.0 9 | 10 | Shota Aoki 11 | Atsushi Kuwagata 12 | Nozomi Mizoguchi 13 | 14 | ament_cmake 15 | ament_cmake_python 16 | 17 | gz_ros2_control 18 | joint_state_publisher_gui 19 | launch 20 | robot_state_publisher 21 | rviz2 22 | xacro 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | ament_cmake_pytest 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /crane_plus_description/test/dummy_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | crane_plus_arm_controller: 6 | type: joint_trajectory_controller/JointTrajectoryController 7 | crane_plus_gripper_controller: 8 | type: joint_trajectory_controller/JointTrajectoryController 9 | joint_state_broadcaster: 10 | type: joint_state_broadcaster/JointStateBroadcaster 11 | 12 | crane_plus_arm_controller: 13 | ros__parameters: 14 | joints: 15 | - crane_plus_joint1 16 | - crane_plus_joint2 17 | - crane_plus_joint3 18 | - crane_plus_joint4 19 | 20 | command_interfaces: 21 | - position 22 | 23 | state_interfaces: 24 | - position 25 | 26 | crane_plus_gripper_controller: 27 | ros__parameters: 28 | joints: 29 | - crane_plus_joint_hand 30 | 31 | command_interfaces: 32 | - position 33 | 34 | state_interfaces: 35 | - position 36 | -------------------------------------------------------------------------------- /crane_plus_description/test/test_robot_description_loader.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from crane_plus_description.robot_description_loader import RobotDescriptionLoader 16 | from launch.launch_context import LaunchContext 17 | import pytest 18 | 19 | 20 | def exec_load(loader): 21 | # Command substitutionの実行方法はCommandのテストを参考にした 22 | # https://github.com/ros2/launch/blob/074cd2903ddccd61bce8f40a0f58da0b7c200481/launch/test/launch/substitutions/test_command.py#L47 23 | context = LaunchContext() 24 | return loader.load().perform(context) 25 | 26 | 27 | def test_load_description(): 28 | # xacroの読み込みが成功することを期待 29 | rdl = RobotDescriptionLoader() 30 | assert exec_load(rdl) 31 | 32 | 33 | def test_change_description_path(): 34 | # xacroのファイルパスを変更し、読み込みが失敗することを期待 35 | rdl = RobotDescriptionLoader() 36 | rdl.robot_description_path = 'hoge' 37 | with pytest.raises(Exception) as e: 38 | exec_load(rdl) 39 | assert e.value 40 | 41 | 42 | def test_port_name(): 43 | # port_nameが変更され、xacroにポート名がセットされることを期待 44 | rdl = RobotDescriptionLoader() 45 | rdl.port_name = '/dev/ttyUSB1' 46 | assert '/dev/ttyUSB1' in exec_load(rdl) 47 | 48 | 49 | def test_use_gazebo(): 50 | # use_gazeboが変更され、xacroにgazebo_ros2_controlがセットされることを期待 51 | rdl = RobotDescriptionLoader() 52 | rdl.use_gazebo = 'true' 53 | rdl.gz_control_config_package = 'crane_plus_description' 54 | rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml' 55 | assert 'gz_ros2_control/GazeboSimSystem' in exec_load(rdl) 56 | 57 | 58 | def test_use_camera(): 59 | # use_cameraが変更されて、xacroにcameraがセットされることを期待 60 | rdl = RobotDescriptionLoader() 61 | rdl.use_camera = 'true' 62 | rdl.gz_control_config_package = 'crane_plus_description' 63 | rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml' 64 | assert 'camera_link' in exec_load(rdl) 65 | 66 | 67 | def test_mock_components(): 68 | # use_mock_componentsが変更されて、xacroにmock_componentsがセットされることを期待 69 | rdl = RobotDescriptionLoader() 70 | rdl.use_mock_components = 'true' 71 | rdl.gz_control_config_package = 'crane_plus_description' 72 | rdl.gz_control_config_file_path = 'test/dummy_controllers.yaml' 73 | assert 'mock_components/GenericSystem' in exec_load(rdl) 74 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/camera.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/camera_stand.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/crane_plus.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0 0 0 1 8 | 0 0 0 1 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 1 1 1 1 17 | 1 1 1 1 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | $(find ${GZ_CONTROL_CONFIG_PACKAGE})/${GZ_CONTROL_CONFIG_FILE_PATH} 27 | 28 | 29 | 30 | 31 | 0.2 32 | 0.2 33 | 34 | 35 | 36 | 37 | 0.2 38 | 0.2 39 | 40 | 41 | 42 | 43 | 0.2 44 | 0.2 45 | 46 | 47 | 48 | 49 | 0.2 50 | 0.2 51 | 52 | 53 | 54 | 55 | 0.2 56 | 0.2 57 | 58 | 59 | 60 | 61 | 0.2 62 | 0.2 63 | 64 | 65 | 66 | 67 | 68 | 69 | ogre2 70 | 71 | 72 | 73 | 74 | 75 | 0 0 0 ${radians(90)} ${radians(-90)} 0 76 | 10 77 | true 78 | image_raw 79 | camera_color_optical_frame 80 | 81 | 82 | 83 | 84 | 85 | ${camera_hfov} 86 | 87 | ${camera_width} 88 | ${camera_height} 89 | 90 | 91 | 0.1 92 | 10.0 93 | 94 | 95 | 96 | ${camera_focal_length} 97 | ${camera_focal_length} 98 | ${(camera_width + 1) / 2} 99 | ${(camera_height + 1) / 2} 100 | 1.0 101 | 102 | 103 | ${camera_focal_length} 104 | ${camera_focal_length} 105 | ${(camera_width + 1) / 2} 106 | ${(camera_height + 1) / 2} 107 | 0 108 | 0 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | gz_ros2_control/GazeboSimSystem 9 | 10 | 11 | 12 | 13 | ${JOINT_1_LOWER_LIMIT} 14 | ${JOINT_1_UPPER_LIMIT} 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | ${JOINT_2_LOWER_LIMIT} 24 | ${JOINT_2_UPPER_LIMIT} 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | ${JOINT_3_LOWER_LIMIT} 34 | ${JOINT_3_UPPER_LIMIT} 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | ${JOINT_4_LOWER_LIMIT} 44 | ${JOINT_4_UPPER_LIMIT} 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | ${JOINT_HAND_LOWER_LIMIT} 54 | ${JOINT_HAND_UPPER_LIMIT} 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/crane_plus.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | mock_components/GenericSystem 10 | 11 | 12 | crane_plus_hardware/CranePlusHardware 13 | ${PORT_NAME} 14 | ${BAUDRATE} 15 | ${TIMEOUT_SECONDS} 16 | ${READ_VELOCITIES} 17 | ${READ_LOADS} 18 | ${READ_VOLTAGES} 19 | ${READ_TEMPERATURES} 20 | 21 | 22 | 23 | 24 | 25 | ${JOINT_1_LOWER_LIMIT} 26 | ${JOINT_1_UPPER_LIMIT} 27 | 28 | 29 | 30 | 31 | 32 | 33 | 1 34 | 35 | 36 | 37 | 38 | ${JOINT_2_LOWER_LIMIT} 39 | ${JOINT_2_UPPER_LIMIT} 40 | 41 | 42 | 43 | 44 | 45 | 46 | 2 47 | 48 | 49 | 50 | 51 | ${JOINT_3_LOWER_LIMIT} 52 | ${JOINT_3_UPPER_LIMIT} 53 | 54 | 55 | 56 | 57 | 58 | 59 | 3 60 | 61 | 62 | 63 | 64 | ${JOINT_4_LOWER_LIMIT} 65 | ${JOINT_4_UPPER_LIMIT} 66 | 67 | 68 | 69 | 70 | 71 | 72 | 4 73 | 74 | 75 | 76 | 77 | ${JOINT_HAND_LOWER_LIMIT} 78 | ${JOINT_HAND_UPPER_LIMIT} 79 | 80 | 81 | 82 | 83 | 84 | 85 | 5 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/crane_plus.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /crane_plus_description/urdf/crane_plus.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | -------------------------------------------------------------------------------- /crane_plus_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package crane_plus_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-01-31) 6 | ------------------ 7 | * Mock components対応 (`#86 `_) 8 | * Jazzy対応 (`#78 `_) 9 | * Gazebo環境にカメラを追加 (`#83 `_) 10 | * READMEにカメラサンプルデモのGIFを追加 (`#70 `_) 11 | * Contributors: Kuwamai, YusukeKato, mizonon 12 | 13 | 2.0.1 (2023-09-08) 14 | ------------------ 15 | * READMEにカメラサンプルデモのGIFを追加 (`#70 `_) 16 | * Contributors: YusukeKato 17 | 18 | 2.0.0 (2023-08-01) 19 | ------------------ 20 | * 軌道生成の失敗時にpick_and_placeを中断する機能を追加 (`#65 `_) 21 | * カメラサンプルのHumble対応 (`#64 `_) 22 | * Feature/support humble (`#58 `_) 23 | * Contributors: Shota Aoki, YusukeKato 24 | 25 | 1.1.0 (2022-08-16) 26 | ------------------ 27 | * Update author tags (`#49 `_) 28 | * Rename CRANE+V2 to CRANE+ V2 (`#44 `_) 29 | * Contributors: Shota Aoki 30 | 31 | 1.0.0 (2022-06-22) 32 | ------------------ 33 | * パッケージバージョン表記の更新 (`#40 `_) 34 | * crane_plus_ignition パッケージを追加し、Gazeboの使用を非推奨にする (`#38 `_) 35 | * xacroファイルの読み込みを一元化するためのPythonスクリプトを追加 (`#36 `_) 36 | * GripperActionControllerに関するコメントを削除 (`#37 `_) 37 | * hardware_interfaceのパラメータをxacro引数から変更する (`#35 `_) 38 | * Update crane_plus_gazebo (`#30 `_) 39 | * Contributors: Atsushi Kuwagata, Shota Aoki 40 | 41 | 0.1.0 (2020-11-11) 42 | ------------------ 43 | * Fix READMEs (`#25 `_) 44 | * Update maintainer and author in package.xml (`#23 `_) 45 | * Update for release (`#21 `_) 46 | * Update crane_plus_examples (`#18 `_) 47 | * Add joint_values example and pick_and_place example (`#15 `_) 48 | * Add pose_groupstate example. (`#14 `_) 49 | * Add crane_plus_examples (`#13 `_) 50 | * Contributors: Shota Aoki 51 | -------------------------------------------------------------------------------- /crane_plus_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crane_plus_examples) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(cv_bridge REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(moveit_ros_planning_interface REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(tf2_geometry_msgs REQUIRED) 25 | find_package(image_geometry REQUIRED) 26 | 27 | # Build and install node executables 28 | set(executable_list 29 | gripper_control 30 | pose_groupstate 31 | joint_values 32 | pick_and_place 33 | pick_and_place_tf 34 | color_detection 35 | aruco_detection 36 | ) 37 | foreach(loop_var IN LISTS executable_list) 38 | add_executable(${loop_var} src/${loop_var}.cpp) 39 | ament_target_dependencies(${loop_var} 40 | cv_bridge 41 | geometry_msgs 42 | moveit_ros_planning_interface 43 | OpenCV 44 | rclcpp 45 | tf2_geometry_msgs 46 | image_geometry 47 | ) 48 | 49 | install(TARGETS 50 | ${loop_var} 51 | DESTINATION lib/${PROJECT_NAME} 52 | ) 53 | endforeach() 54 | 55 | 56 | install(DIRECTORY 57 | launch 58 | config 59 | DESTINATION share/${PROJECT_NAME}/ 60 | ) 61 | 62 | if(BUILD_TESTING) 63 | find_package(ament_lint_auto REQUIRED) 64 | ament_lint_auto_find_test_dependencies() 65 | endif() 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /crane_plus_examples/README.md: -------------------------------------------------------------------------------- 1 | # crane_plus_examples 2 | 3 | このパッケージはCRANE+ V2 ROS 2パッケージのサンプルコード集です。 4 | 5 | ## 準備（実機を使う場合） 6 | 7 | ![crane_plus](https://rt-net.github.io/images/crane-plus/CRANEV2-500x500.png) 8 | 9 | ### 1. CRANE+ V2本体をPCに接続する 10 | 11 | CRANE+ V2本体をPCに接続します。 12 | 接続方法は製品マニュアルを参照してください。 13 | 14 | **※CRANE+ V2本体が接触しないように、十分なスペースを確保してください。** 15 | 16 | ### 2. USB通信ポートの接続を確認する 17 | 18 | USB通信ポートの設定については`crane_plus_control`の 19 | [README](../crane_plus_control/README.md) 20 | を参照してください。 21 | 22 | **正しく設定できていない場合、CRANE+ V2が動作しない、振動する、などの不安定な動きになるので注意してください** 23 | 24 | ### 3. move_groupとcontrollerを起動する 25 | 26 | #### 標準のCRANE+ V2を使用する場合 27 | 28 | 次のコマンドでmove_group (`crane_plus_moveit_config`)と 29 | controller (`crane_plus_control`)を起動します。 30 | 31 | ```sh 32 | $ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 33 | ``` 34 | 35 | #### Webカメラ搭載モデルを使用する場合 36 | 37 | Webカメラ搭載モデルの場合は、次のコマンドを実行してください。 38 | ```video_device```は使用するWebカメラを指定してください。 39 | 40 | ```sh 41 | $ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 use_camera:=true video_device:=/dev/video0 42 | ``` 43 | 44 | ## 準備（Gazeboを使う場合） 45 | ======= 46 | ![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png) 47 | 48 | ### 1. move_groupとGazeboを起動する 49 | 50 | 次のコマンドでmove_group (`crane_plus_moveit_config`)とGazeboを起動します。 51 | 52 | ```sh 53 | $ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py 54 | ``` 55 | 56 | #### Webカメラ搭載モデルを使用する場合 57 | 58 | Webカメラ搭載モデルの場合は、次のコマンドを実行してください。 59 | 60 | ```sh 61 | $ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py use_camera:=true 62 | ``` 63 | 64 | CRANE+ V2の前にArUcoマーカ付きのBoxを置いたシミュレータ環境を使用する場合は次のコマンドを実行します。 65 | [aruco\_detection](#aruco_detection)サンプルを実行する際に使用することを想定しています。 66 | 67 | ```sh 68 | $ ros2 launch crane_plus_gazebo crane_plus_with_aruco_cube.launch.py use_camera:=true 69 | ``` 70 | 71 | CRANE+ V2の前に赤いBoxを置いたシミュレータ環境を使用する場合は次のコマンドを実行します。 72 | [color\_detection](#color_detection)サンプルを実行する際に使用すること想定しています。 73 | 74 | ```sh 75 | $ ros2 launch crane_plus_gazebo crane_plus_with_red_cube.launch.py use_camera:=true 76 | ``` 77 | 78 | ## 準備（Mock Componentsを使う場合） 79 | 80 | ### 1. move_groupとcontrollerを起動する 81 | 82 | 次のコマンドでmove_group (`crane_plus_moveit_config`)と 83 | controller (`crane_plus_control`)を起動します。 84 | 85 | ```sh 86 | $ ros2 launch crane_plus_examples demo.launch.py use_mock_components:=true 87 | ``` 88 | 89 | Mock Componentsではカメラを使ったサンプルを実行することはできません。 90 | 91 | ## サンプルプログラムを実行する 92 | 93 | 準備ができたらサンプルプログラムを実行します。 94 | 例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 95 | 96 | ```sh 97 | $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' 98 | ``` 99 | 100 | 終了するときは`Ctrl+c`を入力します。 101 | 102 | ## Gazeboでサンプルプログラムを実行する場合 103 | 104 | Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 105 | 106 | ```sh 107 | $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' use_sim_time:=true 108 | ``` 109 | 110 | ## Examples 111 | 112 | `demo.launch.py`を実行している状態で各サンプルを実行できます。 113 | 114 | - [gripper_control](#gripper_control) 115 | - [pose_groupstate](#pose_groupstate) 116 | - [joint_values](#joint_values) 117 | - [pick_and_place](#pick_and_place) 118 | 119 | 実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 120 | 121 | ```sh 122 | $ ros2 launch crane_plus_examples example.launch.py -s 123 | Arguments (pass arguments as ':='): 124 | 125 | 'example': 126 | Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place] 127 | (default: 'gripper_control') 128 | ``` 129 | 130 | --- 131 | 132 | ### gripper_control 133 | 134 | グリッパを開閉させるコード例です。 135 | 136 | 次のコマンドを実行します。 137 | 138 | ```sh 139 | $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' 140 | ``` 141 | 142 | 143 | 144 | [back to example list](#examples) 145 | 146 | --- 147 | 148 | ### pose_groupstate 149 | 150 | group_stateを使うコード例です。 151 | 152 | SRDFファイル[crane_plus_moveit_config/config/crane_plus.srdf](../crane_plus_moveit_config/config/crane_plus.srdf) 153 | に記載されている`home`と`vertical`の姿勢に移行します。 154 | 155 | 次のコマンドを実行します。 156 | 157 | ```sh 158 | $ ros2 launch crane_plus_examples example.launch.py example:='pose_groupstate' 159 | ``` 160 | 161 | 162 | 163 | [back to example list](#examples) 164 | 165 | --- 166 | 167 | ### joint_values 168 | 169 | アームのジョイント角度を１つずつ変更するコード例です。 170 | 171 | 次のコマンドを実行します。 172 | 173 | ```sh 174 | $ ros2 launch crane_plus_examples example.launch.py example:='joint_values' 175 | ``` 176 | 177 | 178 | 179 | [back to example list](#examples) 180 | 181 | --- 182 | 183 | ### pick_and_place 184 | 185 | モノを掴む・持ち上げる・運ぶ・置くコード例です。 186 | 187 | 次のコマンドを実行します。 188 | 189 | ```sh 190 | $ ros2 launch crane_plus_examples example.launch.py example:='pick_and_place' 191 | ``` 192 | 193 | 194 | 195 | [back to example list](#examples) 196 | 197 | --- 198 | 199 | ## Camera Examples 200 | 201 | Webカメラ搭載モデルのカメラを使用したサンプルコードです。 202 | 203 | [「Webカメラ搭載モデルを使用する場合」](#Webカメラ搭載モデルを使用する場合)の手順に従って、 204 | `demo.launch`を実行している状態で、 205 | 各サンプルを実行できます。 206 | 207 | - [aruco\_detection](#aruco_detection) 208 | - [color\_detection](#color_detection) 209 | 210 | 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 211 | 212 | ```sh 213 | $ ros2 launch crane_plus_examples camera_example.launch.py -s 214 | Arguments (pass arguments as ':='): 215 | 216 | 'example': 217 | Set an example executable name: [color_detection] 218 | (default: 'color_detection') 219 | ``` 220 | 221 | --- 222 | 223 | ### aruco_detection 224 | 225 | モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。 226 | マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷して、一辺50mmの立方体に取り付けて使用します。 227 | 228 | 検出されたマーカの位置姿勢はtfのフレームとして配信されます。 229 | tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。 230 | 掴む対象は`target_0`に設定されています。 231 | マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。 232 | 233 | 次のコマンドを実行します。 234 | 235 | ```bash 236 | ros2 launch crane_plus_examples camera_example.launch.py example:='aruco_detection' 237 | ``` 238 | 239 | #### Videos 240 | [![crane_plus_aruco_detection_demo](https://rt-net.github.io/images/crane-plus/aruco_detection.gif)](https://youtu.be/m9dus6LCocc) 241 | 242 | [back to example list](#examples) 243 | 244 | --- 245 | 246 | ### color_detection 247 | 248 | 特定の色の物体を検出して掴むコード例です。 249 | 250 | デフォルトでは赤い物体の位置をtfのフレームとして配信します。 251 | tfの`frame_id`は`target_0`です。 252 | 色検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。 253 | 254 | 次のコマンドを実行します。 255 | 256 | ```sh 257 | ros2 launch crane_plus_examples camera_example.launch.py example:='color_detection' 258 | ``` 259 | 260 | #### Videos 261 | [![crane_plus_color_detection_demo](https://rt-net.github.io/images/crane-plus/color_detection.gif)](https://youtu.be/Kn0eWA7sALY) 262 | 263 | [back to example list](#examples) 264 | 265 | --- 266 | -------------------------------------------------------------------------------- /crane_plus_examples/aruco_markers.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rt-net/crane_plus/809cc37fabf1bbd3baea77f7eb134c5e971887f3/crane_plus_examples/aruco_markers.pdf -------------------------------------------------------------------------------- /crane_plus_examples/config/camera_info.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [501.403812, 0.000000, 323.432723, 0.000000, 501.936444, 226.205450, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.013577, -0.043209, 0.000381, 0.000113, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [499.781769, 0.000000, 323.060085, 0.000000, 0.000000, 501.324219, 225.825888, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /crane_plus_examples/launch/camera_example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from crane_plus_description.robot_description_loader import RobotDescriptionLoader 16 | from launch import LaunchDescription 17 | from launch.actions import DeclareLaunchArgument 18 | from launch.substitutions import LaunchConfiguration 19 | from launch_ros.actions import Node 20 | from launch_ros.actions import SetParameter 21 | from moveit_configs_utils import MoveItConfigsBuilder 22 | 23 | 24 | def generate_launch_description(): 25 | declare_use_camera = DeclareLaunchArgument( 26 | 'use_camera', default_value='true', description='Use camera.' 27 | ) 28 | 29 | declare_example_name = DeclareLaunchArgument( 30 | 'example', 31 | default_value='color_detection', 32 | description=('Set an example executable name: [color_detection]'), 33 | ) 34 | 35 | declare_use_sim_time = DeclareLaunchArgument( 36 | 'use_sim_time', 37 | default_value='false', 38 | description=('Set true when using the gazebo simulator.'), 39 | ) 40 | 41 | description_loader = RobotDescriptionLoader() 42 | description_loader.use_camera = LaunchConfiguration('use_camera') 43 | 44 | moveit_config = MoveItConfigsBuilder('crane_plus').to_moveit_configs() 45 | moveit_config.robot_description = { 46 | 'robot_description': description_loader.load(), 47 | } 48 | 49 | picking_node = Node( 50 | name='pick_and_place_tf', 51 | package='crane_plus_examples', 52 | executable='pick_and_place_tf', 53 | output='screen', 54 | parameters=[moveit_config.to_dict()], 55 | ) 56 | 57 | detection_node = Node( 58 | name=[LaunchConfiguration('example'), '_node'], 59 | package='crane_plus_examples', 60 | executable=LaunchConfiguration('example'), 61 | output='screen', 62 | ) 63 | 64 | return LaunchDescription( 65 | [ 66 | declare_use_camera, 67 | declare_use_sim_time, 68 | SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), 69 | detection_node, 70 | picking_node, 71 | declare_example_name, 72 | ] 73 | ) 74 | -------------------------------------------------------------------------------- /crane_plus_examples/launch/demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_index_python.packages import get_package_share_directory 16 | from crane_plus_description.robot_description_loader import RobotDescriptionLoader 17 | from launch import LaunchDescription 18 | from launch.actions import DeclareLaunchArgument 19 | from launch.actions import IncludeLaunchDescription 20 | from launch.conditions import IfCondition 21 | from launch.conditions import UnlessCondition 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import Node 25 | 26 | 27 | def generate_launch_description(): 28 | declare_port_name = DeclareLaunchArgument( 29 | 'port_name', default_value='/dev/ttyUSB0', description='Set port name.' 30 | ) 31 | 32 | declare_use_camera = DeclareLaunchArgument( 33 | 'use_camera', default_value='false', description='Use camera.' 34 | ) 35 | 36 | declare_video_device = DeclareLaunchArgument( 37 | 'video_device', 38 | default_value='/dev/video0', 39 | description='Set video device.', 40 | ) 41 | 42 | declare_use_mock_components = DeclareLaunchArgument( 43 | 'use_mock_components', 44 | default_value='false', 45 | description='Use mock_components or not.', 46 | ) 47 | 48 | declare_rviz_config = DeclareLaunchArgument( 49 | 'rviz_config', 50 | default_value=get_package_share_directory('crane_plus_moveit_config') 51 | + '/config/moveit.rviz', 52 | description='Set the path to rviz configuration file.', 53 | condition=UnlessCondition(LaunchConfiguration('use_camera')), 54 | ) 55 | 56 | declare_rviz_config_camera = DeclareLaunchArgument( 57 | 'rviz_config', 58 | default_value=get_package_share_directory('crane_plus_examples') 59 | + '/launch/camera_example.rviz', 60 | description='Set the path to rviz configuration file.', 61 | condition=IfCondition(LaunchConfiguration('use_camera')), 62 | ) 63 | 64 | description_loader = RobotDescriptionLoader() 65 | description_loader.port_name = LaunchConfiguration('port_name') 66 | description_loader.use_camera = LaunchConfiguration('use_camera') 67 | description_loader.use_mock_components = LaunchConfiguration('use_mock_components') 68 | description = description_loader.load() 69 | 70 | move_group = IncludeLaunchDescription( 71 | PythonLaunchDescriptionSource( 72 | [ 73 | get_package_share_directory('crane_plus_moveit_config'), 74 | '/launch/run_move_group.launch.py', 75 | ] 76 | ), 77 | launch_arguments={ 78 | 'loaded_description': description, 79 | 'rviz_config': LaunchConfiguration('rviz_config'), 80 | }.items(), 81 | ) 82 | 83 | control_node = IncludeLaunchDescription( 84 | PythonLaunchDescriptionSource( 85 | [ 86 | get_package_share_directory('crane_plus_control'), 87 | '/launch/crane_plus_control.launch.py', 88 | ] 89 | ), 90 | launch_arguments={'loaded_description': description}.items(), 91 | ) 92 | 93 | camera_info_file = ( 94 | 'file://' + get_package_share_directory('crane_plus_examples') + '/config/camera_info.yaml' 95 | ) 96 | usb_cam_node = Node( 97 | package='usb_cam', 98 | executable='usb_cam_node_exe', 99 | parameters=[ 100 | {'video_device': LaunchConfiguration('video_device')}, 101 | {'frame_id': 'camera_color_optical_frame'}, 102 | {'camera_info_url': camera_info_file}, 103 | {'pixel_format': 'yuyv2rgb'}, 104 | ], 105 | condition=IfCondition(LaunchConfiguration('use_camera')), 106 | ) 107 | 108 | return LaunchDescription( 109 | [ 110 | declare_port_name, 111 | declare_use_camera, 112 | declare_video_device, 113 | declare_use_mock_components, 114 | declare_rviz_config, 115 | declare_rviz_config_camera, 116 | move_group, 117 | control_node, 118 | usb_cam_node, 119 | ] 120 | ) 121 | -------------------------------------------------------------------------------- /crane_plus_examples/launch/example.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 RT Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from crane_plus_description.robot_description_loader import RobotDescriptionLoader 16 | from launch import LaunchDescription 17 | from launch.actions import DeclareLaunchArgument 18 | from launch.substitutions import LaunchConfiguration 19 | from launch_ros.actions import Node 20 | from launch_ros.actions import SetParameter 21 | from moveit_configs_utils import MoveItConfigsBuilder 22 | 23 | 24 | def generate_launch_description(): 25 | declare_use_camera = DeclareLaunchArgument( 26 | 'use_camera', default_value='false', description='Use camera.' 27 | ) 28 | 29 | declare_example_name = DeclareLaunchArgument( 30 | 'example', 31 | default_value='gripper_control', 32 | description=( 33 | 'Set an example executable name: ' 34 | '[gripper_control, pose_groupstate, joint_values, pick_and_place]' 35 | ), 36 | ) 37 | 38 | declare_use_sim_time = DeclareLaunchArgument( 39 | 'use_sim_time', 40 | default_value='false', 41 | description=('Set true when using the gazebo simulator.'), 42 | ) 43 | 44 | description_loader = RobotDescriptionLoader() 45 | description_loader.use_camera = LaunchConfiguration('use_camera') 46 | 47 | moveit_config = MoveItConfigsBuilder('crane_plus').to_moveit_configs() 48 | moveit_config.robot_description = { 49 | 'robot_description': description_loader.load(), 50 | } 51 | 52 | example_node = Node( 53 | name=[LaunchConfiguration('example'), '_node'], 54 | package='crane_plus_examples', 55 | executable=LaunchConfiguration('example'), 56 | output='screen', 57 | parameters=[moveit_config.to_dict()], 58 | ) 59 | 60 | return LaunchDescription( 61 | [ 62 | declare_use_camera, 63 | declare_use_sim_time, 64 | SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), 65 | declare_example_name, 66 | example_node, 67 | ] 68 | ) 69 | -------------------------------------------------------------------------------- /crane_plus_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crane_plus_examples 5 | 3.0.0 6 | CRANE+ V2 examples package 7 | RT Corporation 8 | Apache License 2.0 9 | 10 | Shota Aoki 11 | Atsushi Kuwagata 12 | Yusuke Kato 13 | 14 | ament_cmake 15 | 16 | crane_plus_control 17 | crane_plus_description 18 | crane_plus_moveit_config 19 | cv_bridge 20 | geometry_msgs 21 | libopencv-dev 22 | moveit_ros_planning_interface 23 | rclcpp 24 | tf2_geometry_msgs 25 | usb_cam 26 | image_geometry 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /crane_plus_examples/src/aruco_detection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://docs.opencv.org/4.2.0/d5/dae/tutorial_aruco_detection.html 17 | // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "geometry_msgs/msg/transform_stamped.hpp" 26 | #include "sensor_msgs/msg/camera_info.hpp" 27 | #include "sensor_msgs/msg/image.hpp" 28 | #include "opencv2/opencv.hpp" 29 | #include "opencv2/aruco.hpp" 30 | #include "cv_bridge/cv_bridge.hpp" 31 | #include "tf2/LinearMath/Quaternion.h" 32 | #include "tf2/LinearMath/Matrix3x3.h" 33 | #include "tf2_ros/transform_broadcaster.h" 34 | using std::placeholders::_1; 35 | 36 | class ImageSubscriber : public rclcpp::Node 37 | { 38 | public: 39 | ImageSubscriber() 40 | : Node("aruco_detection") 41 | { 42 | image_subscription_ = this->create_subscription( 43 | "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); 44 | 45 | camera_info_subscription_ = this->create_subscription( 46 | "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); 47 | 48 | tf_broadcaster_ = 49 | std::make_unique(*this); 50 | } 51 | 52 | private: 53 | rclcpp::Subscription::SharedPtr image_subscription_; 54 | rclcpp::Subscription::SharedPtr camera_info_subscription_; 55 | sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; 56 | std::unique_ptr tf_broadcaster_; 57 | 58 | void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) 59 | { 60 | auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); 61 | cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR); 62 | 63 | if (camera_info_) { 64 | // ArUcoマーカのデータセットを読み込む 65 | // DICT_6x6_50は6x6ビットのマーカが50個収録されたもの 66 | const auto MARKER_DICT = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50); 67 | // マーカID 68 | std::vector ids; 69 | // 画像座標系上のマーカ頂点位置 70 | std::vector> corners; 71 | // マーカの検出 72 | cv::aruco::detectMarkers(cv_img->image, MARKER_DICT, corners, ids); 73 | // マーカの検出数 74 | int n_markers = ids.size(); 75 | // カメラパラメータ 76 | const auto CAMERA_MATRIX = cv::Mat(3, 3, CV_64F, camera_info_->k.data()); 77 | const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, camera_info_->d.data()); 78 | // マーカ一辺の長さ 0.04 [m] 79 | const float MARKER_LENGTH = 0.04; 80 | 81 | // マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 82 | if (n_markers > 0) { 83 | for (int i = 0; i < n_markers; i++) { 84 | // マーカの回転ベクトルと位置ベクトル 85 | std::vector rvecs, tvecs; 86 | // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 87 | cv::aruco::estimatePoseSingleMarkers( 88 | corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs); 89 | 90 | // tfの配信 91 | geometry_msgs::msg::TransformStamped t; 92 | t.header = msg->header; 93 | t.child_frame_id = "target_" + std::to_string(ids[i]); 94 | t.transform.translation.x = tvecs[i][0]; 95 | t.transform.translation.y = tvecs[i][1]; 96 | t.transform.translation.z = tvecs[i][2]; 97 | tf2::Quaternion q; 98 | cv::Mat cv_rotation_matrix; 99 | cv::Rodrigues(rvecs[i], cv_rotation_matrix); 100 | tf2::Matrix3x3 tf2_rotation_matrix = tf2::Matrix3x3( 101 | cv_rotation_matrix.at(0, 0), 102 | cv_rotation_matrix.at(0, 1), 103 | cv_rotation_matrix.at(0, 2), 104 | cv_rotation_matrix.at(1, 0), 105 | cv_rotation_matrix.at(1, 1), 106 | cv_rotation_matrix.at(1, 2), 107 | cv_rotation_matrix.at(2, 0), 108 | cv_rotation_matrix.at(2, 1), 109 | cv_rotation_matrix.at(2, 2)); 110 | tf2_rotation_matrix.getRotation(q); 111 | t.transform.rotation.x = q.x(); 112 | t.transform.rotation.y = q.y(); 113 | t.transform.rotation.z = q.z(); 114 | t.transform.rotation.w = q.w(); 115 | tf_broadcaster_->sendTransform(t); 116 | } 117 | } 118 | } 119 | } 120 | 121 | void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) 122 | { 123 | camera_info_ = msg; 124 | } 125 | }; 126 | 127 | int main(int argc, char * argv[]) 128 | { 129 | rclcpp::init(argc, argv); 130 | rclcpp::spin(std::make_shared()); 131 | rclcpp::shutdown(); 132 | return 0; 133 | } 134 | -------------------------------------------------------------------------------- /crane_plus_examples/src/color_detection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2023 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "geometry_msgs/msg/transform_stamped.hpp" 25 | #include "sensor_msgs/msg/camera_info.hpp" 26 | #include "sensor_msgs/msg/image.hpp" 27 | #include "tf2/LinearMath/Quaternion.h" 28 | #include "tf2/LinearMath/Matrix3x3.h" 29 | #include "tf2_ros/transform_broadcaster.h" 30 | #include "opencv2/opencv.hpp" 31 | #include "opencv2/imgproc/imgproc.hpp" 32 | #include "cv_bridge/cv_bridge.hpp" 33 | #include "image_geometry/pinhole_camera_model.hpp" 34 | using std::placeholders::_1; 35 | 36 | class ImageSubscriber : public rclcpp::Node 37 | { 38 | public: 39 | ImageSubscriber() 40 | : Node("color_detection") 41 | { 42 | image_subscription_ = this->create_subscription( 43 | "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); 44 | 45 | camera_info_subscription_ = this->create_subscription( 46 | "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); 47 | 48 | image_thresholded_publisher_ = 49 | this->create_publisher("image_thresholded", 10); 50 | 51 | tf_broadcaster_ = 52 | std::make_unique(*this); 53 | } 54 | 55 | private: 56 | rclcpp::Subscription::SharedPtr image_subscription_; 57 | rclcpp::Subscription::SharedPtr camera_info_subscription_; 58 | rclcpp::Publisher::SharedPtr image_thresholded_publisher_; 59 | sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; 60 | std::unique_ptr tf_broadcaster_; 61 | 62 | void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) 63 | { 64 | // カメラのパラメータを取得してから処理を行う 65 | if (camera_info_) { 66 | // 赤い物体を検出するようにHSVの範囲を設定 67 | // 周囲の明るさ等の動作環境に合わせて調整 68 | const int LOW_H_1 = 0, HIGH_H_1 = 10; 69 | const int LOW_H_2 = 170, HIGH_H_2 = 179; 70 | const int LOW_S = 100, HIGH_S = 255; 71 | const int LOW_V = 100, HIGH_V = 255; 72 | 73 | // ウェブカメラの画像を受け取る 74 | auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); 75 | 76 | // 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） 77 | cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); 78 | 79 | // 画像処理用の変数を用意 80 | cv::Mat img_mask_1; 81 | cv::Mat img_mask_2; 82 | cv::Mat img_thresholded; 83 | 84 | // 画像の二値化 85 | cv::inRange( 86 | cv_img->image, 87 | cv::Scalar(LOW_H_1, LOW_S, LOW_V), 88 | cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), 89 | img_mask_1); 90 | cv::inRange( 91 | cv_img->image, 92 | cv::Scalar(LOW_H_2, LOW_S, LOW_V), 93 | cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), 94 | img_mask_2); 95 | 96 | // マスク画像の合成 97 | img_thresholded = img_mask_1 | img_mask_2; 98 | 99 | // ノイズ除去の処理 100 | cv::morphologyEx( 101 | img_thresholded, 102 | img_thresholded, 103 | cv::MORPH_OPEN, 104 | cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); 105 | 106 | // 穴埋めの処理 107 | cv::morphologyEx( 108 | img_thresholded, 109 | img_thresholded, 110 | cv::MORPH_CLOSE, 111 | cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); 112 | 113 | // 画像の検出領域におけるモーメントを計算 114 | cv::Moments moment = moments(img_thresholded); 115 | double d_m01 = moment.m01; 116 | double d_m10 = moment.m10; 117 | double d_area = moment.m00; 118 | 119 | // 検出した領域のピクセル数が10000より大きい場合 120 | if (d_area > 10000) { 121 | // カメラモデル作成 122 | image_geometry::PinholeCameraModel camera_model; 123 | 124 | // カメラのパラメータを設定 125 | camera_model.fromCameraInfo(camera_info_); 126 | 127 | // 画像座標系における把持対象物の位置（2D） 128 | const double pixel_x = d_m10 / d_area; 129 | const double pixel_y = d_m01 / d_area; 130 | const cv::Point2d point(pixel_x, pixel_y); 131 | 132 | // 補正後の画像座標系における把持対象物の位置を取得（2D） 133 | const cv::Point2d rect_point = camera_model.rectifyPoint(point); 134 | 135 | // カメラ座標系から見た把持対象物の方向（Ray）を取得する 136 | const cv::Point3d ray = camera_model.projectPixelTo3dRay(rect_point); 137 | 138 | // カメラの高さを0.44[m]として把持対象物の位置を計算 139 | const double CAMERA_HEIGHT = 0.46; 140 | cv::Point3d object_position( 141 | ray.x * CAMERA_HEIGHT, 142 | ray.y * CAMERA_HEIGHT, 143 | ray.z * CAMERA_HEIGHT); 144 | 145 | // 把持対象物の位置をTFに配信 146 | geometry_msgs::msg::TransformStamped t; 147 | t.header = msg->header; 148 | t.child_frame_id = "target_0"; 149 | t.transform.translation.x = object_position.x; 150 | t.transform.translation.y = object_position.y; 151 | t.transform.translation.z = object_position.z; 152 | tf_broadcaster_->sendTransform(t); 153 | } 154 | 155 | // 閾値による二値化画像を配信 156 | sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = 157 | cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); 158 | image_thresholded_publisher_->publish(*img_thresholded_msg); 159 | } 160 | } 161 | 162 | void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) 163 | { 164 | camera_info_ = msg; 165 | } 166 | }; 167 | 168 | int main(int argc, char * argv[]) 169 | { 170 | rclcpp::init(argc, argv); 171 | rclcpp::spin(std::make_shared()); 172 | rclcpp::shutdown(); 173 | return 0; 174 | } 175 | -------------------------------------------------------------------------------- /crane_plus_examples/src/gripper_control.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes 17 | // /run_move_group/src/run_move_group.cpp 18 | 19 | #include 20 | 21 | #include "moveit/move_group_interface/move_group_interface.h" 22 | #include "rclcpp/rclcpp.hpp" 23 | 24 | using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; 25 | 26 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control"); 27 | 28 | double to_radians(const double deg_angle) 29 | { 30 | return deg_angle * M_PI / 180.0; 31 | } 32 | 33 | int main(int argc, char ** argv) 34 | { 35 | rclcpp::init(argc, argv); 36 | rclcpp::NodeOptions node_options; 37 | node_options.automatically_declare_parameters_from_overrides(true); 38 | auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); 39 | // For current state monitor 40 | rclcpp::executors::SingleThreadedExecutor executor; 41 | executor.add_node(move_group_gripper_node); 42 | std::thread([&executor]() {executor.spin();}).detach(); 43 | 44 | MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); 45 | move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 46 | move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 47 | 48 | auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); 49 | 50 | gripper_joint_values[0] = to_radians(30); 51 | move_group_gripper.setJointValueTarget(gripper_joint_values); 52 | move_group_gripper.move(); 53 | 54 | gripper_joint_values[0] = to_radians(-30); 55 | move_group_gripper.setJointValueTarget(gripper_joint_values); 56 | move_group_gripper.move(); 57 | 58 | gripper_joint_values[0] = to_radians(0); 59 | move_group_gripper.setJointValueTarget(gripper_joint_values); 60 | move_group_gripper.move(); 61 | 62 | rclcpp::shutdown(); 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /crane_plus_examples/src/joint_values.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes 17 | // /run_move_group/src/run_move_group.cpp 18 | 19 | #include 20 | 21 | #include "moveit/move_group_interface/move_group_interface.h" 22 | #include "rclcpp/rclcpp.hpp" 23 | 24 | using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; 25 | 26 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("joint_values"); 27 | 28 | double to_radians(const double deg_angle) 29 | { 30 | return deg_angle * M_PI / 180.0; 31 | } 32 | 33 | int main(int argc, char ** argv) 34 | { 35 | rclcpp::init(argc, argv); 36 | rclcpp::NodeOptions node_options; 37 | node_options.automatically_declare_parameters_from_overrides(true); 38 | auto move_group_node = rclcpp::Node::make_shared("joint_values", node_options); 39 | // For current state monitor 40 | rclcpp::executors::SingleThreadedExecutor executor; 41 | executor.add_node(move_group_node); 42 | std::thread([&executor]() {executor.spin();}).detach(); 43 | 44 | MoveGroupInterface move_group_arm(move_group_node, "arm"); 45 | move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 46 | move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 47 | 48 | move_group_arm.setNamedTarget("vertical"); 49 | move_group_arm.move(); 50 | 51 | auto joint_values = move_group_arm.getCurrentJointValues(); 52 | double target_joint_value = to_radians(45.0); 53 | for (size_t i = 0; i < joint_values.size(); i++) { 54 | joint_values[i] = target_joint_value; 55 | move_group_arm.setJointValueTarget(joint_values); 56 | move_group_arm.move(); 57 | } 58 | 59 | move_group_arm.setNamedTarget("vertical"); 60 | move_group_arm.move(); 61 | 62 | rclcpp::shutdown(); 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /crane_plus_examples/src/pick_and_place.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes 17 | // /run_move_group/src/run_move_group.cpp 18 | 19 | #include 20 | 21 | #include "geometry_msgs/msg/pose.hpp" 22 | #include "geometry_msgs/msg/quaternion.hpp" 23 | #include "moveit/move_group_interface/move_group_interface.h" 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 26 | 27 | using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; 28 | 29 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place"); 30 | 31 | double to_radians(const double deg_angle) 32 | { 33 | return deg_angle * M_PI / 180.0; 34 | } 35 | 36 | int main(int argc, char ** argv) 37 | { 38 | rclcpp::init(argc, argv); 39 | rclcpp::NodeOptions node_options; 40 | node_options.automatically_declare_parameters_from_overrides(true); 41 | auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); 42 | auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); 43 | // For current state monitor 44 | rclcpp::executors::SingleThreadedExecutor executor; 45 | executor.add_node(move_group_arm_node); 46 | executor.add_node(move_group_gripper_node); 47 | std::thread([&executor]() {executor.spin();}).detach(); 48 | 49 | MoveGroupInterface move_group_arm(move_group_arm_node, "arm"); 50 | move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 51 | move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 52 | 53 | MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); 54 | move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 55 | move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 56 | auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); 57 | double GRIPPER_DEFAULT = 0.0; 58 | double GRIPPER_OPEN = to_radians(-30); 59 | double GRIPPER_CLOSE = to_radians(10); 60 | 61 | // Set goal tolerances to improve IK success rate 62 | move_group_arm.setGoalPositionTolerance(1e-5); 63 | move_group_arm.setGoalOrientationTolerance(1e-4); 64 | 65 | move_group_arm.setNamedTarget("vertical"); 66 | move_group_arm.move(); 67 | 68 | gripper_joint_values[0] = GRIPPER_DEFAULT; 69 | move_group_gripper.setJointValueTarget(gripper_joint_values); 70 | move_group_gripper.move(); 71 | 72 | // ----- Picking Preparation ----- 73 | move_group_arm.setNamedTarget("home"); 74 | move_group_arm.move(); 75 | 76 | gripper_joint_values[0] = GRIPPER_OPEN; 77 | move_group_gripper.setJointValueTarget(gripper_joint_values); 78 | move_group_gripper.move(); 79 | 80 | geometry_msgs::msg::Pose target_pose; 81 | tf2::Quaternion q; 82 | target_pose.position.x = 0.0; 83 | target_pose.position.y = -0.09; 84 | target_pose.position.z = 0.17; 85 | q.setRPY(to_radians(0), to_radians(90), to_radians(-90)); 86 | target_pose.orientation = tf2::toMsg(q); 87 | move_group_arm.setPoseTarget(target_pose); 88 | move_group_arm.move(); 89 | 90 | q.setRPY(to_radians(0), to_radians(180), to_radians(-90)); 91 | target_pose.orientation = tf2::toMsg(q); 92 | move_group_arm.setPoseTarget(target_pose); 93 | move_group_arm.move(); 94 | 95 | target_pose.position.z = 0.14; 96 | move_group_arm.setPoseTarget(target_pose); 97 | move_group_arm.move(); 98 | 99 | // Grasp 100 | gripper_joint_values[0] = GRIPPER_CLOSE; 101 | move_group_gripper.setJointValueTarget(gripper_joint_values); 102 | move_group_gripper.move(); 103 | 104 | target_pose.position.z = 0.17; 105 | move_group_arm.setPoseTarget(target_pose); 106 | move_group_arm.move(); 107 | 108 | // ----- Placing Preparation ----- 109 | move_group_arm.setNamedTarget("home"); 110 | move_group_arm.move(); 111 | 112 | target_pose.position.x = 0.15; 113 | target_pose.position.y = 0.0; 114 | target_pose.position.z = 0.06; 115 | q.setRPY(to_radians(0), to_radians(90), to_radians(0)); 116 | target_pose.orientation = tf2::toMsg(q); 117 | move_group_arm.setPoseTarget(target_pose); 118 | move_group_arm.move(); 119 | 120 | // Release 121 | gripper_joint_values[0] = GRIPPER_OPEN; 122 | move_group_gripper.setJointValueTarget(gripper_joint_values); 123 | move_group_gripper.move(); 124 | 125 | // Return to home and vertical pose 126 | move_group_arm.setNamedTarget("home"); 127 | move_group_arm.move(); 128 | move_group_arm.setNamedTarget("vertical"); 129 | move_group_arm.move(); 130 | 131 | gripper_joint_values[0] = GRIPPER_DEFAULT; 132 | move_group_gripper.setJointValueTarget(gripper_joint_values); 133 | move_group_gripper.move(); 134 | 135 | rclcpp::shutdown(); 136 | return 0; 137 | } 138 | -------------------------------------------------------------------------------- /crane_plus_examples/src/pick_and_place_tf.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://github.com/ros-planning/moveit2_tutorials/blob 17 | // /a547cf49ff7d1fe16a93dfe020c6027bcb035b51/doc/move_group_interface 18 | // /src/move_group_interface_tutorial.cpp 19 | // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "angles/angles.h" 26 | #include "geometry_msgs/msg/pose.hpp" 27 | #include "geometry_msgs/msg/quaternion.hpp" 28 | #include "geometry_msgs/msg/transform_stamped.hpp" 29 | #include "geometry_msgs/msg/twist.hpp" 30 | #include "moveit/move_group_interface/move_group_interface.h" 31 | #include "rclcpp/rclcpp.hpp" 32 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 33 | #include "tf2/convert.h" 34 | #include "tf2/exceptions.h" 35 | #include "tf2_ros/transform_listener.h" 36 | #include "tf2_ros/buffer.h" 37 | #include "std_msgs/msg/string.hpp" 38 | using namespace std::chrono_literals; 39 | using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; 40 | 41 | class PickAndPlaceTf : public rclcpp::Node 42 | { 43 | public: 44 | PickAndPlaceTf( 45 | rclcpp::Node::SharedPtr move_group_arm_node, 46 | rclcpp::Node::SharedPtr move_group_gripper_node) 47 | : Node("pick_and_place_tf_node") 48 | { 49 | using namespace std::placeholders; 50 | move_group_arm_ = std::make_shared(move_group_arm_node, "arm"); 51 | move_group_arm_->setMaxVelocityScalingFactor(1.0); 52 | move_group_arm_->setMaxAccelerationScalingFactor(1.0); 53 | 54 | move_group_gripper_ = std::make_shared(move_group_gripper_node, "gripper"); 55 | move_group_gripper_->setMaxVelocityScalingFactor(1.0); 56 | move_group_gripper_->setMaxAccelerationScalingFactor(1.0); 57 | 58 | // IKの成功率を向上させるため、目標位置姿勢の許容範囲を小さく設定 59 | move_group_arm_->setGoalPositionTolerance(1e-5); 60 | move_group_arm_->setGoalOrientationTolerance(1e-4); 61 | 62 | // SRDFに定義されている"home"の姿勢にする 63 | move_group_arm_->setNamedTarget("home"); 64 | move_group_arm_->move(); 65 | 66 | // 可動範囲を制限する 67 | moveit_msgs::msg::Constraints constraints; 68 | constraints.name = "arm_constraints"; 69 | 70 | moveit_msgs::msg::JointConstraint joint_constraint; 71 | joint_constraint.joint_name = "crane_plus_joint1"; 72 | joint_constraint.position = 0.0; 73 | joint_constraint.tolerance_above = angles::from_degrees(100); 74 | joint_constraint.tolerance_below = angles::from_degrees(100); 75 | joint_constraint.weight = 1.0; 76 | constraints.joint_constraints.push_back(joint_constraint); 77 | 78 | joint_constraint.joint_name = "crane_plus_joint3"; 79 | joint_constraint.position = 0.0; 80 | joint_constraint.tolerance_above = angles::from_degrees(0); 81 | joint_constraint.tolerance_below = angles::from_degrees(180); 82 | joint_constraint.weight = 1.0; 83 | constraints.joint_constraints.push_back(joint_constraint); 84 | 85 | move_group_arm_->setPathConstraints(constraints); 86 | 87 | // 待機姿勢 88 | control_arm(0.0, 0.0, 0.17, 0, 0, 0); 89 | 90 | tf_buffer_ = 91 | std::make_unique(this->get_clock()); 92 | tf_listener_ = 93 | std::make_shared(*tf_buffer_); 94 | 95 | timer_ = this->create_wall_timer( 96 | 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); 97 | } 98 | 99 | private: 100 | void on_timer() 101 | { 102 | // target_0のtf位置姿勢を取得 103 | geometry_msgs::msg::TransformStamped tf_msg; 104 | 105 | try { 106 | tf_msg = tf_buffer_->lookupTransform( 107 | "base_link", "target_0", 108 | tf2::TimePointZero); 109 | } catch (const tf2::TransformException & ex) { 110 | RCLCPP_INFO( 111 | this->get_logger(), "Could not transform base_link to target: %s", 112 | ex.what()); 113 | return; 114 | } 115 | 116 | rclcpp::Time now = this->get_clock()->now(); 117 | const std::chrono::nanoseconds FILTERING_TIME = 2s; 118 | const std::chrono::nanoseconds STOP_TIME_THRESHOLD = 3s; 119 | const float DISTANCE_THRESHOLD = 0.01; 120 | tf2::Stamped tf; 121 | tf2::convert(tf_msg, tf); 122 | const auto TF_ELAPSED_TIME = now.nanoseconds() - tf.stamp_.time_since_epoch().count(); 123 | const auto TF_STOP_TIME = now.nanoseconds() - tf_past_.stamp_.time_since_epoch().count(); 124 | 125 | // 現在時刻から2秒以内に受け取ったtfを使用 126 | if (TF_ELAPSED_TIME < FILTERING_TIME.count()) { 127 | double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length(); 128 | // 把持対象の位置が停止していることを判定 129 | if (tf_diff < DISTANCE_THRESHOLD) { 130 | // 把持対象が3秒以上停止している場合ピッキング動作開始 131 | if (TF_STOP_TIME > STOP_TIME_THRESHOLD.count()) { 132 | picking(tf.getOrigin()); 133 | } 134 | } else { 135 | tf_past_ = tf; 136 | } 137 | } 138 | } 139 | 140 | void picking(tf2::Vector3 target_position) 141 | { 142 | const double GRIPPER_DEFAULT = 0.0; 143 | const double GRIPPER_OPEN = angles::from_degrees(-30.0); 144 | const double GRIPPER_CLOSE = angles::from_degrees(10.0); 145 | 146 | // 何かを掴んでいた時のためにハンドを開く 147 | control_gripper(GRIPPER_OPEN); 148 | 149 | // ロボット座標系（2D）の原点から見た把持対象物への角度を計算 150 | double x = target_position.x(); 151 | double y = target_position.y(); 152 | double theta_rad = std::atan2(y, x); 153 | double theta_deg = theta_rad * 180.0 / 3.1415926535; 154 | 155 | // 把持対象物に正対する 156 | control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg); 157 | 158 | // 掴みに行く 159 | const double GRIPPER_OFFSET = 0.13; 160 | double gripper_offset_x = GRIPPER_OFFSET * std::cos(theta_rad); 161 | double gripper_offset_y = GRIPPER_OFFSET * std::sin(theta_rad); 162 | if (!control_arm(x - gripper_offset_x, y - gripper_offset_y, 0.04, 0, 90, theta_deg)) { 163 | // アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る 164 | control_arm(0.0, 0.0, 0.17, 0, 0, 0); 165 | return; 166 | } 167 | 168 | // ハンドを閉じる 169 | control_gripper(GRIPPER_CLOSE); 170 | 171 | // 移動する 172 | control_arm(0.0, 0.0, 0.17, 0, 90, 0); 173 | 174 | // 下ろす 175 | control_arm(0.0, -0.15, 0.05, 0, 90, -90); 176 | 177 | // ハンドを開く 178 | control_gripper(GRIPPER_OPEN); 179 | 180 | // 少しだけハンドを持ち上げる 181 | control_arm(0.0, -0.15, 0.10, 0, 90, -90); 182 | 183 | // 待機姿勢に戻る 184 | control_arm(0.0, 0.0, 0.17, 0, 0, 0); 185 | 186 | // ハンドを閉じる 187 | control_gripper(GRIPPER_DEFAULT); 188 | } 189 | 190 | // グリッパ制御 191 | void control_gripper(const double angle) 192 | { 193 | auto joint_values = move_group_gripper_->getCurrentJointValues(); 194 | joint_values[0] = angle; 195 | move_group_gripper_->setJointValueTarget(joint_values); 196 | move_group_gripper_->move(); 197 | } 198 | 199 | // アーム制御 200 | bool control_arm( 201 | const double x, const double y, const double z, 202 | const double roll, const double pitch, const double yaw) 203 | { 204 | geometry_msgs::msg::Pose target_pose; 205 | tf2::Quaternion q; 206 | target_pose.position.x = x; 207 | target_pose.position.y = y; 208 | target_pose.position.z = z; 209 | q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw)); 210 | target_pose.orientation = tf2::toMsg(q); 211 | move_group_arm_->setPoseTarget(target_pose); 212 | // アーム動作の成否を取得 213 | moveit::core::MoveItErrorCode result = move_group_arm_->move(); 214 | return result.val == moveit::core::MoveItErrorCode::SUCCESS; 215 | } 216 | 217 | std::shared_ptr move_group_arm_; 218 | std::shared_ptr move_group_gripper_; 219 | std::unique_ptr tf_buffer_; 220 | std::shared_ptr tf_listener_{nullptr}; 221 | rclcpp::TimerBase::SharedPtr timer_{nullptr}; 222 | tf2::Stamped tf_past_; 223 | }; 224 | 225 | int main(int argc, char ** argv) 226 | { 227 | rclcpp::init(argc, argv); 228 | rclcpp::NodeOptions node_options; 229 | node_options.automatically_declare_parameters_from_overrides(true); 230 | auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); 231 | auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); 232 | 233 | rclcpp::executors::MultiThreadedExecutor exec; 234 | auto pick_and_place_tf_node = std::make_shared( 235 | move_group_arm_node, 236 | move_group_gripper_node); 237 | exec.add_node(pick_and_place_tf_node); 238 | exec.add_node(move_group_arm_node); 239 | exec.add_node(move_group_gripper_node); 240 | exec.spin(); 241 | rclcpp::shutdown(); 242 | return 0; 243 | } 244 | -------------------------------------------------------------------------------- /crane_plus_examples/src/pose_groupstate.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 RT Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Reference: 16 | // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes 17 | // /run_move_group/src/run_move_group.cpp 18 | 19 | #include "moveit/move_group_interface/move_group_interface.h" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; 23 | 24 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("pose_groupstate"); 25 | 26 | int main(int argc, char ** argv) 27 | { 28 | rclcpp::init(argc, argv); 29 | rclcpp::NodeOptions node_options; 30 | node_options.automatically_declare_parameters_from_overrides(true); 31 | auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); 32 | // For current state monitor 33 | rclcpp::executors::SingleThreadedExecutor executor; 34 | executor.add_node(move_group_arm_node); 35 | std::thread([&executor]() {executor.spin();}).detach(); 36 | 37 | MoveGroupInterface move_group_arm(move_group_arm_node, "arm"); 38 | move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 39 | move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 40 | 41 | move_group_arm.setNamedTarget("home"); 42 | move_group_arm.move(); 43 | 44 | move_group_arm.setNamedTarget("vertical"); 45 | move_group_arm.move(); 46 | 47 | move_group_arm.setNamedTarget("home"); 48 | move_group_arm.move(); 49 | 50 | rclcpp::shutdown(); 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /crane_plus_gazebo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package crane_plus_gazebo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-01-31) 6 | ------------------ 7 | * Jazzy対応 (`#78 `_) 8 | * Gazebo環境にカメラを追加 (`#83 `_) 9 | * テスト実行時はダミーのパラメータファイルを読み込めるようにdescription_loaderを修正 (`#73 `_) 10 | * Contributors: Kuwamai, YusukeKato, mizonon 11 | 12 | 2.0.1 (2023-09-08) 13 | ------------------ 14 | * テスト実行時はダミーのパラメータファイルを読み込めるようにdescription_loaderを修正 (`#73 `_) 15 | * Contributors: YusukeKato 16 | 17 | 2.0.0 (2023-08-01) 18 | ------------------ 19 | * Gazeboカメラ位置の調整 (`#67 `_) 20 | * Feature/support humble (`#58 `_) 21 | * Contributors: Shota Aoki, YusukeKato 22 | 23 | 1.1.0 (2022-08-16) 24 | ------------------ 25 | * Update author tags (`#49 `_) 26 | * gui.config追加 (`#45 `_) 27 | * Rename CRANE+V2 to CRANE+ V2 (`#44 `_) 28 | * fix typo 29 | * Contributors: Atsushi Kuwagata, Shota Aoki 30 | 31 | 1.0.0 (2022-06-22) 32 | ------------------ 33 | * パッケージバージョン表記の更新 (`#40 `_) 34 | * crane_plus_ignitionのpackage.xmlを修正 (`#39 `_) 35 | * crane_plus_ignition パッケージを追加し、Gazeboの使用を非推奨にする (`#38 `_) 36 | * Contributors: Atsushi Kuwagata, Shota Aoki 37 | -------------------------------------------------------------------------------- /crane_plus_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crane_plus_gazebo) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY 7 | launch 8 | worlds 9 | gui 10 | models 11 | DESTINATION share/${PROJECT_NAME}/ 12 | ) 13 | 14 | if(BUILD_TESTING) 15 | find_package(ament_lint_auto REQUIRED) 16 | ament_lint_auto_find_test_dependencies() 17 | endif() 18 | 19 | ament_package() 20 | -------------------------------------------------------------------------------- /crane_plus_gazebo/README.md: -------------------------------------------------------------------------------- 1 | # crane_plus_gazebo 2 | 3 | CRANE+ V2 の[Gazebo](https://gazebosim.org/home) 4 | シミュレーションパッケージです。 5 | 6 | ## ノードの起動 7 | 8 | 次のコマンドを実行するとGazeboが起動し、CRANE+ V2モデルとTable、Boxが表示されます。 9 | 10 | 初回起動時はTableとBoxのモデルをダウンロードするため、モデルの表示に時間がかかることがあります。 11 | 12 | 実機との接続や`crane_plus_examples/launch/demo.launch.py`の実行は必要ありません。 13 | 14 | ```sh 15 | $ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py 16 | ``` 17 | 18 | カメラ付きモデルを使用する場合は下記コマンドを実行します。 19 | 20 | ```sh 21 | $ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py use_camera:=true 22 | ``` 23 | 24 | CRANE+ V2の前にArUcoマーカ付きのBoxを置いたシミュレータ環境を使用する場合は下記コマンドを実行します。 25 | 26 | ```sh 27 | $ ros2 launch crane_plus_gazebo crane_plus_with_aruco_cube.launch.py 28 | ``` 29 | 30 | CRANE+ V2の前に赤いBoxを置いたシミュレータ環境を使用する場合は下記コマンドを実行します。 31 | 32 | ```sh 33 | $ ros2 launch crane_plus_gazebo crane_plus_with_red_cube.launch.py 34 | ``` 35 | 36 | ![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png) 37 | -------------------------------------------------------------------------------- /crane_plus_gazebo/gui/gui.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1000 6 | 845 7 |