├── .gitignore ├── CHANGELOG.rst ├── LICENSE ├── README.md ├── docker ├── Dockerfile ├── README.md ├── script │ ├── 00_ros2_install.sh │ ├── 10_eigen_install.sh │ ├── 11_libpcl_install.sh │ ├── 12_opencv_install.sh │ ├── 13_openvino_install.sh │ ├── 20_librealsense_install.sh │ ├── 30_gpg_install.sh │ ├── 31_gpd_install.sh │ ├── 32_ur_modern_driver_install.sh │ ├── 50_ros2_deps.sh │ ├── install_ros2_grasp_library.sh │ ├── ros_entrypoint.sh │ └── ros_env.sh └── setup_docker_display.sh ├── grasp_apps ├── draw_x │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ ├── draw_x.launch.py │ │ └── draw_x.yaml │ ├── package.xml │ └── src │ │ └── draw_x.cpp ├── fixed_position_pick │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ ├── fixed_position_pick.launch.py │ │ └── fixed_position_pick.yaml │ ├── package.xml │ └── src │ │ └── fixed_position_pick.cpp ├── random_pick │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ └── random_pick.yaml │ ├── package.xml │ ├── rviz2 │ │ └── random_pick.rviz │ └── src │ │ └── random_pick.cpp └── recognize_pick │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ └── recognize_pick.yaml │ ├── package.xml │ ├── rviz2 │ └── recognize_pick.rviz │ └── src │ ├── place_publisher.cpp │ └── recognize_pick.cpp ├── grasp_msgs ├── CMakeLists.txt ├── msg │ ├── CloudIndexed.msg │ ├── CloudSamples.msg │ ├── CloudSources.msg │ ├── GraspConfig.msg │ ├── GraspConfigList.msg │ └── SamplesMsg.msg └── package.xml ├── grasp_ros2 ├── CMakeLists.txt ├── README.md ├── cfg │ ├── grasp_ros2_params.yaml │ ├── random_pick.yaml │ ├── recognize_pick.yaml │ └── test_grasp_ros2.yaml ├── include │ └── grasp_library │ │ └── ros2 │ │ ├── consts.hpp │ │ ├── grasp_detector_base.hpp │ │ ├── grasp_detector_gpd.hpp │ │ ├── grasp_planner.hpp │ │ └── ros_params.hpp ├── package.xml ├── rviz2 │ └── grasp.rviz ├── src │ ├── consts.cpp │ ├── grasp_composition.cpp │ ├── grasp_detector_gpd.cpp │ ├── grasp_planner.cpp │ └── ros_params.cpp └── tests │ ├── CMakeLists.txt │ ├── resource │ └── table_top.pcd │ ├── tgrasp_ros2.cpp │ └── tgrasp_ros2.h.in ├── grasp_tutorials ├── CMakeLists.txt ├── README.md ├── _static │ └── images │ │ ├── draw_x_v_1.png │ │ ├── ilc_platform.png │ │ ├── moveit_app_zoo_global_view.png │ │ ├── pick_place.png │ │ ├── random_pick.png │ │ ├── random_pick_rviz.png │ │ ├── recognize_pick.png │ │ ├── ros2_grasp_library.png │ │ ├── table.png │ │ ├── ur5_setup_verify_gazebo.png │ │ ├── ur5_setup_verify_rviz.png │ │ ├── workflow.png │ │ └── workflow.vsdx ├── conf.py ├── doc │ ├── bringup_robot.rst │ ├── draw_x.rst │ ├── fixed_position_pick.rst │ ├── getting_start.rst │ ├── grasp_api.rst │ ├── grasp_planner.rst │ ├── grasp_ros2 │ │ ├── img │ │ │ └── ros2_grasp_library.png │ │ ├── install_gpd.md │ │ ├── install_openvino.md │ │ ├── tutorials_1_grasp_ros2_with_camera.md │ │ ├── tutorials_2_grasp_ros2_test.md │ │ └── tutorials_3_grasp_ros2_launch_options.md │ ├── handeye_calibration.rst │ ├── overview.rst │ ├── random_pick.rst │ ├── recognize_pick.rst │ ├── robot_interface.rst │ └── template.rst ├── index.rst └── package.xml ├── grasp_utils ├── handeye_dashboard │ ├── README.md │ ├── config │ │ └── Default.perspective │ ├── data │ │ ├── camera-robot.json │ │ └── dataset.json │ ├── doc │ │ └── images │ │ │ ├── handeye_calibration_result.png │ │ │ ├── handeye_dashboard.png │ │ │ ├── handeye_target_detection.png │ │ │ └── robot_state.png │ ├── images │ │ ├── Intel.png │ │ ├── UR5.png │ │ ├── capture.png │ │ └── tool-calibration.png │ ├── launch │ │ └── handeye_dashboard.launch.py │ ├── package.xml │ ├── plugin.xml │ ├── resource │ │ └── handeye_dashboard │ ├── setup.py │ └── src │ │ └── handeye_dashboard │ │ ├── __init__.py │ │ ├── handeye_calibration.py │ │ ├── main.py │ │ └── plugin.py ├── handeye_target_detection │ ├── .clang-format │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ └── handeye.rviz │ ├── data │ │ ├── detected │ │ │ ├── aruco │ │ │ │ ├── 3X4_aruco.png │ │ │ │ └── 5X7_aruco.png │ │ │ ├── charuco │ │ │ │ └── charuco.png │ │ │ ├── chessboard │ │ │ │ └── chessboard.png │ │ │ └── circlegrid │ │ │ │ ├── 3X5_circles_grid.png │ │ │ │ └── 4X11_circles_grid.png │ │ └── pattern │ │ │ ├── aruco_3X4_DICT_4X4_50.png │ │ │ ├── aruco_5X7_DICT_6X6_250.png │ │ │ ├── asymmetric_circles_grid_3X5.png │ │ │ ├── asymmetric_circles_grid_4X11.png │ │ │ ├── charuco_5X7_DICT_6X6_250.jpg │ │ │ └── chessboard_9X6.png │ ├── include │ │ └── PoseEstimator.h │ ├── launch │ │ ├── pose_estimation.launch.py │ │ └── pose_estimation.yaml │ ├── package.xml │ └── src │ │ ├── pose_estimation_node.cpp │ │ └── pose_estimator.cpp ├── handeye_tf_service │ ├── CMakeLists.txt │ ├── README.md │ ├── package.xml │ ├── src │ │ └── handeye_tf_server.cpp │ └── srv │ │ └── HandeyeTF.srv └── robot_interface │ ├── CMakeLists.txt │ ├── Doxyfile │ ├── README.md │ ├── include │ └── robot_interface │ │ ├── control_base.hpp │ │ └── control_ur.hpp │ ├── launch │ ├── ur_test.launch.py │ └── ur_test.yaml │ ├── package.xml │ ├── src │ ├── control_base.cpp │ └── control_ur.cpp │ └── test │ ├── ur_test_move_command.cpp │ └── ur_test_state_publish.cpp └── moveit_msgs_light ├── CMakeLists.txt ├── README.md ├── msg ├── CollisionObject.msg ├── Grasp.msg ├── GripperTranslation.msg ├── MoveItErrorCodes.msg ├── ObjectType.msg └── PlaceLocation.msg ├── package.xml └── srv └── GraspPlanning.srv /.gitignore: -------------------------------------------------------------------------------- 1 | # rviz file 2 | *.rviz 3 | 4 | # document buid file 5 | build 6 | _build 7 | 8 | # vscode file 9 | .vscode 10 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | changelog for ros2_grasp_library 2 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3 | 4 | 0.5.0 (2019-11-06) 5 | ------------------ 6 | * Added examples for advanced industrial robot applications 7 | * draw X 8 | * fixed position pick and place 9 | * random picking with OpenVINO grasp planning 10 | * recognition picking with OpenVINO grasp planning and OpenVINO mask-rcnn object segmentation 11 | * Support ROS2 hand-eye calibration 12 | * Support robot interface for manipulation 13 | * Added tutorials on how to 14 | * Build and launch example applications 15 | * Operate hand-eye calibration and publish the transformation 16 | * Quickly enable robot interface on a new industrial robot 17 | 18 | 0.4.0 (2019-03-13) 19 | ------------------ 20 | * Support "service-driven" grasp detection mechanism (via configure auto_mode) to optimize CPU load for real-time processing. 21 | * Support grasp transformation from camera frame to a specified target frame expected in the visual manipulation. 22 | * Support launch option "grasp_approach" to specify expected approach direction in the target frame specified by 'grasp_frame_id'. Grasp Planner will return grasp poses with approach direction approximate to this parameter. 23 | * Support launch option "device" to configure device for grasp pose inference to execute, 0 for CPU, 1 for GPU, 2 for VPU, 3 for FPGA. In case OpenVINO plug-ins are installed (tutorial), this configure deploy the CNN based deep learning inference on to the target device. 24 | * Add tutorials for introduction to Intel DLDT toolkit and Intel OpenVINO toolkit. 25 | * Add tutorials for launch options and customization notes. 26 | 27 | 0.3.0 (2018-12-28) 28 | ------------------ 29 | * Support grasp pose detection from RGBD point cloud. 30 | * Support MoveIt! grasp planning service. 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DISCONTINUATION OF PROJECT # 2 | This project will no longer be maintained by Intel. 3 | Intel has ceased development and contributions including, but not limited to, maintenance, bug fixes, new releases, or updates, to this project. 4 | Intel no longer accepts patches to this project. 5 | If you have an ongoing need to use this project, are interested in independently developing it, or would like to maintain patches for the open source software community, please create your own fork of this project. 6 | 7 | Contact: webadmin@linux.intel.com 8 | # ROS2 Grasp Library 9 | 10 | A ROS2 intelligent visual grasp solution for advanced industrial usages, with OpenVINO™ grasp detection and MoveIt Grasp Planning. 11 | 12 | ## Overview 13 | ROS2 Grasp Library enables state-of-the-art CNN based deep learning grasp detection algorithms on ROS2 for intelligent visual grasp in industrial robot usage scenarios. This package provides ROS2 interfaces compliant with the open source [MoveIt](http://moveit.ros.org/) motion planning framework supported by most of the [robot models](https://moveit.ros.org/robots) in ROS industrial. This package delivers 14 | * A ROS2 Grasp Planner providing grasp planning service, as an extensible capability of MoveIt ([moveit_msgs::srv::GraspPlanning](http://docs.ros.org/api/moveit_msgs/html/srv/GraspPlanning.html)), translating grasp detection results into MoveIt Interfaces ([moveit_msgs::msg::Grasp](http://docs.ros.org/api/moveit_msgs/html/msg/Grasp.html)) 15 | * A ROS2 Grasp Detctor abstracting interfaces for grasp detection results 16 | * A ROS2 hand-eye calibration module generating transformation from camera frame to robot frame 17 | * ROS2 example applications demonstrating how to use this ROS2 Grasp Library in advanced industrial usages for intelligent visual grasp 18 | 19 | ## Grasp Detection Algorithms 20 | Grasp detection back-end algorithms enabled by this ROS2 Grasp Library: 21 | - [Grasp Pose Detection](https://github.com/atenpas/gpd) detects 6-DOF grasp poses for a 2-finger grasp (e.g. a parallel jaw gripper) in 3D point clouds from RGBD sensor or PCD file. The grasp detection was enabled with Intel® [DLDT](https://github.com/opencv/dldt) toolkit and Intel® [OpenVINO™](https://software.intel.com/en-us/openvino-toolkit) toolkit. 22 | 23 | ROS2 Grasp Library 24 | 25 | ## Tutorials 26 | Refer to ROS2 Grasp Library [Tutorials](http://intel.github.io/ros2_grasp_library) for how to 27 | * Install, build, and launch the ROS2 Grasp Planner and Detector 28 | * Use launch options to customize in a new workspace 29 | * Bring up the intelligent visual grasp solution on a new robot 30 | * Do hand-eye calibration for a new camera setup 31 | * Launch the example applications 32 | 33 | ## Example Applications 34 | 35 | ### Random Picking (OpenVINO Grasp Detection) 36 | 37 | [Random Pick with OpenVINO Grasp Detection - Link to Youtube video demo](https://www.youtube.com/embed/b4EPvHdidOA) 38 | 39 | ### Recognition Picking (OpenVINO Grasp Detection + OpenVINO Mask-rcnn Object Segmentation) 40 | 41 | [Recognition Pick with OpenVINO Grasp Detection - Link to Youtube video demo](https://www.youtube.com/embed/trIt0uKRXBs) 42 | 43 | ## Known Issues 44 | * Cloud camera failed at "Invalid sizes when resizing a matrix or array" when dealing with XYZRGBA pointcloud from ROS2 Realsenes, tracked as [#6](https://github.com/atenpas/gpg/issues/6) of gpg, [patch](https://github.com/atenpas/gpg/pull/7) under review. 45 | * 'colcon test' sometimes failed with test suite "tgrasp_ros2", due to ROS2 service request failure issue (reported ros2 examples issue [#228](https://github.com/ros2/examples/issues/228) and detailed discussed in ros2 demo issue [#304](https://github.com/ros2/demos/issues/304)) 46 | * Rviz2 failed to receive Static TF from camera due to transient_local QoS (expected in the coming ROS2 Eloquent, discussed in geometry2 issue [#183](https://github.com/ros2/geometry2/issues/183)), workaround [patch](https://github.com/intel/ros2_intel_realsense/pull/88) available till the adaption to Eloquent 47 | 48 | ## Contribute to This Project 49 | It's welcomed to contribute to this project. Here're some recommended practices: 50 | * When adding a new feature it's expected to add tests covering the new functionalities 51 | ```bash 52 | colcon test --packages-select 53 | ``` 54 | * Before submitting a patch, it's recommended to pass all existing tests to avoid regression 55 | ```bash 56 | colcon test --packages-select 57 | ``` 58 | 59 | ###### *Any security issue should be reported using process at https://01.org/security* 60 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ######################################################## 2 | # Based on Ubuntu 18.04 3 | ######################################################## 4 | 5 | # Set the base image to ubuntu 18.04 6 | 7 | FROM ubuntu:bionic 8 | 9 | MAINTAINER Liu Cong "congx.liu@intel.com" 10 | 11 | ARG DEPS_DIR=/root/deps 12 | WORKDIR $DEPS_DIR 13 | 14 | # install ros2 grasp library deps 15 | COPY ./script/ $DEPS_DIR/script/ 16 | RUN bash script/install_ros2_grasp_library_deps.sh /root/deps 17 | 18 | WORKDIR /root 19 | ENTRYPOINT ["/root/script/ros_entrypoint.sh"] 20 | CMD ["bash"] 21 | -------------------------------------------------------------------------------- /docker/README.md: -------------------------------------------------------------------------------- 1 | # Precondition 2 | ## add docker group 3 | ``` 4 | sudo groupadd docker 5 | sudo usermod -aG docker $USER 6 | ``` 7 | ## Build docker image 8 | ``` 9 | cd ros2_grasp_library/docker 10 | docker build -t intel/ros2:ros2_grasp_library_deps . 11 | 12 | ``` 13 | If your use proxy 14 | ``` 15 | docker build -t intel/ros2:ros2_grasp_library_deps --build-arg http_proxy=: --build-arg https_proxy=: . 16 | ``` 17 | ## OPTION:Please refer below command to verify image creating success 18 | ``` 19 | docker images 20 | 21 | REPOSITORY TAG IMAGE ID CREATED SIZE 22 | intel/ros2 ros2_grasp_library_deps b6d619a01f33 1 hours ago 6.92GB 23 | ``` 24 | # Run OpenVINO Grasp Library with RGBD Camera 25 | ## Terminal 1: Build ros2_grasp_library and launch Rviz2 to illustrate detection results. 26 | After the project runs, there will be a pop-up x window, you need to set the operating environment first. 27 | ``` 28 | ./setup_docker_display.sh 29 | 30 | docker run -it --rm --privileged -v /tmp/.X11-unix:/tmp/.X11-unix:rw -v /tmp/.docker.xauth:/tmp/.docker.xauth:rw -v /dev/bus/usb:/dev/bus/usb \ 31 | -v /dev:/dev:rw -e XAUTHORITY=/tmp/.docker.xauth -e DISPLAY --name ros2_grasp_library intel/ros2:ros2_grasp_library_deps bash 32 | 33 | # cd /root/ 34 | # mkdir -p ros2_ws/src 35 | # cd ros2_ws/src 36 | # git clone https://github.com/intel/ros2_grasp_library.git 37 | # git clone https://github.com/intel/ros2_intel_realsense.git -b refactor 38 | # git clone https://github.com/intel/ros2_openvino_toolkit.git 39 | # cd .. 40 | # colcon build --symlink-install --packages-select grasp_msgs moveit_msgs people_msgs grasp_ros2 realsense_msgs realsense_ros realsense_node 41 | # source ./install/local_setup.bash 42 | # ros2 run rviz2 rviz2 -d src/ros2_grasp_library/grasp_ros2/rviz2/grasp.rviz 43 | ``` 44 | ## Terminal 2: launch RGBD camera 45 | ``` 46 | docker exec -t -i ros2_grasp_library bash 47 | 48 | # source /root/ros2_ws/install/setup.bash 49 | # ros2 run realsense_node realsense_node 50 | ``` 51 | ## Terminal 3: launch Grasp Library 52 | ``` 53 | docker exec -t -i ros2_grasp_library bash 54 | 55 | # source /root/ros2_ws/install/setup.bash 56 | # ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/grasp_ros2_params.yaml 57 | ``` 58 | Note: If you haven't already installed or want more information on how to use docker, please see the article here for more information: 59 | https://docs.docker.com/install/ 60 | 61 | -------------------------------------------------------------------------------- /docker/script/00_ros2_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | ros2_version=dashing 5 | SUDO=$1 6 | if [ "$SUDO" == "sudo" ];then 7 | SUDO="sudo" 8 | else 9 | SUDO="" 10 | fi 11 | 12 | # fix popup caused by libssl 13 | $SUDO apt-get install -y debconf-utils \ 14 | echo 'libssl1.0.0:amd64 libraries/restart-without-asking boolean true' | $SUDO debconf-set-selections 15 | 16 | # Authorize gpg key with apt 17 | $SUDO apt-get update && $SUDO apt-get install -y curl gnupg2 lsb-release &&\ 18 | curl http://repo.ros2.org/repos.key | $SUDO apt-key add - 19 | 20 | # Add the repository to sources list 21 | $SUDO sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' 22 | 23 | # Install development tools and ROS tools 24 | $SUDO apt-get update && $SUDO apt-get install -y \ 25 | python-rosdep \ 26 | python3-vcstool \ 27 | python3-colcon-common-extensions 28 | 29 | # Install ROS 2 packages 30 | echo "install $ros2_version" 31 | $SUDO apt-get update && $SUDO apt-get install -y ros-${ros2_version}-desktop 32 | -------------------------------------------------------------------------------- /docker/script/10_eigen_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | DEPS_DIR=${DEPS_PATH} 6 | eigen_version=https://gitlab.com/libeigen/eigen/-/archive/3.2/eigen-3.2.tar.gz 7 | SUDO=$1 8 | if [ "$SUDO" == "sudo" ];then 9 | SUDO="sudo" 10 | else 11 | SUDO="" 12 | fi 13 | 14 | cd $DEPS_DIR 15 | 16 | $SUDO apt-get install -y gfortran 17 | wget -t 3 -c $eigen_version 18 | tar -xvf eigen-3.2.tar.gz 19 | cd eigen-3.2 &&mkdir -p build && cd build 20 | cmake -DCMAKE_BUILD_TYPE=Release .. 21 | make -j4 22 | $SUDO make install 23 | $SUDO rm -rf /usr/include/eigen3/ 24 | $SUDO ln -sf /usr/local/include/eigen3 /usr/include/ 25 | $SUDO make install 26 | -------------------------------------------------------------------------------- /docker/script/11_libpcl_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | DEPS_DIR=${DEPS_PATH} 6 | pcl_version=https://github.com/PointCloudLibrary/pcl/archive/pcl-1.8.1.tar.gz 7 | SUDO=$1 8 | if [ "$SUDO" == "sudo" ];then 9 | SUDO="sudo" 10 | else 11 | SUDO="" 12 | fi 13 | 14 | cd $DEPS_DIR 15 | 16 | $SUDO apt-get install -y libhdf5-dev python3-h5py python3-pip 17 | wget -t 3 -c $pcl_version 18 | tar -xvf pcl-1.8.1.tar.gz 19 | cd pcl-pcl-1.8.1 &&mkdir -p build && cd build 20 | cmake -DCMAKE_BUILD_TYPE=Release .. 21 | make -j4 22 | $SUDO make install 23 | -------------------------------------------------------------------------------- /docker/script/12_opencv_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | opencv_version=4.1.2 5 | SUDO=$1 6 | if [ "$SUDO" == "sudo" ];then 7 | SUDO="sudo" 8 | else 9 | SUDO="" 10 | fi 11 | 12 | #install opencv 13 | cd $DEPS_DIR 14 | $SUDO apt-get update && $SUDO apt-get install -y build-essential \ 15 | libgtk2.0-dev \ 16 | pkg-config \ 17 | libavcodec-dev \ 18 | libavformat-dev \ 19 | libswscale-dev \ 20 | python-dev \ 21 | python-numpy \ 22 | libtbb2 \ 23 | libtbb-dev \ 24 | libjpeg-dev \ 25 | libpng-dev \ 26 | libtiff-dev \ 27 | libdc1394-22-dev 28 | git clone --depth 1 https://github.com/opencv/opencv.git -b $opencv_version 29 | git clone --depth 1 https://github.com/opencv/opencv_contrib.git -b $opencv_version 30 | cd $DEPS_DIR/opencv 31 | mkdir -p build && cd build 32 | cd $DEPS_DIR/opencv/build 33 | cmake -D OPENCV_EXTRA_MODULES_PATH=$DEPS_DIR/opencv_contrib/modules \ 34 | -D CMAKE_BUILD_TYPE=Release \ 35 | -D CMAKE_INSTALL_PREFIX=/usr/local \ 36 | -D BUILD_EXAMPLES=ON \ 37 | -D BUILD_opencv_xfeatures2d=OFF \ 38 | .. 39 | make -j4 40 | $SUDO make install 41 | echo "/usr/local/lib" | $SUDO tee /etc/ld.so.conf.d/opencv.conf 42 | $SUDO ldconfig 43 | -------------------------------------------------------------------------------- /docker/script/13_openvino_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | MKL_URL=https://github.com/intel/mkl-dnn/releases/download/v0.19/mklml_lnx_2019.0.5.20190502.tgz 5 | MKL_VERSION=mklml_lnx_2019.0.5.20190502 6 | OPENVINO_VERSION=2019_R3.1 7 | 8 | SUDO=$1 9 | if [ "$SUDO" == "" ];then 10 | SUDO="sudo" 11 | fi 12 | 13 | # install mkl 2019.0.5.20190502 14 | $SUDO apt-get update && $SUDO apt-get install -y wget 15 | cd $DEPS_DIR 16 | wget -t 3 -c ${MKL_URL} &&\ 17 | tar -xvf ${MKL_VERSION}.tgz &&\ 18 | cd ${MKL_VERSION} &&\ 19 | $SUDO mkdir -p /usr/local/lib/mklml &&\ 20 | $SUDO cp -rf ./lib /usr/local/lib/mklml &&\ 21 | $SUDO cp -rf ./include /usr/local/lib/mklml &&\ 22 | $SUDO touch /usr/local/lib/mklml/version.info 23 | 24 | #install opencl 19.41.14441 25 | cd $DEPS_DIR 26 | mkdir -p opencl && cd opencl &&\ 27 | wget -t 3 -c https://github.com/intel/compute-runtime/releases/download/19.41.14441/intel-gmmlib_19.3.2_amd64.deb &&\ 28 | wget -t 3 -c https://github.com/intel/compute-runtime/releases/download/19.41.14441/intel-igc-core_1.0.2597_amd64.deb &&\ 29 | wget -t 3 -c https://github.com/intel/compute-runtime/releases/download/19.41.14441/intel-igc-opencl_1.0.2597_amd64.deb &&\ 30 | wget -t 3 -c https://github.com/intel/compute-runtime/releases/download/19.41.14441/intel-opencl_19.41.14441_amd64.deb &&\ 31 | wget -t 3 -c https://github.com/intel/compute-runtime/releases/download/19.41.14441/intel-ocloc_19.41.14441_amd64.deb &&\ 32 | $SUDO dpkg -i *.deb 33 | 34 | #install cmake 3.11 35 | if [ $(cmake --version|grep "version"|awk '{print $3}') != "3.14.3" ];then 36 | cd $DEPS_DIR 37 | wget -t 3 -c https://www.cmake.org/files/v3.14/cmake-3.14.3.tar.gz && \ 38 | tar xf cmake-3.14.3.tar.gz && \ 39 | (cd cmake-3.14.3 && ./bootstrap --parallel=$(nproc --all) && make --jobs=$(nproc --all) && $SUDO make install) && \ 40 | rm -rf cmake-3.14.3 cmake-3.14.3.tar.gz 41 | fi 42 | #install openvino 2019_R3.1 43 | cd $DEPS_DIR 44 | $SUDO apt-get update && $SUDO apt-get install -y git 45 | git clone --depth 1 https://github.com/openvinotoolkit/openvino -b ${OPENVINO_VERSION} 46 | cd $DEPS_DIR/openvino/inference-engine 47 | git submodule update --init --recursive &&\ 48 | chmod +x install_dependencies.sh &&\ 49 | $SUDO ./install_dependencies.sh 50 | mkdir -p build && cd build &&\ 51 | cmake -DCMAKE_BUILD_TYPE=Release \ 52 | -DCMAKE_INSTALL_PREFIX=/usr/local \ 53 | -DGEMM=MKL -DMKLROOT=/usr/local/lib/mklml \ 54 | -DTHREADING=OMP \ 55 | -DENABLE_MKL_DNN=ON \ 56 | -DENABLE_CLDNN=ON \ 57 | -DENABLE_OPENCV=OFF \ 58 | .. 59 | cd $DEPS_DIR/openvino/inference-engine/build 60 | make -j8 61 | 62 | cd $DEPS_DIR/openvino/inference-engine/build 63 | $SUDO mkdir -p /usr/share/InferenceEngine &&\ 64 | $SUDO cp InferenceEngineConfig*.cmake /usr/share/InferenceEngine &&\ 65 | $SUDO cp targets.cmake /usr/share/InferenceEngine &&\ 66 | echo `pwd`/../bin/intel64/Release/lib | $SUDO tee -a /etc/ld.so.conf.d/openvino.conf &&\ 67 | $SUDO ldconfig 68 | $SUDO ln -sf $DEPS_DIR/openvino /opt/openvino_toolkit/openvino 69 | -------------------------------------------------------------------------------- /docker/script/20_librealsense_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | librealsense_version=2.31.0-0~realsense0.1791 5 | SUDO=$1 6 | if [ "$SUDO" == "sudo" ];then 7 | SUDO="sudo" 8 | else 9 | SUDO="" 10 | fi 11 | 12 | # install librealsense v2.34.0-0~realsense0.2251 13 | echo "install librealsense 2.34.0-0~realsense0.2251" 14 | cd $DEPS_DIR 15 | if [ $http_proxy == "" ];then 16 | $SUDO apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE 17 | else 18 | $SUDO apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --keyserver-options http-proxy=$http_proxy --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE 19 | fi 20 | $SUDO sh -c 'echo "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo `lsb_release -cs` main" > /etc/apt/sources.list.d/librealsense.list' 21 | $SUDO apt-get update && $SUDO apt-get install -y librealsense2=${librealsense_version} \ 22 | librealsense2-dev=${librealsense_version} \ 23 | librealsense2-udev-rules=${librealsense_version} \ 24 | librealsense2-gl=${librealsense_version} \ 25 | librealsense2-utils=${librealsense_version} \ 26 | librealsense2-dbg=${librealsense_version} \ 27 | librealsense2-dkms 28 | -------------------------------------------------------------------------------- /docker/script/30_gpg_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | SUDO=$1 5 | if [ "$SUDO" == "sudo" ];then 6 | SUDO="sudo" 7 | else 8 | SUDO="" 9 | fi 10 | 11 | # install gpg 12 | cd $DEPS_DIR 13 | wget -t 3 -c https://github.com/atenpas/gpg/archive/3dcd656d70f095ad1bda3d2fb597a994198466ab.zip 14 | unzip 3dcd656d70f095ad1bda3d2fb597a994198466ab.zip 15 | cd gpg-3dcd656d70f095ad1bda3d2fb597a994198466ab 16 | mkdir -p build && cd build 17 | cmake .. && make 18 | $SUDO make install 19 | ls /usr/local/lib/libgrasp_candidates_generator.so 20 | -------------------------------------------------------------------------------- /docker/script/31_gpd_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | SUDO=$1 5 | if [ "$SUDO" == "sudo" ];then 6 | SUDO="sudo" 7 | else 8 | SUDO="" 9 | fi 10 | 11 | # install gpd 12 | cd $DEPS_DIR 13 | git clone --depth 1 https://github.com/sharronliu/gpd.git -b libgpd 14 | cd gpd/src/gpd 15 | mkdir -p build && cd build 16 | cmake -DUSE_OPENVINO=ON .. && make 17 | $SUDO make install 18 | -------------------------------------------------------------------------------- /docker/script/32_ur_modern_driver_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEPS_DIR=${DEPS_PATH} 4 | SUDO=$1 5 | if [ "$SUDO" == "sudo" ];then 6 | SUDO="sudo" 7 | else 8 | SUDO="" 9 | fi 10 | 11 | # install ur_modern_driver 12 | cd $DEPS_DIR 13 | git clone --depth 1 https://github.com/RoboticsYY/ur_modern_driver.git -b libur_modern_driver 14 | cd ur_modern_driver/libur_modern_driver 15 | mkdir -p build && cd build 16 | cmake -DCMAKE_BUILD_TYPE=Release .. && make 17 | $SUDO make install 18 | -------------------------------------------------------------------------------- /docker/script/50_ros2_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SUDO=$1 4 | if [ "$SUDO" == "sudo" ];then 5 | SUDO="sudo" 6 | else 7 | SUDO="" 8 | fi 9 | 10 | $SUDO apt-get install -y ros-dashing-object-msgs \ 11 | python3-scipy \ 12 | ros-dashing-eigen3-cmake-module 13 | 14 | WORK_DIR=${DEPS_PATH}/../ros2_ws 15 | mkdir -p $WORK_DIR/src &&cd $WORK_DIR/src 16 | 17 | git clone --depth 1 https://github.com/RoboticsYY/ros2_ur_description.git 18 | git clone --depth 1 https://github.com/RoboticsYY/handeye 19 | git clone --depth 1 https://github.com/RoboticsYY/criutils.git 20 | git clone --depth 1 https://github.com/RoboticsYY/baldor.git 21 | git clone --depth 1 https://github.com/intel/ros2_intel_realsense.git -b refactor 22 | git clone --depth 1 https://github.com/intel/ros2_grasp_library.git 23 | 24 | cd $WORK_DIR 25 | source /opt/ros/dashing/setup.sh 26 | export InferenceEngine_DIR=/opt/openvino_toolkit/openvino/inference-engine/build/ 27 | export export CPU_EXTENSION_LIB=/opt/openvino_toolkit/openvino/inference-engine/bin/intel64/Release/lib/libcpu_extension.so 28 | export GFLAGS_LIB=/opt/openvino_toolkit/openvino/inference-engine/bin/intel64/Release/lib/libgflags_nothreads.a 29 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$InferenceEngine_DIR/../bin/intel64/Release/lib:/usr/local/lib/mklml/lib 30 | 31 | colcon build --symlink-install 32 | -------------------------------------------------------------------------------- /docker/script/install_ros2_grasp_library.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | deps_path=$1 6 | if [ -z "$deps_path" ]; then 7 | echo -e "warring:\n install_ros2_grasp_library_deps.sh " 8 | echo -e "If you want to use'sudo' : install_ros2_grasp_library_deps.sh sudo" 9 | exit 0 10 | fi 11 | 12 | shift 13 | 14 | # mkdir deps-path 15 | echo "DEPS_PATH = $deps_path" 16 | mkdir -p $deps_path 17 | export DEPS_PATH=$deps_path 18 | 19 | CURRENT_DIR=$(dirname "$(readlink -f "${BASH_SOURCE[0]}")") 20 | echo "CURRENT_DIR = ${CURRENT_DIR}" 21 | 22 | # install ros2 dashing 23 | bash ${CURRENT_DIR}/00_ros2_install.sh $@ 24 | 25 | # instal eigen 3.2 26 | bash ${CURRENT_DIR}/10_eigen_install.sh $@ 27 | 28 | # install libpcl 1.8.1 29 | bash ${CURRENT_DIR}/11_libpcl_install.sh $@ 30 | 31 | # install opencv 4.1.2 32 | bash ${CURRENT_DIR}/12_opencv_install.sh $@ 33 | 34 | # install openvino 2019_R3.1 35 | bash ${CURRENT_DIR}/13_openvino_install.sh $@ 36 | 37 | # install librealsense 2.31 38 | bash ${CURRENT_DIR}/20_librealsense_install.sh $@ 39 | 40 | # install gpg 41 | bash ${CURRENT_DIR}/30_gpg_install.sh $@ 42 | 43 | # install gpd 44 | bash ${CURRENT_DIR}/31_gpd_install.sh $@ 45 | 46 | # install ur_modern_driver 47 | bash ${CURRENT_DIR}/32_ur_modern_driver_install.sh $@ 48 | 49 | # build ros2 other deps 50 | bash ${CURRENT_DIR}/50_ros2_deps.sh $@ 51 | -------------------------------------------------------------------------------- /docker/script/ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | # setup ros2 environment 6 | source /opt/ros/dashing/setup.bash 7 | 8 | source /root/ros2_ws/install/setup.bash 9 | 10 | exec "$@" 11 | -------------------------------------------------------------------------------- /docker/script/ros_env.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ROS_PATH=$(pwd) 4 | 5 | # setup ros2 environment 6 | source /opt/ros/dashing/setup.bash 7 | source ${ROS_PATH}/install/setup.bash 8 | export ROS_DOMAIN_ID=100 # robot_group_id 9 | 10 | export InferenceEngine_DIR=/opt/openvino_toolkit/openvino/inference-engine/build/ 11 | export export CPU_EXTENSION_LIB=/opt/openvino_toolkit/openvino/inference-engine/bin/intel64/Release/lib/libcpu_extension.so 12 | export GFLAGS_LIB=/opt/openvino_toolkit/openvino/inference-engine/bin/intel64/Release/lib/libgflags_nothreads.a 13 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$InferenceEngine_DIR/../bin/intel64/Release/lib:/usr/local/lib/mklml/lib 14 | -------------------------------------------------------------------------------- /docker/setup_docker_display.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup docker display 5 | XSOCK=/tmp/.X11-unix 6 | XAUTH=/tmp/.docker.xauth 7 | touch $XAUTH 8 | xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - 9 | -------------------------------------------------------------------------------- /grasp_apps/draw_x/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel 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 | cmake_minimum_required(VERSION 3.5) 16 | project(draw_x) 17 | 18 | if(NOT CMAKE_CXX_STANDARD) 19 | set(CMAKE_CXX_STANDARD 14) 20 | endif() 21 | 22 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 23 | add_compile_options(-Wall -Wextra -Wpedantic) 24 | endif() 25 | 26 | if(CMAKE_BUILD_TYPE EQUAL "RELEASE") 27 | message(STATUS "Create Release Build.") 28 | set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}") 29 | else() 30 | message(STATUS "Create Debug Build.") 31 | endif() 32 | 33 | find_package(ament_cmake REQUIRED) 34 | find_package(rclcpp REQUIRED) 35 | find_package(robot_interface REQUIRED) 36 | 37 | include_directories( 38 | include 39 | ${rclcpp_INCLUDE_DIRS} 40 | ${robot_interface_INCLUDE_DIRS} 41 | ) 42 | 43 | # draw_x app 44 | add_executable(${PROJECT_NAME} 45 | src/draw_x.cpp 46 | ) 47 | 48 | ament_target_dependencies(${PROJECT_NAME} 49 | "rclcpp" 50 | "robot_interface" 51 | ) 52 | 53 | target_link_libraries(${PROJECT_NAME} 54 | ${ament_LIBRARIES} 55 | ${robot_interface_LIBRARIES} 56 | ) 57 | 58 | # Install binaries 59 | install(TARGETS ${PROJECT_NAME} 60 | RUNTIME DESTINATION bin 61 | ) 62 | install(TARGETS ${PROJECT_NAME} 63 | DESTINATION lib/${PROJECT_NAME} 64 | ) 65 | 66 | # Install launch files. 67 | install(DIRECTORY 68 | launch 69 | DESTINATION share/${PROJECT_NAME}/ 70 | ) 71 | 72 | # Flags 73 | if(UNIX OR APPLE) 74 | # Linker flags. 75 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 76 | # GCC specific flags. ICC is compatible with them. 77 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z noexecstack -z relro -z now") 78 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack -z relro -z now") 79 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 80 | # In Clang, -z flags are not compatible, they need to be passed to linker via -Wl. 81 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} \ 82 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 83 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} \ 84 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 85 | endif() 86 | 87 | # Compiler flags. 88 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") 89 | # GCC specific flags. 90 | if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9 OR 91 | CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.9) 92 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector-strong") 93 | else() 94 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 95 | endif() 96 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 97 | # Clang is compatbile with some of the flags. 98 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 99 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 100 | # Same as above, with exception that ICC compilation crashes with -fPIE option, even 101 | # though it uses -pie linker option that require -fPIE during compilation. Checksec 102 | # shows that it generates correct PIE anyway if only -pie is provided. 103 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fstack-protector") 104 | endif() 105 | 106 | # Generic flags. 107 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -fno-operator-names -Wformat -Wformat-security \ 108 | -Wall -fopenmp") 109 | set( CUDA_PROPAGATE_HOST_FLAGS OFF ) 110 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_FORTIFY_SOURCE=2") 111 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") 112 | endif() 113 | 114 | ament_package() 115 | -------------------------------------------------------------------------------- /grasp_apps/draw_x/README.md: -------------------------------------------------------------------------------- 1 | ../../grasp_tutorials/doc/draw_x.rst -------------------------------------------------------------------------------- /grasp_apps/draw_x/launch/draw_x.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | import launch 18 | import launch.actions 19 | import launch.substitutions 20 | import launch_ros.actions 21 | from ament_index_python.packages import get_package_share_directory 22 | 23 | def generate_launch_description(): 24 | 25 | # .yaml file for configuring the parameters 26 | yaml = os.path.join( 27 | get_package_share_directory('draw_x'), 28 | 'launch', 'draw_x.yaml' 29 | ) 30 | 31 | return launch.LaunchDescription([ 32 | 33 | launch_ros.actions.Node( 34 | package='draw_x', 35 | node_executable='draw_x', 36 | output='screen', arguments=['__params:='+yaml]), 37 | 38 | ]) -------------------------------------------------------------------------------- /grasp_apps/draw_x/launch/draw_x.yaml: -------------------------------------------------------------------------------- 1 | robot_control: 2 | ros__parameters: 3 | host: "192.168.0.5" 4 | shutdown_on_disconnect: true 5 | joint_names: ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] 6 | -------------------------------------------------------------------------------- /grasp_apps/draw_x/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | draw_x 5 | 0.5.0 6 | A demo app for draw_x 7 | Yu Yan 8 | Yu Yan 9 | Apache License 2.0 10 | 11 | ament_cmake 12 | rclcpp 13 | robot_interface 14 | 15 | rclcpp 16 | robot_interface 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /grasp_apps/draw_x/src/draw_x.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | /* pose in joint values*/ 6 | static const std::vector HOME = {0.87, -1.44, 1.68, -1.81, -1.56, 0}; 7 | /* pose in [x, y, z, R, P, Y]*/ 8 | static const std::vector CORNER1_POSE = { 0.1, -0.65, 0.15, 3.14, 0, -3.14}; 9 | static const std::vector CORNER2_POSE = {-0.1, -0.45, 0.15, 3.14, 0, -3.14}; 10 | static const std::vector CORNER3_POSE = {-0.1, -0.65, 0.15, 3.14, 0, -3.14}; 11 | static const std::vector CORNER4_POSE = { 0.1, -0.45, 0.15, 3.14, 0, -3.14}; 12 | 13 | int main(int argc, char **argv) 14 | { 15 | rclcpp::init(argc, argv); 16 | 17 | // init robot control 18 | auto robot = std::make_shared("robot_control", 19 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 20 | robot->parseArgs(); 21 | robot->startLoop(); 22 | rclcpp::sleep_for(2s); 23 | 24 | // Move to home 25 | robot->moveToJointValues(HOME, 1.05, 1.4); 26 | 27 | // Move to the first corner 28 | robot->moveToTcpPose(CORNER1_POSE[0], CORNER1_POSE[1], CORNER1_POSE[2], 29 | CORNER1_POSE[3], CORNER1_POSE[4], CORNER1_POSE[5], 1.05, 1.4); 30 | 31 | robot->moveToTcpPose(CORNER1_POSE[0], CORNER1_POSE[1], CORNER1_POSE[2] - 0.05, 32 | CORNER1_POSE[3], CORNER1_POSE[4], CORNER1_POSE[5], 1.05, 1.4); 33 | 34 | // Move to the second corner 35 | robot->moveToTcpPose(CORNER2_POSE[0], CORNER2_POSE[1], CORNER2_POSE[2] - 0.05, 36 | CORNER2_POSE[3], CORNER2_POSE[4], CORNER2_POSE[5], 1.05, 1.4); 37 | 38 | robot->moveToTcpPose(CORNER2_POSE[0], CORNER2_POSE[1], CORNER2_POSE[2], 39 | CORNER2_POSE[3], CORNER2_POSE[4], CORNER2_POSE[5], 1.05, 1.4); 40 | 41 | // Move to the third corner 42 | robot->moveToTcpPose(CORNER3_POSE[0], CORNER3_POSE[1], CORNER3_POSE[2], 43 | CORNER3_POSE[3], CORNER3_POSE[4], CORNER3_POSE[5], 1.05, 1.4); 44 | 45 | robot->moveToTcpPose(CORNER3_POSE[0], CORNER3_POSE[1], CORNER3_POSE[2] - 0.05, 46 | CORNER3_POSE[3], CORNER3_POSE[4], CORNER3_POSE[5], 1.05, 1.4); 47 | 48 | // Move to the fourth corner 49 | robot->moveToTcpPose(CORNER4_POSE[0], CORNER4_POSE[1], CORNER4_POSE[2] - 0.05, 50 | CORNER4_POSE[3], CORNER4_POSE[4], CORNER4_POSE[5], 1.05, 1.4); 51 | 52 | robot->moveToTcpPose(CORNER4_POSE[0], CORNER4_POSE[1], CORNER4_POSE[2], 53 | CORNER4_POSE[3], CORNER4_POSE[4], CORNER4_POSE[5], 1.05, 1.4); 54 | 55 | // Move back to home 56 | robot->moveToJointValues(HOME, 1.05, 1.4); 57 | 58 | rclcpp::shutdown(); 59 | return 0; 60 | 61 | } 62 | -------------------------------------------------------------------------------- /grasp_apps/fixed_position_pick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel 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 | cmake_minimum_required(VERSION 3.5) 16 | project(fixed_position_pick) 17 | 18 | if(NOT CMAKE_CXX_STANDARD) 19 | set(CMAKE_CXX_STANDARD 14) 20 | endif() 21 | 22 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 23 | add_compile_options(-Wall -Wextra -Wpedantic) 24 | endif() 25 | 26 | if(CMAKE_BUILD_TYPE EQUAL "RELEASE") 27 | message(STATUS "Create Release Build.") 28 | set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}") 29 | else() 30 | message(STATUS "Create Debug Build.") 31 | endif() 32 | 33 | find_package(ament_cmake REQUIRED) 34 | find_package(rclcpp REQUIRED) 35 | find_package(robot_interface REQUIRED) 36 | 37 | include_directories( 38 | include 39 | ${rclcpp_INCLUDE_DIRS} 40 | ${robot_interface_INCLUDE_DIRS} 41 | ) 42 | 43 | # draw_x app 44 | add_executable(${PROJECT_NAME} 45 | src/fixed_position_pick.cpp 46 | ) 47 | 48 | ament_target_dependencies(${PROJECT_NAME} 49 | "rclcpp" 50 | "robot_interface" 51 | ) 52 | 53 | target_link_libraries(${PROJECT_NAME} 54 | ${ament_LIBRARIES} 55 | ${robot_interface_LIBRARIES} 56 | ) 57 | 58 | # Install binaries 59 | install(TARGETS ${PROJECT_NAME} 60 | RUNTIME DESTINATION bin 61 | ) 62 | install(TARGETS ${PROJECT_NAME} 63 | DESTINATION lib/${PROJECT_NAME} 64 | ) 65 | 66 | # Install launch files. 67 | install(DIRECTORY 68 | launch 69 | DESTINATION share/${PROJECT_NAME}/ 70 | ) 71 | 72 | # Flags 73 | if(UNIX OR APPLE) 74 | # Linker flags. 75 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 76 | # GCC specific flags. ICC is compatible with them. 77 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z noexecstack -z relro -z now") 78 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack -z relro -z now") 79 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 80 | # In Clang, -z flags are not compatible, they need to be passed to linker via -Wl. 81 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} \ 82 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 83 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} \ 84 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 85 | endif() 86 | 87 | # Compiler flags. 88 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") 89 | # GCC specific flags. 90 | if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9 OR 91 | CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.9) 92 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector-strong") 93 | else() 94 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 95 | endif() 96 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 97 | # Clang is compatbile with some of the flags. 98 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 99 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 100 | # Same as above, with exception that ICC compilation crashes with -fPIE option, even 101 | # though it uses -pie linker option that require -fPIE during compilation. Checksec 102 | # shows that it generates correct PIE anyway if only -pie is provided. 103 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fstack-protector") 104 | endif() 105 | 106 | # Generic flags. 107 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -fno-operator-names -Wformat -Wformat-security \ 108 | -Wall -fopenmp") 109 | set( CUDA_PROPAGATE_HOST_FLAGS OFF ) 110 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_FORTIFY_SOURCE=2") 111 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") 112 | endif() 113 | 114 | ament_package() -------------------------------------------------------------------------------- /grasp_apps/fixed_position_pick/README.md: -------------------------------------------------------------------------------- 1 | ../../grasp_tutorials/doc/fixed_position_pick.rst -------------------------------------------------------------------------------- /grasp_apps/fixed_position_pick/launch/fixed_position_pick.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | import launch 18 | import launch.actions 19 | import launch.substitutions 20 | import launch_ros.actions 21 | from ament_index_python.packages import get_package_share_directory 22 | 23 | def generate_launch_description(): 24 | 25 | # .yaml file for configuring the parameters 26 | yaml = os.path.join( 27 | get_package_share_directory('fixed_position_pick'), 28 | 'launch', 'fixed_position_pick.yaml' 29 | ) 30 | 31 | return launch.LaunchDescription([ 32 | 33 | launch_ros.actions.Node( 34 | package='fixed_position_pick', 35 | node_executable='fixed_position_pick', 36 | output='screen', arguments=['__params:='+yaml]), 37 | 38 | ]) -------------------------------------------------------------------------------- /grasp_apps/fixed_position_pick/launch/fixed_position_pick.yaml: -------------------------------------------------------------------------------- 1 | robot_control: 2 | ros__parameters: 3 | host: "192.168.0.5" 4 | shutdown_on_disconnect: true 5 | joint_names: ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] 6 | -------------------------------------------------------------------------------- /grasp_apps/fixed_position_pick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fixed_position_pick 5 | 0.5.0 6 | A demo app for draw_x 7 | Yu Yan 8 | Yu Yan 9 | Apache License 2.0 10 | 11 | ament_cmake 12 | rclcpp 13 | robot_interface 14 | 15 | rclcpp 16 | robot_interface 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | -------------------------------------------------------------------------------- /grasp_apps/fixed_position_pick/src/fixed_position_pick.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | /* pose in joint values*/ 6 | static const std::vector HOME = {0.87, -1.44, 1.68, -1.81, -1.56, 0}; 7 | /* pose in [x, y, z, qx, qy, qz, qw]*/ 8 | static const std::vector PICK_POSE = { -0.157402, -0.679509, 0.094437, 0.190600, 0.948295, 0.239947, 0.082662}; 9 | static const std::vector PLACE_POSE = {-0.350, -0.296, 0.145, -0.311507, 0.950216, -0.004305, 0.005879}; 10 | 11 | int main(int argc, char **argv) 12 | { 13 | rclcpp::init(argc, argv); 14 | 15 | // init robot control 16 | auto robot = std::make_shared("robot_control", 17 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 18 | robot->parseArgs(); 19 | robot->startLoop(); 20 | rclcpp::sleep_for(2s); 21 | 22 | // Move to home 23 | robot->moveToJointValues(HOME, 1.05, 1.4); 24 | 25 | // Pick 26 | geometry_msgs::msg::PoseStamped pose_pick; 27 | pose_pick.header.frame_id = "base"; 28 | pose_pick.header.stamp = robot->now(); 29 | pose_pick.pose.position.x = PICK_POSE[0]; 30 | pose_pick.pose.position.y = PICK_POSE[1]; 31 | pose_pick.pose.position.z = PICK_POSE[2]; 32 | pose_pick.pose.orientation.x = PICK_POSE[3]; 33 | pose_pick.pose.orientation.y = PICK_POSE[4]; 34 | pose_pick.pose.orientation.z = PICK_POSE[5]; 35 | pose_pick.pose.orientation.w = PICK_POSE[6]; 36 | 37 | robot->pick(pose_pick, 1.05, 1.4, 0.5, 0.1); 38 | 39 | // Place 40 | geometry_msgs::msg::PoseStamped pose_place; 41 | pose_place.header.frame_id = "base"; 42 | pose_place.header.stamp = robot->now(); 43 | pose_place.pose.position.x = PLACE_POSE[0]; 44 | pose_place.pose.position.y = PLACE_POSE[1]; 45 | pose_place.pose.position.z = PLACE_POSE[2]; 46 | pose_place.pose.orientation.x = PLACE_POSE[3]; 47 | pose_place.pose.orientation.y = PLACE_POSE[4]; 48 | pose_place.pose.orientation.z = PLACE_POSE[5]; 49 | pose_place.pose.orientation.w = PLACE_POSE[6]; 50 | 51 | robot->place(pose_place, 1.05, 1.4, 0.5, 0.1); 52 | 53 | // Move back to home 54 | robot->moveToJointValues(HOME, 1.05, 1.4); 55 | 56 | rclcpp::shutdown(); 57 | return 0; 58 | 59 | } -------------------------------------------------------------------------------- /grasp_apps/random_pick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel 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 | option(BUILD_RANDOM_PICK "build random_pick app" ON) 16 | if(NOT BUILD_RANDOM_PICK STREQUAL "ON") 17 | return() 18 | endif() 19 | 20 | cmake_minimum_required(VERSION 3.5) 21 | project(random_pick) 22 | 23 | if(NOT CMAKE_CXX_STANDARD) 24 | set(CMAKE_CXX_STANDARD 14) 25 | endif() 26 | 27 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 28 | add_compile_options(-Wall -Wextra -Wpedantic) 29 | endif() 30 | 31 | if(CMAKE_BUILD_TYPE EQUAL "RELEASE") 32 | message(STATUS "Create Release Build.") 33 | set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}") 34 | else() 35 | message(STATUS "Create Debug Build.") 36 | endif() 37 | 38 | find_package(ament_cmake REQUIRED) 39 | find_package(rclcpp REQUIRED) 40 | find_package(moveit_msgs REQUIRED) 41 | find_package(robot_interface REQUIRED) 42 | find_package(tf2_ros REQUIRED) 43 | 44 | include_directories( 45 | include 46 | ${rclcpp_INCLUDE_DIRS} 47 | ${moveit_msgs_INCLUDE_DIRS} 48 | ${robot_interface_INCLUDE_DIRS} 49 | ${tf2_ros_INCLUDE_DIRS} 50 | ) 51 | 52 | # random_pick app 53 | add_executable(${PROJECT_NAME} 54 | src/random_pick.cpp 55 | ) 56 | 57 | ament_target_dependencies(${PROJECT_NAME} 58 | "rclcpp" 59 | "moveit_msgs" 60 | "robot_interface" 61 | "tf2_ros" 62 | ) 63 | 64 | target_link_libraries(${PROJECT_NAME} 65 | ${ament_LIBRARIES} 66 | ${robot_interface_LIBRARIES} 67 | ) 68 | 69 | # Install binaries 70 | install(TARGETS ${PROJECT_NAME} 71 | RUNTIME DESTINATION bin 72 | ) 73 | install(TARGETS ${PROJECT_NAME} 74 | DESTINATION lib/${PROJECT_NAME} 75 | ) 76 | 77 | # Flags 78 | if(UNIX OR APPLE) 79 | # Linker flags. 80 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 81 | # GCC specific flags. ICC is compatible with them. 82 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z noexecstack -z relro -z now") 83 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack -z relro -z now") 84 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 85 | # In Clang, -z flags are not compatible, they need to be passed to linker via -Wl. 86 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} \ 87 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 88 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} \ 89 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 90 | endif() 91 | 92 | # Compiler flags. 93 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") 94 | # GCC specific flags. 95 | if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9 OR 96 | CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.9) 97 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector-strong") 98 | else() 99 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 100 | endif() 101 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 102 | # Clang is compatbile with some of the flags. 103 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 104 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 105 | # Same as above, with exception that ICC compilation crashes with -fPIE option, even 106 | # though it uses -pie linker option that require -fPIE during compilation. Checksec 107 | # shows that it generates correct PIE anyway if only -pie is provided. 108 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fstack-protector") 109 | endif() 110 | 111 | # Generic flags. 112 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -fno-operator-names -Wformat -Wformat-security \ 113 | -Wall -fopenmp") 114 | set( CUDA_PROPAGATE_HOST_FLAGS OFF ) 115 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_FORTIFY_SOURCE=2") 116 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") 117 | endif() 118 | 119 | ament_package() 120 | -------------------------------------------------------------------------------- /grasp_apps/random_pick/README.md: -------------------------------------------------------------------------------- 1 | ../../grasp_tutorials/doc/random_pick.rst -------------------------------------------------------------------------------- /grasp_apps/random_pick/cfg/random_pick.yaml: -------------------------------------------------------------------------------- 1 | robot_control: 2 | ros__parameters: 3 | host: "192.168.1.5" 4 | -------------------------------------------------------------------------------- /grasp_apps/random_pick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | random_pick 5 | 0.5.0 6 | A demo app for grasp detection, and random picking 7 | Sharron LIU 8 | Sharron LIU 9 | Apache License 2.0 10 | 11 | ament_cmake 12 | rclcpp 13 | moveit_msgs 14 | people_msgs 15 | robot_interface 16 | tf2_ros 17 | 18 | rclcpp 19 | moveit_msgs 20 | people_msgs 21 | robot_interface 22 | tf2_ros 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /grasp_apps/random_pick/src/random_pick.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define robot_enable 11 | 12 | using GraspPlanning = moveit_msgs::srv::GraspPlanning; 13 | /* pick position in [x, y, z, R, P, Y]*/ 14 | static std::vector pick_ = {0.0, -0.54, 0.145, 3.14, 0.0, 1.956}; 15 | /* place position in [x, y, z, R, P, Y]*/ 16 | static std::vector place_ = {-0.50, -0.30, 0.20, 3.14, 0.0, 1.956}; 17 | /* pre-pick position in joint values*/ 18 | static std::vector joint_values_pick = {1.065, -1.470, 1.477, -1.577, -1.556, 0}; 19 | /* place position in joint values*/ 20 | static std::vector joint_values_place = {0.385, -1.470, 1.477, -1.577, -1.556, 0}; 21 | static double vel_ = 0.9, acc_ = 0.9, vscale_ = 0.9, appr_ = 0.1; 22 | static std::shared_ptr robot_ = nullptr; 23 | static rclcpp::Node::SharedPtr node_ = nullptr; 24 | static std::shared_ptr result_ = nullptr; 25 | 26 | int main(int argc, char **argv) 27 | { 28 | rclcpp::init(argc, argv); 29 | 30 | // init robot control 31 | robot_ = std::make_shared("robot_control", 32 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 33 | robot_->parseArgs(); 34 | robot_->startLoop(); 35 | rclcpp::sleep_for(2s); 36 | 37 | #ifdef robot_enable 38 | // reset joint 39 | robot_->moveToJointValues(joint_values_place, vel_, acc_); 40 | #endif 41 | 42 | // init random pick node 43 | node_ = rclcpp::Node::make_shared("random_pick"); 44 | tf2_ros::StaticTransformBroadcaster tfb(node_); 45 | // create client for grasp planning 46 | auto client = node_->create_client("plan_grasps"); 47 | // wait for service 48 | while (!client->wait_for_service(5s)) { 49 | RCLCPP_INFO(node_->get_logger(), "Wait for service"); 50 | } 51 | RCLCPP_INFO(node_->get_logger(), "Got service"); 52 | 53 | while(rclcpp::ok()) 54 | { 55 | // request grasp poses 56 | auto request = std::make_shared(); 57 | auto result_future = client->async_send_request(request); 58 | RCLCPP_INFO(node_->get_logger(), "Request sent"); 59 | // wait for response 60 | if (rclcpp::spin_until_future_complete(node_, result_future) != 61 | rclcpp::executor::FutureReturnCode::SUCCESS) 62 | { 63 | continue; 64 | } 65 | // get response 66 | if (moveit_msgs::msg::MoveItErrorCodes::SUCCESS == result_future.get()->error_code.val) { 67 | result_ = result_future.get(); 68 | RCLCPP_INFO(node_->get_logger(), "Response received %d", result_->error_code.val); 69 | } else continue; 70 | 71 | geometry_msgs::msg::PoseStamped p = result_->grasps[0].grasp_pose; 72 | // publish grasp pose 73 | tf2::Quaternion q(p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w); 74 | double roll, pitch, yaw; 75 | tf2::Matrix3x3 r; 76 | r.setRotation(q); 77 | r.getRPY(roll, pitch, yaw); 78 | RCLCPP_INFO(node_->get_logger(), "**********pick pose [position %f %f %f, quat %f %f %f %f, RPY %f %f %f]", 79 | p.pose.position.x, p.pose.position.y, p.pose.position.z, 80 | p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w, 81 | roll, pitch, yaw); 82 | geometry_msgs::msg::TransformStamped tf_msg; 83 | tf_msg.header = p.header; 84 | tf_msg.child_frame_id = "grasp_pose"; 85 | tf_msg.transform.translation.x = p.pose.position.x; 86 | tf_msg.transform.translation.y = p.pose.position.y; 87 | tf_msg.transform.translation.z = p.pose.position.z; 88 | tf_msg.transform.rotation = p.pose.orientation; 89 | tfb.sendTransform(tf_msg); 90 | 91 | #ifdef robot_enable 92 | // pick 93 | robot_->moveToJointValues(joint_values_pick, vel_, acc_); 94 | robot_->pick(p, vel_, acc_, vscale_, appr_); 95 | // place 96 | robot_->moveToJointValues(joint_values_place, vel_, acc_); 97 | robot_->place(place_[0], place_[1], place_[2], place_[3], place_[4], place_[5], vel_, acc_, vscale_, appr_); 98 | #endif 99 | } 100 | 101 | rclcpp::shutdown(); 102 | return 0; 103 | 104 | } 105 | -------------------------------------------------------------------------------- /grasp_apps/recognize_pick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel 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 | option(BUILD_RECOGNIZE_PICK "build recognize_pick app" OFF) 16 | if(NOT BUILD_RECOGNIZE_PICK STREQUAL "ON") 17 | return() 18 | endif() 19 | 20 | cmake_minimum_required(VERSION 3.5) 21 | project(recognize_pick) 22 | 23 | if(NOT CMAKE_CXX_STANDARD) 24 | set(CMAKE_CXX_STANDARD 14) 25 | endif() 26 | 27 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 28 | add_compile_options(-Wall -Wextra -Wpedantic) 29 | endif() 30 | 31 | if(CMAKE_BUILD_TYPE EQUAL "RELEASE") 32 | message(STATUS "Create Release Build.") 33 | set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}") 34 | else() 35 | message(STATUS "Create Debug Build.") 36 | endif() 37 | 38 | find_package(ament_cmake REQUIRED) 39 | find_package(rclcpp REQUIRED) 40 | find_package(moveit_msgs REQUIRED) 41 | find_package(people_msgs REQUIRED) 42 | find_package(robot_interface REQUIRED) 43 | find_package(tf2_ros REQUIRED) 44 | 45 | include_directories( 46 | include 47 | ${rclcpp_INCLUDE_DIRS} 48 | ${moveit_msgs_INCLUDE_DIRS} 49 | ${people_msgs_INCLUDE_DIRS} 50 | ${robot_interface_INCLUDE_DIRS} 51 | ${tf2_ros_INCLUDE_DIRS} 52 | ) 53 | 54 | # recognize_pick app 55 | add_executable(${PROJECT_NAME} 56 | src/recognize_pick.cpp 57 | ) 58 | 59 | ament_target_dependencies(${PROJECT_NAME} 60 | "rclcpp" 61 | "moveit_msgs" 62 | "people_msgs" 63 | "robot_interface" 64 | "tf2_ros" 65 | ) 66 | 67 | target_link_libraries(${PROJECT_NAME} 68 | ${ament_LIBRARIES} 69 | ${robot_interface_LIBRARIES} 70 | ) 71 | 72 | # place publisher app 73 | set(PLACE_PUBLISHER place_publisher) 74 | add_executable(${PLACE_PUBLISHER} 75 | src/place_publisher.cpp 76 | ) 77 | 78 | ament_target_dependencies(${PLACE_PUBLISHER} 79 | "rclcpp" 80 | "moveit_msgs" 81 | ) 82 | 83 | target_link_libraries(${PLACE_PUBLISHER} 84 | ${ament_LIBRARIES} 85 | ) 86 | 87 | # Install binaries 88 | install(TARGETS ${PROJECT_NAME} ${PLACE_PUBLISHER} 89 | RUNTIME DESTINATION bin 90 | ) 91 | install(TARGETS ${PROJECT_NAME} ${PLACE_PUBLISHER} 92 | DESTINATION lib/${PROJECT_NAME} 93 | ) 94 | 95 | # Flags 96 | if(UNIX OR APPLE) 97 | # Linker flags. 98 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" OR ${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 99 | # GCC specific flags. ICC is compatible with them. 100 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -z noexecstack -z relro -z now") 101 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack -z relro -z now") 102 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 103 | # In Clang, -z flags are not compatible, they need to be passed to linker via -Wl. 104 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} \ 105 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 106 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} \ 107 | -Wl,-z,noexecstack -Wl,-z,relro -Wl,-z,now") 108 | endif() 109 | 110 | # Compiler flags. 111 | if(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") 112 | # GCC specific flags. 113 | if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9 OR 114 | CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.9) 115 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector-strong") 116 | else() 117 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 118 | endif() 119 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 120 | # Clang is compatbile with some of the flags. 121 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIE -fstack-protector") 122 | elseif(${CMAKE_CXX_COMPILER_ID} STREQUAL "Intel") 123 | # Same as above, with exception that ICC compilation crashes with -fPIE option, even 124 | # though it uses -pie linker option that require -fPIE during compilation. Checksec 125 | # shows that it generates correct PIE anyway if only -pie is provided. 126 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fstack-protector") 127 | endif() 128 | 129 | # Generic flags. 130 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -fno-operator-names -Wformat -Wformat-security \ 131 | -Wall -fopenmp") 132 | set( CUDA_PROPAGATE_HOST_FLAGS OFF ) 133 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_FORTIFY_SOURCE=2") 134 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") 135 | endif() 136 | 137 | ament_package() 138 | -------------------------------------------------------------------------------- /grasp_apps/recognize_pick/README.md: -------------------------------------------------------------------------------- 1 | ../../grasp_tutorials/doc/recognize_pick.rst -------------------------------------------------------------------------------- /grasp_apps/recognize_pick/cfg/recognize_pick.yaml: -------------------------------------------------------------------------------- 1 | robot_control: 2 | ros__parameters: 3 | host: "192.168.1.5" 4 | -------------------------------------------------------------------------------- /grasp_apps/recognize_pick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | recognize_pick 5 | 0.5.0 6 | A demo app for object segmentation, grasp detection, and picking 7 | Sharron LIU 8 | Sharron LIU 9 | Apache License 2.0 10 | 11 | ament_cmake 12 | rclcpp 13 | moveit_msgs 14 | people_msgs 15 | robot_interface 16 | tf2_ros 17 | 18 | rclcpp 19 | moveit_msgs 20 | people_msgs 21 | robot_interface 22 | tf2_ros 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /grasp_apps/recognize_pick/src/place_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel 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 | #include 16 | #include 17 | #include 18 | 19 | int main(int argc, char ** argv) { 20 | std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); 21 | auto node = rclcpp::Node::make_shared("PlacePublisher"); 22 | auto pub = node->create_publisher("/recognize_pick/place", 10); 23 | 24 | rclcpp::Clock clock(RCL_ROS_TIME); 25 | 26 | moveit_msgs::msg::PlaceLocation p; 27 | if (args.size() < 5) { 28 | p.place_pose.pose.position.x = -0.45; 29 | p.place_pose.pose.position.y = -0.30; 30 | p.place_pose.pose.position.z = 0.125; 31 | } else { 32 | p.place_pose.pose.position.x = atof(args[2].c_str()); 33 | p.place_pose.pose.position.y = atof(args[3].c_str()); 34 | p.place_pose.pose.position.z = atof(args[4].c_str()); 35 | } 36 | if (args.size() < 2) { 37 | RCLCPP_INFO(node->get_logger(), "Place publisher specifying object name and place position."); 38 | RCLCPP_INFO(node->get_logger(), "Usage: place_publisher object_name [x y z]"); 39 | RCLCPP_INFO(node->get_logger(), "Example: place_publisher sports_ball"); 40 | RCLCPP_INFO(node->get_logger(), "Example: place_publisher sports_ball -0.45 -0.30 0.125"); 41 | rclcpp::shutdown(); 42 | return 0; 43 | } else { 44 | p.id = args[1]; 45 | } 46 | 47 | RCLCPP_INFO(node->get_logger(), "place publisher %s [%f %f %f]", 48 | p.id.c_str(), p.place_pose.pose.position.x, p.place_pose.pose.position.y, p.place_pose.pose.position.z); 49 | 50 | while (rclcpp::ok()) { 51 | p.place_pose.header.stamp = clock.now(); 52 | p.place_pose.header.frame_id = "base"; 53 | pub->publish(p); 54 | rclcpp::Rate(0.5).sleep(); 55 | } 56 | rclcpp::spin(node); 57 | rclcpp::shutdown(); 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /grasp_apps/recognize_pick/src/recognize_pick.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define robot_enable 11 | 12 | using GraspPlanning = moveit_msgs::srv::GraspPlanning; 13 | /* pick position in [x, y, z, R, P, Y]*/ 14 | static std::vector pick_ = {0.0, -0.54, 0.145, 3.14, 0.0, 1.956}; 15 | /* place position in [x, y, z, R, P, Y]*/ 16 | static std::vector place_ = {-0.45, -0.30, 0.125, 3.14, 0.0, 1.956}; 17 | /* pre-pick position in joint values*/ 18 | static std::vector joint_values_pick = {1.065, -1.470, 1.477, -1.577, -1.556, 0}; 19 | /* place position in joint values*/ 20 | static std::vector joint_values_place = {0.385, -1.470, 1.477, -1.577, -1.556, 0}; 21 | static double vel_ = 0.4, acc_ = 0.4, vscale_ = 0.5, appr_ = 0.1; 22 | static std::shared_ptr robot_ = nullptr; 23 | static rclcpp::Node::SharedPtr node_ = nullptr; 24 | static std::shared_ptr result_ = nullptr; 25 | static moveit_msgs::msg::PlaceLocation::SharedPtr place_pose_ = nullptr; 26 | 27 | static void place_callback(const moveit_msgs::msg::PlaceLocation::SharedPtr msg) { 28 | place_pose_ = msg; 29 | } 30 | 31 | int main(int argc, char **argv) 32 | { 33 | rclcpp::init(argc, argv); 34 | 35 | // init robot control 36 | robot_ = std::make_shared("robot_control", 37 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 38 | robot_->parseArgs(); 39 | robot_->startLoop(); 40 | rclcpp::sleep_for(2s); 41 | 42 | #ifdef robot_enable 43 | // reset joint 44 | robot_->moveToJointValues(joint_values_place, vel_, acc_); 45 | #endif 46 | 47 | // init random pick node 48 | node_ = rclcpp::Node::make_shared("random_pick"); 49 | tf2_ros::StaticTransformBroadcaster tfb(node_); 50 | // subscribe place callback 51 | auto sub = node_->create_subscription( 52 | "/recognize_pick/place", rclcpp::QoS(rclcpp::KeepLast(0)), place_callback); 53 | // create client 54 | auto client = node_->create_client("plan_grasps"); 55 | // wait for service 56 | while (!client->wait_for_service(5s)) { 57 | RCLCPP_INFO(node_->get_logger(), "Wait for service"); 58 | } 59 | RCLCPP_INFO(node_->get_logger(), "Got service"); 60 | 61 | while(rclcpp::ok()) 62 | { 63 | if (place_pose_ == nullptr) { 64 | RCLCPP_INFO(node_->get_logger(), "Wait for place mission"); 65 | rclcpp::spin_some(node_); 66 | rclcpp::sleep_for(std::chrono::seconds(2)); 67 | continue; 68 | } 69 | moveit_msgs::msg::PlaceLocation::SharedPtr place = place_pose_; 70 | RCLCPP_INFO(node_->get_logger(), "Place %s", place->id.c_str()); 71 | // get grasp poses 72 | auto request = std::make_shared(); 73 | request->target.id = place->id; 74 | 75 | auto result_future = client->async_send_request(request); 76 | RCLCPP_INFO(node_->get_logger(), "Request sent"); 77 | // wait for response 78 | if (rclcpp::spin_until_future_complete(node_, result_future) != 79 | rclcpp::executor::FutureReturnCode::SUCCESS) 80 | { 81 | continue; 82 | } 83 | // get response 84 | if (moveit_msgs::msg::MoveItErrorCodes::SUCCESS == result_future.get()->error_code.val) { 85 | result_ = result_future.get(); 86 | RCLCPP_INFO(node_->get_logger(), "Response received %d", result_->error_code.val); 87 | } else continue; 88 | 89 | geometry_msgs::msg::PoseStamped p = result_->grasps[0].grasp_pose; 90 | // publish grasp pose 91 | tf2::Quaternion q(p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w); 92 | double roll, pitch, yaw; 93 | tf2::Matrix3x3 r; 94 | r.setRotation(q); 95 | r.getRPY(roll, pitch, yaw); 96 | RCLCPP_INFO(node_->get_logger(), "**********pick pose [position %f %f %f, quat %f %f %f %f, RPY %f %f %f]", 97 | p.pose.position.x, p.pose.position.y, p.pose.position.z, 98 | p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w, 99 | roll, pitch, yaw); 100 | geometry_msgs::msg::TransformStamped tf_msg; 101 | tf_msg.header = p.header; 102 | tf_msg.child_frame_id = "grasp_pose"; 103 | tf_msg.transform.translation.x = p.pose.position.x; 104 | tf_msg.transform.translation.y = p.pose.position.y; 105 | tf_msg.transform.translation.z = p.pose.position.z; 106 | tf_msg.transform.rotation = p.pose.orientation; 107 | tfb.sendTransform(tf_msg); 108 | 109 | #ifdef robot_enable 110 | // pick 111 | robot_->moveToJointValues(joint_values_pick, vel_, acc_); 112 | robot_->pick(p, vel_, acc_, vscale_, appr_); 113 | // place 114 | robot_->moveToJointValues(joint_values_place, vel_, acc_); 115 | robot_->place(place_[0], place_[1], place_[2], place_[3], place_[4], place_[5], vel_, acc_, vscale_, appr_); 116 | #endif 117 | rclcpp::spin_some(node_); 118 | place_pose_ = nullptr; 119 | } 120 | 121 | rclcpp::shutdown(); 122 | return 0; 123 | 124 | } 125 | -------------------------------------------------------------------------------- /grasp_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(grasp_msgs) 4 | 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | find_package(builtin_interfaces REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | 19 | set(msg_files 20 | "msg/GraspConfig.msg" 21 | "msg/GraspConfigList.msg" 22 | "msg/SamplesMsg.msg" 23 | ) 24 | rosidl_generate_interfaces(${PROJECT_NAME} 25 | ${msg_files} 26 | DEPENDENCIES builtin_interfaces std_msgs geometry_msgs 27 | ADD_LINTER_TESTS 28 | ) 29 | 30 | ament_export_dependencies(rosidl_default_runtime) 31 | 32 | ament_package() 33 | -------------------------------------------------------------------------------- /grasp_msgs/msg/CloudIndexed.msg: -------------------------------------------------------------------------------- 1 | # This message holds a point cloud and a list of indices into the point cloud 2 | # at which to sample grasp candidates. 3 | 4 | # The point cloud. 5 | gpd/CloudSources cloud_sources 6 | 7 | # The indices into the point cloud at which to sample grasp candidates. 8 | std_msgs/Int64[] indices 9 | -------------------------------------------------------------------------------- /grasp_msgs/msg/CloudSamples.msg: -------------------------------------------------------------------------------- 1 | # This message holds a point cloud and a list of samples at which the grasp 2 | # detector should search for grasp candidates. 3 | 4 | # The point cloud. 5 | gpd/CloudSources cloud_sources 6 | 7 | # The samples, as (x,y,z) points, at which to search for grasp candidates. 8 | geometry_msgs/Point[] samples 9 | -------------------------------------------------------------------------------- /grasp_msgs/msg/CloudSources.msg: -------------------------------------------------------------------------------- 1 | # This message holds a point cloud that can be a combination of point clouds 2 | # from different camera sources (at least one). For each point in the cloud, 3 | # this message also stores the index of the camera that produced the point. 4 | 5 | # The point cloud. 6 | sensor_msgs/PointCloud2 cloud 7 | 8 | # For each point in the cloud, the index of the camera that acquired the point. 9 | std_msgs/Int64[] camera_source 10 | 11 | # A list of camera positions at which the point cloud was acquired. 12 | geometry_msgs/Point[] view_points -------------------------------------------------------------------------------- /grasp_msgs/msg/GraspConfig.msg: -------------------------------------------------------------------------------- 1 | # This message describes a 2-finger grasp configuration by its 6-DOF pose, 2 | # consisting of a 3-DOF position and 3-DOF orientation, and the opening 3 | # width of the robot hand. 4 | 5 | # Position 6 | geometry_msgs/Point bottom # centered bottom/base of the robot hand) 7 | geometry_msgs/Point top # centered top/fingertip of the robot hand) 8 | geometry_msgs/Point surface # centered position on object surface 9 | 10 | # Orientation represented as three axis (R = [approach binormal axis]) 11 | geometry_msgs/Vector3 approach # The grasp approach direction 12 | geometry_msgs/Vector3 binormal # The binormal 13 | geometry_msgs/Vector3 axis # The hand axis 14 | 15 | geometry_msgs/Point sample # Point at which the grasp was found 16 | 17 | std_msgs/Float32 width # Required aperture (opening width) of the robot hand 18 | 19 | std_msgs/Float32 score # Score assigned to the grasp by the classifier 20 | -------------------------------------------------------------------------------- /grasp_msgs/msg/GraspConfigList.msg: -------------------------------------------------------------------------------- 1 | # This message stores a list of grasp configurations. 2 | 3 | # The time of acquisition, and the coordinate frame ID. 4 | std_msgs/Header header 5 | 6 | # The list of grasp configurations. 7 | grasp_msgs/GraspConfig[] grasps 8 | 9 | # Name of the known object these grasps associated to. 10 | string object_name 11 | -------------------------------------------------------------------------------- /grasp_msgs/msg/SamplesMsg.msg: -------------------------------------------------------------------------------- 1 | # This message describes a set of point samples at which to detect grasps. 2 | 3 | # Header 4 | std_msgs/Header header 5 | 6 | # The samples, as (x,y,z) points, at which to search for grasp candidates. 7 | geometry_msgs/Point[] samples 8 | -------------------------------------------------------------------------------- /grasp_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | grasp_msgs 5 | 0.5.0 6 | ROS2 messages definitions for grasp library 7 | Sharron LIU 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | builtin_interfaces 13 | std_msgs 14 | geometry_msgs 15 | 16 | rosidl_default_runtime 17 | builtin_interfaces 18 | std_msgs 19 | geometry_msgs 20 | 21 | ament_lint_common 22 | 23 | rosidl_interface_packages 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /grasp_ros2/README.md: -------------------------------------------------------------------------------- 1 | ../grasp_tutorials/doc/grasp_planner.rst -------------------------------------------------------------------------------- /grasp_ros2/cfg/grasp_ros2_params.yaml: -------------------------------------------------------------------------------- 1 | GraspDetectorGPD: 2 | ros__parameters: 3 | cloud_topic: /camera/pointcloud 4 | #cloud_topic: /mechmind/color_point_cloud 5 | rviz: true 6 | device: 0 # 0:CPU, 1:GPU, 2:VPU 7 | auto_mode: true 8 | plane_remove: true 9 | # grasp workspace in camera frames 10 | workspace: [-0.21, 0.29, -0.22, 0.15, 0.0, 1.0] # Realsense 11 | #workspace: [-0.16, 0.34, -0.26, 0.14, 1.4, 1.8] # Mechmind 12 | # gripper geometry parameters in metre 13 | # finger_width: the finger thickness 14 | # hand_outer_diameter: the maximum robot hand aperture 15 | # hand_depth: the hand depth (the finger length) 16 | # hand_height: the finger breadth 17 | finger_width: 0.005 18 | hand_outer_diameter: 0.100 19 | hand_depth: 0.038 20 | hand_height: 0.020 21 | GraspPlanner: 22 | ros__parameters: 23 | grasp_score_threshold: 20 24 | grasp_frame_id: "camera_color_optical_frame" # Realsense 25 | #grasp_frame_id: "mechmind_camera" # Mechmind 26 | grasp_offset: [0.000, 0.000, 0.0] 27 | eef_offset: 0.174 28 | eef_yaw_offset: 0.7854 # M_PI/4 29 | finger_joint_names: ["panda_finger_joint1", "panda_finger_joint2"] 30 | -------------------------------------------------------------------------------- /grasp_ros2/cfg/random_pick.yaml: -------------------------------------------------------------------------------- 1 | GraspDetectorGPD: 2 | ros__parameters: 3 | #cloud_topic: /camera/pointcloud 4 | cloud_topic: /mechmind/color_point_cloud 5 | rviz: true 6 | device: 0 # 0:CPU, 1:GPU, 2:VPU 7 | auto_mode: true 8 | plane_remove: true 9 | # grasp workspace in camera frames 10 | #workspace: [-0.21, 0.29, -0.22, 0.15, 0.0, 1.0] # Realsense 11 | workspace: [-0.16, 0.28, -0.26, 0.14, 1.4, 1.65] # Mechmind 12 | # gripper geometry parameters in metre 13 | # finger_width: the finger thickness 14 | # hand_outer_diameter: the maximum robot hand aperture 15 | # hand_depth: the hand depth (the finger length) 16 | # hand_height: the finger breadth 17 | finger_width: 0.005 18 | hand_outer_diameter: 0.100 19 | hand_depth: 0.038 20 | hand_height: 0.020 21 | num_samples: 200 22 | GraspPlanner: 23 | ros__parameters: 24 | grasp_score_threshold: 0 25 | grasp_frame_id: "base" 26 | grasp_approach: [0.0, 0.0, -1.0] 27 | grasp_approach_angle: 0.523 # 1.047=PI/3 # 0.785=PI/4 # 0.523=PI/6 # 0.345=PI/9 # acceptable approaching angle 28 | grasp_offset: [0.004, 0.000, 0.0] 29 | # grasp boundry in grasp_frame_id 30 | grasp_boundry: [-0.2, 0.2, -0.65, -0.30, -0.15, 0.15] 31 | eef_offset: 0.162 32 | eef_yaw_offset: -0.7854 # M_PI/4 33 | finger_joint_names: ["panda_finger_joint1", "panda_finger_joint2"] 34 | -------------------------------------------------------------------------------- /grasp_ros2/cfg/recognize_pick.yaml: -------------------------------------------------------------------------------- 1 | GraspDetectorGPD: 2 | ros__parameters: 3 | cloud_topic: /camera/pointcloud 4 | # cloud_topic: "/camera/aligned_depth_to_color/color/points" 5 | object_topic: "/ros2_openvino_toolkit/segmented_obejcts" 6 | rviz: true 7 | device: 1 # 0:CPU, 1:GPU, 2:VPU 8 | auto_mode: false 9 | plane_remove: true 10 | object_detect: true 11 | # grasp workspace in camera frames 12 | workspace: [-0.23, 0.23, -0.33, 0.05, 0.0, 1.0] 13 | # gripper geometry parameters in metre 14 | # finger_width: the finger thickness 15 | # hand_outer_diameter: the maximum robot hand aperture 16 | # hand_depth: the hand depth (the finger length) 17 | # hand_height: the finger breadth 18 | finger_width: 0.005 19 | hand_outer_diameter: 0.100 20 | hand_depth: 0.038 21 | hand_height: 0.020 22 | GraspPlanner: 23 | ros__parameters: 24 | grasp_score_threshold: 1 25 | grasp_frame_id: "base" 26 | grasp_approach: [0.0, 0.0, -1.0] # expect approaching in -z axis 27 | grasp_approach_angle: 0.7 # 1.047=PI/3 # 0.785=PI/4 # 0.523=PI/6 # 0.345=PI/9 # acceptable approaching angle 28 | grasp_offset: [0.006, -0.003, 0.000] 29 | # grasp boundry in grasp_frame_id 30 | grasp_boundry: [-0.2, 0.2, -0.65, -0.30, -0.15, 0.15] 31 | eef_offset: 0.154 32 | eef_yaw_offset: 0.7854 # M_PI/4 33 | finger_joint_names: ["panda_finger_joint1", "panda_finger_joint2"] 34 | -------------------------------------------------------------------------------- /grasp_ros2/cfg/test_grasp_ros2.yaml: -------------------------------------------------------------------------------- 1 | GraspDetectorGPD: 2 | ros__parameters: 3 | cloud_topic: /camera/pointcloud 4 | rviz: false 5 | device: 0 6 | auto_mode: false 7 | GraspPlanner: 8 | ros__parameters: 9 | grasp_score_threshold: 0 10 | grasp_frame_id: "camera_color_optical_frame" 11 | -------------------------------------------------------------------------------- /grasp_ros2/include/grasp_library/ros2/consts.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 GRASP_LIBRARY__ROS2__CONSTS_HPP_ 16 | #define GRASP_LIBRARY__ROS2__CONSTS_HPP_ 17 | 18 | #include 19 | 20 | namespace grasp_ros2 21 | { 22 | 23 | /** Consts class 24 | * 25 | * \brief A class contains global constatnts definition for grasp library. 26 | * 27 | */ 28 | class Consts 29 | { 30 | public: 31 | /** Topic name of "PointCloud2" message published by an RGBD sensor.*/ 32 | static const char kTopicPointCloud2[]; 33 | /** Topic name of "detected objects" message published by Object Detector.*/ 34 | static const char kTopicDetectedObjects[]; 35 | /** Topic name of "detected grasps" message published by this Grasp Detector.*/ 36 | static const char kTopicDetectedGrasps[]; 37 | /** Topic name of "rviz grasps" message published by this Grasp Detector.*/ 38 | static const char kTopicVisualGrasps[]; 39 | /** Topic name of "tabletop pointcloud" message published by this Grasp Detector.*/ 40 | static const char kTopicTabletop[]; 41 | }; 42 | 43 | } // namespace grasp_ros2 44 | 45 | #endif // GRASP_LIBRARY__ROS2__CONSTS_HPP_ 46 | -------------------------------------------------------------------------------- /grasp_ros2/include/grasp_library/ros2/grasp_detector_base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 GRASP_LIBRARY__ROS2__GRASP_DETECTOR_BASE_HPP_ 16 | #define GRASP_LIBRARY__ROS2__GRASP_DETECTOR_BASE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace grasp_ros2 22 | { 23 | 24 | /** GraspCallback class 25 | * 26 | * \brief Abstract base class for grasp callback. 27 | * 28 | * A grasp planner inherits from this class get called back for grasp detection resutls. 29 | */ 30 | class GraspCallback 31 | { 32 | public: 33 | /** 34 | * \brief Callback for grasp detection results. 35 | * 36 | * \param msg Pointer to grasp detection results. 37 | */ 38 | virtual void grasp_callback(const grasp_msgs::msg::GraspConfigList::SharedPtr msg) = 0; 39 | }; 40 | 41 | /** GraspDetectorBase class 42 | * 43 | * \brief A base class for detecting grasp poses from visual input. 44 | * 45 | * This class defines uniform interface for grasp library, regardless whichever algorithm 46 | * is used for grasp detection. 47 | */ 48 | class GraspDetectorBase 49 | { 50 | public: 51 | /** 52 | * \brief Constructor. 53 | */ 54 | GraspDetectorBase() 55 | : object_name_(""), grasp_cb_(nullptr) 56 | { 57 | } 58 | 59 | /** 60 | * \brief Destructor. 61 | */ 62 | ~GraspDetectorBase() 63 | { 64 | } 65 | 66 | /** 67 | * \brief Start grasp detection. 68 | * When this function is called, GraspDetector starts processing visual input. 69 | * \param name Name of the object for which to detect grasps 70 | */ 71 | void start(std::string name = "") 72 | { 73 | started_ = true; 74 | object_name_ = name; 75 | } 76 | 77 | /** 78 | * \brief Stop grasp detection. 79 | * When this function is called, GraspDetector stops processing visual input. 80 | */ 81 | void stop() 82 | { 83 | started_ = false; 84 | } 85 | 86 | /** 87 | * \brief Register grasp callback function. 88 | * 89 | * \param cb Callback function to be registered. 90 | */ 91 | void add_callback(GraspCallback * cb) 92 | { 93 | grasp_cb_ = cb; 94 | } 95 | 96 | protected: 97 | bool started_ = false; 98 | std::string object_name_; 99 | GraspCallback * grasp_cb_; 100 | }; 101 | 102 | } // namespace grasp_ros2 103 | 104 | #endif // GRASP_LIBRARY__ROS2__GRASP_DETECTOR_BASE_HPP_ 105 | -------------------------------------------------------------------------------- /grasp_ros2/include/grasp_library/ros2/ros_params.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation. All Rights Reserved 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 GRASP_LIBRARY__ROS2__ROS_PARAMS_HPP_ 16 | #define GRASP_LIBRARY__ROS2__ROS_PARAMS_HPP_ 17 | 18 | // ROS2 core 19 | #include 20 | 21 | // ROS2 projects 22 | #include 23 | #include "grasp_library/ros2/grasp_planner.hpp" 24 | 25 | namespace grasp_ros2 26 | { 27 | 28 | /** ROSParameters class 29 | * 30 | * \brief A class to bridge parameters passed from ROS. 31 | * 32 | */ 33 | class ROSParameters 34 | { 35 | public: 36 | static void getDetectionParams( 37 | rclcpp::Node * node, 38 | GraspDetector::GraspDetectionParameters & param); 39 | static void getPlanningParams(rclcpp::Node * Node, GraspPlanner::GraspPlanningParameters & param); 40 | }; 41 | 42 | } // namespace grasp_ros2 43 | 44 | #endif // GRASP_LIBRARY__ROS2__ROS_PARAMS_HPP_ 45 | -------------------------------------------------------------------------------- /grasp_ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | grasp_ros2 5 | 0.5.0 6 | ROS2 grasp library as MoveIt plug-in 7 | Sharron LIU 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | builtin_interfaces 13 | rclcpp 14 | rclcpp_components 15 | std_msgs 16 | sensor_msgs 17 | grasp_msgs 18 | moveit_msgs 19 | people_msgs 20 | visualization_msgs 21 | pcl_conversions 22 | tf2 23 | tf2_ros 24 | tf2_geometry_msgs 25 | trajectory_msgs 26 | 27 | rosidl_default_runtime 28 | builtin_interfaces 29 | rclcpp 30 | rclcpp_components 31 | std_msgs 32 | sensor_msgs 33 | grasp_msgs 34 | moveit_msgs 35 | people_msgs 36 | visualization_msgs 37 | pcl_conversions 38 | tf2 39 | tf2_ros 40 | tf2_geometry_msgs 41 | trajectory_msgs 42 | 43 | ament_lint_auto 44 | ament_lint_common 45 | 46 | 47 | ament_cmake 48 | 49 | 50 | -------------------------------------------------------------------------------- /grasp_ros2/src/consts.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | #include "grasp_library/ros2/consts.hpp" 16 | 17 | namespace grasp_ros2 18 | { 19 | 20 | const char Consts::kTopicPointCloud2[] = "/camera/pointcloud"; 21 | const char Consts::kTopicDetectedObjects[] = "/ros2_openvino_toolkit/segmented_obejcts"; 22 | const char Consts::kTopicDetectedGrasps[] = "/grasp_library/clustered_grasps"; 23 | const char Consts::kTopicVisualGrasps[] = "/grasp_library/grasps_rviz"; 24 | const char Consts::kTopicTabletop[] = "/grasp_library/tabletop_points"; 25 | 26 | } // namespace grasp_ros2 27 | -------------------------------------------------------------------------------- /grasp_ros2/src/grasp_composition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel 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 | #include 16 | 17 | #include 18 | 19 | #include "grasp_library/ros2/grasp_detector_gpd.hpp" 20 | #include "grasp_library/ros2/grasp_planner.hpp" 21 | 22 | using GraspDetectorGPD = grasp_ros2::GraspDetectorGPD; 23 | using GraspDetectorBase = grasp_ros2::GraspDetectorBase; 24 | using GraspPlanner = grasp_ros2::GraspPlanner; 25 | 26 | int main(int argc, char ** argv) 27 | { 28 | rclcpp::init(argc, argv); 29 | rclcpp::executors::MultiThreadedExecutor exec; 30 | 31 | auto detect_node = std::make_shared( 32 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 33 | exec.add_node(detect_node); 34 | GraspDetectorBase * grasp_detector = dynamic_cast(detect_node.get()); 35 | auto plan_node = std::make_shared( 36 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true), grasp_detector); 37 | exec.add_node(plan_node); 38 | exec.spin(); 39 | 40 | detect_node = nullptr; 41 | plan_node = nullptr; 42 | rclcpp::shutdown(); 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /grasp_ros2/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake REQUIRED) 2 | find_package(ament_cmake_gtest REQUIRED) 3 | find_package(rclcpp REQUIRED) 4 | find_package(grasp_msgs REQUIRED) 5 | find_package(moveit_msgs REQUIRED) 6 | find_package(pcl_conversions REQUIRED) 7 | set(TEST_NAME tgrasp_ros2) 8 | 9 | ament_add_gtest(${TEST_NAME} tgrasp_ros2.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/consts.cpp) 10 | 11 | if(TARGET ${TEST_NAME}) 12 | get_filename_component(RESOURCE_DIR "resource" ABSOLUTE) 13 | configure_file(tgrasp_ros2.h.in tgrasp_ros2.h) 14 | include_directories(${CMAKE_CURRENT_BINARY_DIR} ${PCL_INCLUDE_DIRS}) 15 | link_directories(${PCL_LIBRARY_DIRS}) 16 | target_include_directories(${TEST_NAME} PUBLIC 17 | ${grasp_library_INCLUDE_DIRS} 18 | ) 19 | ament_target_dependencies(${TEST_NAME} 20 | pcl_conversions 21 | rclcpp 22 | grasp_msgs 23 | moveit_msgs 24 | sensor_msgs 25 | ) 26 | target_link_libraries(${TEST_NAME} ${GTEST_LIBRARIES} ${PCL_LIBRARIES}) 27 | 28 | # Install binaries 29 | install(TARGETS ${TEST_NAME} 30 | RUNTIME DESTINATION bin 31 | ) 32 | 33 | install(TARGETS ${TEST_NAME} 34 | DESTINATION lib/${PROJECT_NAME} 35 | ) 36 | 37 | endif() 38 | -------------------------------------------------------------------------------- /grasp_ros2/tests/tgrasp_ros2.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel 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 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "grasp_library/ros2/consts.hpp" 31 | #include "./tgrasp_ros2.h" 32 | 33 | using Consts = grasp_ros2::Consts; 34 | using GraspPlanning = moveit_msgs::srv::GraspPlanning; 35 | 36 | static bool received_topic = false; 37 | static int num_grasps = 0; 38 | static bool pcd_stop = false; 39 | static rclcpp::Node::SharedPtr node = nullptr; 40 | static std::shared_ptr result = nullptr; 41 | 42 | static void pcd_publisher() 43 | { 44 | char path[512]; 45 | snprintf(path, sizeof(path), "%s/table_top.pcd", RESOURCE_DIR); 46 | pcl::PointCloud cloud; 47 | if (0 != pcl::io::loadPCDFile(path, cloud)) { 48 | return; 49 | } 50 | sensor_msgs::msg::PointCloud2 msg; 51 | pcl::toROSMsg(cloud, msg); 52 | msg.header.frame_id = "camera_color_optical_frame"; 53 | auto pcd_node = rclcpp::Node::make_shared("PCDPublisher"); 54 | auto pcd_pub = pcd_node->create_publisher( 55 | Consts::kTopicPointCloud2, 10); 56 | rclcpp::Rate loop_rate(30); 57 | while (!pcd_stop && rclcpp::ok()) { 58 | pcd_pub->publish(msg); 59 | loop_rate.sleep(); 60 | } 61 | } 62 | 63 | static void topic_cb(const grasp_msgs::msg::GraspConfigList::SharedPtr msg) 64 | { 65 | RCLCPP_INFO(node->get_logger(), "Topic received"); 66 | received_topic = true; 67 | num_grasps = msg->grasps.size(); 68 | } 69 | 70 | TEST(GraspLibraryTests, TestGraspService) { 71 | EXPECT_TRUE(result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); 72 | EXPECT_GT(result->grasps.size(), uint32_t(0)); 73 | } 74 | 75 | TEST(GraspLibraryTests, TestGraspTopic) { 76 | rclcpp::Rate(1).sleep(); 77 | EXPECT_TRUE(received_topic); 78 | EXPECT_GT(num_grasps, 0); 79 | } 80 | 81 | int main(int argc, char * argv[]) 82 | { 83 | rclcpp::init(argc, argv); 84 | 85 | std::thread pcd_thread(pcd_publisher); 86 | pcd_thread.detach(); 87 | 88 | node = rclcpp::Node::make_shared("GraspLibraryTest"); 89 | auto sub = node->create_subscription( 90 | Consts::kTopicDetectedGrasps, rclcpp::QoS(rclcpp::KeepLast(1)), topic_cb); 91 | 92 | auto client = node->create_client("plan_grasps"); 93 | while (!client->wait_for_service(std::chrono::seconds(1))) { 94 | if (!rclcpp::ok()) { 95 | RCLCPP_ERROR(node->get_logger(), "Client interrupted"); 96 | return 1; 97 | } 98 | RCLCPP_INFO(node->get_logger(), "Wait for service"); 99 | } 100 | auto request = std::make_shared(); 101 | auto result_future = client->async_send_request(request); 102 | RCLCPP_INFO(node->get_logger(), "Request sent"); 103 | 104 | if (rclcpp::spin_until_future_complete(node, result_future) != 105 | rclcpp::executor::FutureReturnCode::SUCCESS) 106 | { 107 | RCLCPP_ERROR(node->get_logger(), "Request failed"); 108 | return 1; 109 | } 110 | result = result_future.get(); 111 | RCLCPP_INFO(node->get_logger(), "Response received %d", result->error_code.val); 112 | 113 | testing::InitGoogleTest(&argc, argv); 114 | int ret = RUN_ALL_TESTS(); 115 | 116 | pcd_stop = true; 117 | rclcpp::Rate(3).sleep(); 118 | // pcd_thread.join() disabled. It causes runtest exit abnormally 119 | 120 | node = nullptr; 121 | rclcpp::shutdown(); 122 | return ret; 123 | } 124 | -------------------------------------------------------------------------------- /grasp_ros2/tests/tgrasp_ros2.h.in: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation. All Rights Reserved 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 GRASP_LIBRARY__TGRASP_LIBRARY_H_ 16 | #define GRASP_LIBRARY__TGRASP_LIBRARY_H_ 17 | 18 | #include 19 | #include 20 | 21 | #define RESOURCE_DIR "@RESOURCE_DIR@" 22 | 23 | #endif // GRASP_LIBRARY__TGRASP_LIBRARY_H_ 24 | -------------------------------------------------------------------------------- /grasp_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel 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 | cmake_minimum_required(VERSION 3.5) 16 | project(grasp_tutorials) 17 | 18 | find_package(ament_cmake REQUIRED) 19 | ament_package() 20 | -------------------------------------------------------------------------------- /grasp_tutorials/README.md: -------------------------------------------------------------------------------- 1 | # ROS2 Grasp Library Tutorials 2 | 3 | This tutorials aim to introduce how to 4 | * Install, build, and launch the ROS2 Grasp Planner and Detector 5 | * Use launch options to customize in a new workspace 6 | * Bring up the intelligent visual grasp solution on a new robot 7 | * Do hand-eye calibration for a new camera setup 8 | * Launch the example applications 9 | 10 | ## Build and test this tutorial 11 | 12 | ```bash 13 | cd ros2_grasp_library/grasp_tutorials 14 | sphinx-build . build # check the outputs in the ./build/ folder 15 | cd ros2_grasp_library/grasp_utils/robot_interface 16 | doxygen Doxyfile # check the outputs in the ./build/ folder 17 | ``` 18 | -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/draw_x_v_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/draw_x_v_1.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/ilc_platform.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/ilc_platform.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/moveit_app_zoo_global_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/moveit_app_zoo_global_view.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/pick_place.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/pick_place.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/random_pick.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/random_pick.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/random_pick_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/random_pick_rviz.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/recognize_pick.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/recognize_pick.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/ros2_grasp_library.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/ros2_grasp_library.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/table.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/table.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/ur5_setup_verify_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/ur5_setup_verify_gazebo.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/ur5_setup_verify_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/ur5_setup_verify_rviz.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/workflow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/workflow.png -------------------------------------------------------------------------------- /grasp_tutorials/_static/images/workflow.vsdx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/_static/images/workflow.vsdx -------------------------------------------------------------------------------- /grasp_tutorials/doc/bringup_robot.rst: -------------------------------------------------------------------------------- 1 | Bring up a New Robot 2 | ==================== 3 | 4 | This tutorial explains what is expected to do when bringing up this ROS2 Grasp Library on a new robot. 5 | 6 | .. _GraspPlanning: http://docs.ros.org/api/moveit_msgs/html/srv/GraspPlanning.html 7 | .. _GPD: https://github.com/atenpas/gpd 8 | .. _OpenVINO™: https://software.intel.com/en-us/openvino-toolkit 9 | .. _Grasp: http://docs.ros.org/api/moveit_msgs/html/msg/Grasp.html 10 | 11 | Minimum APIs to Implement 12 | ------------------------- 13 | 14 | - `moveToTcpPose` 15 | 16 | - Move the TCP (tool center point, usually the end effector of the robot arm, not the hand) to a pose specified with position [x, y, z] and orientation [alpha, betta, gamma] (also called [roll, pitch, yaw]). This function returns when the robot `moved` to the specified pose. 17 | 18 | - `moveToJointValues` 19 | 20 | - Move each joint of the robot to the specified values (usually angles). This function differs from the `moveToTcpPose` since a same TCP pose may be reached with various solutions of joint values. This function is used when the application expect the joints of the robot in specific state, that is proper to performe any successive picking or place action. This function returns when the robot `moved` to the specified joint values. 21 | 22 | - `open` 23 | 24 | - Open the gripper. 25 | 26 | - `close` 27 | 28 | - Close the gripper. 29 | 30 | - `startLoop` 31 | 32 | - Start a loop to read and publish the robot state. Robot states are subsribed by Rviz for visualization. 33 | 34 | Optional implementation and possible extentions 35 | ----------------------------------------------- 36 | 37 | - Optionally you may implement the `pick` and `place` interface to customize the pick and placed pipeline, or even plug-in the collision avoidance motion planning. 38 | 39 | - Python extention is not supported. It's possible to implement the Robot Interface in python and bind to C++. 40 | 41 | 42 | Refer to `Robot Interface API <../api/html/index.html>`_ for more detailed definition. 43 | 44 | Example UR5 Implementation 45 | -------------------------- 46 | 47 | Refer to the UR5 `example `_ implementatino for Robot Interface. 48 | 49 | Test Your Implementation 50 | ------------------------ 51 | 52 | It's important to test your implementation before integrating this part with other components in ROS2 Grasp Library. 53 | 54 | Refer to `UR5 tests `_, adapt it to your robot tests. 55 | 56 | Bring up Robot Control Applications 57 | ----------------------------------- 58 | 59 | Once finished the testing, you may start to bring up the `Draw X `_ app or the `fixed position pick and place `_ app on your new robot. These application does not require camera, instead they control the robot only. 60 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/draw_x.rst: -------------------------------------------------------------------------------- 1 | Draw X 2 | ====== 3 | 4 | Overview 5 | -------------- 6 | 7 | This demo shows how to use the robot interface to draw letter ``X`` 8 | at the fixed positions with an UR5 robot arm. 9 | 10 | Requirement 11 | ------------ 12 | 13 | Before running the code, make sure you have 14 | followed the instructions below to setup the robot correctly. 15 | 16 | - Hardware 17 | 18 | - Host running ROS2 19 | 20 | - `UR5`_ 21 | 22 | - Software 23 | 24 | - `ROS2 Dashing`_ Desktop 25 | 26 | - `robot_interface`_ 27 | 28 | .. _UR5: https://www.universal-robots.com/products/ur5-robot 29 | 30 | .. _ROS2 Dashing: https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/ 31 | 32 | .. _robot_interface: https://github.com/intel/ros2_grasp_library/tree/master/grasp_utils/robot_interface 33 | 34 | Download and Build the Example Code 35 | ------------------------------------ 36 | 37 | Within your ROS2 workspace, download and compile the example code: 38 | 39 | :: 40 | 41 | cd /src 42 | 43 | git clone https://github.com/intel/ros2_grasp_library.git 44 | 45 | cd .. 46 | 47 | colcon build --base-paths src/ros2_grasp_library/grasp_apps/draw_x 48 | 49 | Launch the Application 50 | ---------------------- 51 | 52 | - Launch the application 53 | 54 | :: 55 | 56 | ros2 launch draw_x draw_x.launch.py 57 | 58 | .. note:: Please make sure the emergency button on the teach pendant is in your hand, 59 | in case there is any accident. 60 | 61 | - Expected Outputs: 62 | 63 | 1. The robot moves its arm to the home pose 64 | 2. The robot moves its arm to the pose above the first corner of X 65 | 3. The robot moves its arm down to the first corner of X 66 | 4. The robot moves its arm to the second corner of X 67 | 5. The robot moves its arm up to the pose above the second corner of X 68 | 6. The robot moves its arm to the pose above the third corner of X 69 | 7. The robot moves its arm down to the third corner of X 70 | 8. The robot moves its arm to the fourth corner of X 71 | 9. The robot moves its arm up to the pose above the fourth corner of X 72 | 10. The robot moves its arm to the home pose again 73 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/fixed_position_pick.rst: -------------------------------------------------------------------------------- 1 | Fixed Position Pick 2 | ==================== 3 | 4 | Overview 5 | -------------- 6 | 7 | This demo shows how to use the robot interface to pick and place a 8 | object at a predefined location with an UR5 robot arm. 9 | 10 | Requirement 11 | ------------ 12 | 13 | Before running the code, make sure you have 14 | followed the instructions below to setup the robot correctly. 15 | 16 | - Hardware 17 | 18 | - Host running ROS2 19 | 20 | - `UR5`_ 21 | 22 | - `Robot Gripper`_ 23 | 24 | - Software 25 | 26 | - `ROS2 Dashing`_ Desktop 27 | 28 | - `robot_interface`_ 29 | 30 | .. _UR5: https://www.universal-robots.com/products/ur5-robot 31 | 32 | .. _ROS2 Dashing: https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/ 33 | 34 | .. _robot_interface: https://github.com/intel/ros2_grasp_library/tree/master/grasp_utils/robot_interface 35 | 36 | .. _Robot Gripper: https://www.universal-robots.com/plus/end-effectors/hitbot-electric-gripper 37 | 38 | Download and Build the Example Code 39 | ------------------------------------ 40 | 41 | Within your ROS2 workspace, download and compile the example code: 42 | 43 | :: 44 | 45 | cd /src 46 | 47 | git clone https://github.com/intel/ros2_grasp_library.git 48 | 49 | cd .. 50 | 51 | colcon build --base-paths src/ros2_grasp_library/grasp_apps/fixed_position_pick 52 | 53 | Launch the Application 54 | ---------------------- 55 | 56 | - Launch the application 57 | 58 | :: 59 | 60 | ros2 launch fixed_position_pick fixed_position_pick.launch.py 61 | 62 | .. note:: Please make sure the emergency button on the teach pendant is in your hand, 63 | in case there is any accident. 64 | 65 | - Expected Outputs: 66 | 67 | 1. The robot moves to the home pose 68 | 2. The robot picks up an object from the predefined location 69 | 3. The robot places the object to another location 70 | 4. The robot moves back to the home pose 71 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/getting_start.rst: -------------------------------------------------------------------------------- 1 | Getting Start 2 | ============= 3 | 4 | This tutorial introduces getting start to use this ROS2 Grasp Library. 5 | 6 | .. _GraspPlanning: http://docs.ros.org/api/moveit_msgs/html/srv/GraspPlanning.html 7 | .. _GPD: https://github.com/atenpas/gpd 8 | .. _OpenVINO™: https://software.intel.com/en-us/openvino-toolkit 9 | .. _Grasp: http://docs.ros.org/api/moveit_msgs/html/msg/Grasp.html 10 | 11 | ROS2 Grasp Planner and Detector 12 | ------------------------------- 13 | 14 | In this section, you will start with an RGBD sensor connecting to a Ubuntu host machine. 15 | 16 | The grasp detection relys on OpenVINO™ toolkit. Follow this `grasp_planner `_ instruction to install the toolkit, then build and install the ROS2 Grasp Planner and Detector with your camera. 17 | 18 | After launch the grasp planner, from rviz you will see grasp detection results highlighted as blue markers. 19 | 20 | .. image:: ./grasp_ros2/img/ros2_grasp_library.png 21 | :width: 643 px 22 | :height: 435 px 23 | :align: center 24 | 25 | Use Launch Options for Customization 26 | ------------------------------------ 27 | 28 | ROS2 parameters are supported to customize the Grasp Detector and Grasp Planner for your local workspace. For example, the topic name of point cloud from RGBD sensor, the camera workspace (in the frame_id of the point cloud image), the grasp approach direction and angle, the grasp boundary (in the frame_id of the robot base). 29 | 30 | Robot Interface 31 | --------------- 32 | 33 | In this section, you will bring up your robot by implementing the Robot Interface. Currently the robot interface is defined in C++, python vesion is still working in progress. 34 | 35 | Robot Interface are the minimum APIs a robot should provide to enable this solution. Follow this `robot_interface `_ insturction to implement the required `move`, `open`, `close`, `startLoop` interfaces. 36 | 37 | Then make sure your implementation passed the Robot Interface tests, to garantee later integration with the example applications. Also you may try the "Robot Control Applications" (like Draw X, fixed position pick and place) to verify your implemntation working well. 38 | 39 | Hand-eye Calibration 40 | -------------------- 41 | 42 | Now start to generate transformation between the camera and the robot. Follow this `handeye_calibration `_ insturtion to finish the procedure of hand-eye calibration. The calibration procedure need to be done at the time when camera is setup. The resulting transformation will be remembered in your local environment for later publishing when launching the applications. 43 | 44 | Launch Intelligent Visual Grasp Applications 45 | -------------------------------------------- 46 | 47 | To this step, you may start to launch the applications. 48 | 49 | `Random Picking `_ runs OpenVINO grasp detection on GPU, and sends request to ROS2 MoveIt Grasp Planner for grasp planning and detection. The most likely successful grasps are returned by the Grasp Pose Detection from CNN inference, taking 3D point cloud inputs from the camera. The picking order is not pre-defined, so called random picking. 50 | 51 | `Recognition Picking `_ runs OpenVINO grasp detection on GPU, and runs OpenVINO object segmentation on CPU or Movidius VPU. The masks of recognized objects are returned from the `mask_rcnn` model. The `place_publisher` publishing the name of the object to pick and the position to place, so called recognition picking. 52 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_api.rst: -------------------------------------------------------------------------------- 1 | ROS2 Grasp Library APIs 2 | ======================= 3 | 4 | .. _GraspPlanning: http://docs.ros.org/api/moveit_msgs/html/srv/GraspPlanning.html 5 | .. _GPD: https://github.com/atenpas/gpd 6 | .. _OpenVINO™: https://software.intel.com/en-us/openvino-toolkit 7 | .. _Grasp: http://docs.ros.org/api/moveit_msgs/html/msg/Grasp.html 8 | .. _PointCloud2: https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg 9 | .. _ObjectsInMasks: https://github.com/intel/ros2_openvino_toolkit/blob/master/people_msgs/msg/ObjectsInMasks.msg 10 | .. _Image: https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Image.msg 11 | .. _TransformStamped: https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/TransformStamped.msg 12 | 13 | Grasp Planning ROS2 Interfaces 14 | ------------------------------ 15 | 16 | - Subscribed Topics 17 | 18 | - PointCloud2 topic from RGBD sensor (sensor_msgs::msg::`PointCloud2`_) 19 | 20 | - Segmented object topic (people_msgs::msg::`ObjectsInMasks`_) 21 | 22 | - Delivered Services 23 | 24 | - plan_grasps (moveit_msgs::srv::`GraspPlanning`_) 25 | 26 | Hand-Eye Calibration ROS2 Interfaces 27 | ------------------------------------ 28 | 29 | - Subscribed Topics 30 | 31 | - RGB image from sensor (sensor_msgs::msg::`Image`_) 32 | 33 | - Broadcasted Transforms 34 | 35 | - Static transform btw camera and robot (geometry_msgs::msg::`TransformStamped`_) 36 | 37 | Robot Interface API 38 | ------------------- 39 | 40 | - `API <../api/html/index.html>`_ 41 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_planner.rst: -------------------------------------------------------------------------------- 1 | Grasp Planner 2 | ============= 3 | 4 | Tutorials 5 | --------- 6 | 7 | - `Install OpenVINO™ toolkit`_ 8 | 9 | .. _Install OpenVINO™ toolkit: https://github.com/intel/ros2_grasp_library/tree/master/grasp_tutorials/doc/grasp_ros2/install_openvino.md 10 | 11 | - `Launch ROS2 Grasp Planner and Detector`_ 12 | 13 | .. _Launch ROS2 Grasp Planner and Detector: https://github.com/intel/ros2_grasp_library/tree/master/grasp_tutorials/doc/grasp_ros2/tutorials_1_grasp_ros2_with_camera.md 14 | 15 | - `Launch tests`_ 16 | 17 | .. _Launch tests: https://github.com/intel/ros2_grasp_library/tree/master/grasp_tutorials/doc/grasp_ros2/tutorials_2_grasp_ros2_test.md 18 | 19 | - `Use launch options`_ 20 | 21 | .. _Use launch options: https://github.com/intel/ros2_grasp_library/tree/master/grasp_tutorials/doc/grasp_ros2/tutorials_3_grasp_ros2_launch_options.md 22 | 23 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_ros2/img/ros2_grasp_library.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_tutorials/doc/grasp_ros2/img/ros2_grasp_library.png -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_ros2/install_gpd.md: -------------------------------------------------------------------------------- 1 | Installation guide for Grasp Pose Detection 2 | 3 | ### Install [GPG](https://github.com/atenpas/gpg) 4 | 1. Get the code 5 | ```bash 6 | git clone https://github.com/atenpas/gpg.git 7 | cd gpg 8 | ``` 9 | 2. Build the library 10 | ```bash 11 | mkdir build && cd build 12 | cmake .. 13 | make 14 | sudo make install 15 | # by default, "libgrasp_candidates_generator.so" shall be installed to "/usr/local/lib" 16 | ``` 17 | 18 | ### Install [GPD](https://github.com/sharronliu/gpd) 19 | 1. Get the code, originally derived from [GPD](https://github.com/atenpas/gpd) tag 1.5.0 20 | ```bash 21 | git clone https://github.com/sharronliu/gpd.git 22 | git checkout libgpd 23 | cd gpd/src/gpd 24 | ``` 25 | 2. Build the library 26 | ```bash 27 | mkdir build && cd build 28 | cmake -DUSE_OPENVINO=ON .. 29 | make 30 | sudo make install 31 | # by default, "libgrasp_pose_detection.so" shall be installed to "/usr/local/lib" 32 | # and header files installed to "/usr/local/include/gpd" 33 | ``` 34 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_ros2/install_openvino.md: -------------------------------------------------------------------------------- 1 | # Intel® DLDT toolkit and Intel® OpenVINO™ toolkit 2 | 3 | This tutorial introduces the DLDT toolkit and OpenVINO toolkit. 4 | 5 | Intel® [DLDT](https://github.com/opencv/dldt) is a Deep Learning Deployment Toolkit common to all architectures. The toolkit allows developers to convert pre-trained deep learning models into optimized Intermediate Representation (IR) models, then deploy the IR models through a high-level C++ Inference Engine API integrated with application logic. Additionally, [Open Model Zoo](https://github.com/opencv/open_model_zoo) provides more than 100 pre-trained optimized deep learning models and a set of demos to expedite development of high-performance deep learning inference applications. Online tutorials are availble for 6 | * [Inference Engine Build Instructions](https://github.com/opencv/dldt/blob/2019/inference-engine/README.md) 7 | 8 | Intel® [OpenVINO™](https://software.intel.com/en-us/openvino-toolkit) (Open Visual Inference & Neural Network Optimization) toolkit enables CNN-based deep learning inference at the edge computation, extends workloads across Intel® hardware (including accelerators) and maximizes performance. The toolkit supports heterogeneous execution across various compution vision devices -- CPU, GPU, Intel® Movidius™ NCS, and FPGA -- using a common API. Online tutorials are available for 9 | * [Model Optimize Developer Guide](https://software.intel.com/en-us/articles/OpenVINO-ModelOptimizer) 10 | * [Inference Engine Developer Guide](https://software.intel.com/en-us/articles/OpenVINO-InferEngine) 11 | * [Intel® Neural Compute Stick 2](https://software.intel.com/en-us/neural-compute-stick/get-started) 12 | 13 | 14 | ## Install DLDT and OpenVINO 15 | It's recommended to refer to the online documents of the toolkits for the latest installation instruction. Below is detailed steps we verified with Ubuntu 18.04 on Intel NUC6i7KYK for your ref. 16 | 1. Build and install Inference Engine 17 | ```bash 18 | git clone https://github.com/opencv/dldt.git 19 | git checkout 2019_R3 20 | # follow the instructions below to install all dependents, including mklml, opencl, etc. 21 | # https://github.com/opencv/dldt/blob/2019_R3/inference-engine/README.md#build-on-linux-systems 22 | # build 23 | mkdir build && cd build 24 | cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local -DGEMM=MKL -DMKLROOT=/usr/local/lib/mklml -DENABLE_MKL_DNN=ON -DENABLE_CLDNN=ON .. 25 | make -j8 26 | sudo make install 27 | ``` 28 | 2. Share the CMake configures for Inference Engine to be found by other packages 29 | ```bash 30 | sudo mkdir /usr/share/InferenceEngine 31 | sudo cp InferenceEngineConfig*.cmake /usr/share/InferenceEngine 32 | sudo cp targets.cmake /usr/share/InferenceEngine 33 | ``` 34 | Then Inference Engine will be found when adding "find_package(InferenceEngine)" into the CMakeLists.txt 35 | 3. Configure library path for dynamic loading 36 | ```bash 37 | echo `pwd`/../bin/intel64/Release/lib | sudo tee -a /etc/ld.so.conf.d/openvino.conf 38 | sudo ldconfig 39 | ``` 40 | 4. Optionally install plug-ins for InferenceEngine deployment on heterogeneous devices 41 | * Install [plug-in](https://github.com/opencv/dldt/blob/2019_R3/inference-engine/README.md#optional-additional-installation-steps-for-the-intel-movidius-neural-compute-stick-and-neural-compute-stick-2) for deployment on Intel Movidius Neural Computation Sticks Myriad X. 42 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_ros2/tutorials_1_grasp_ros2_with_camera.md: -------------------------------------------------------------------------------- 1 | # OpenVINO Grasp Library with RGBD Camera 2 | 3 | This tutorial introduce the OpenVINO environment setup, how to build and launch Grasp Library with RGBD camera. 4 | 5 | ## Requirements 6 | ### Hardware 7 | * Host running ROS2/ROS 8 | * RGBD sensor 9 | ### Software 10 | We verified the software with Ubuntu 18.04 Bionic and ROS2 Dashing release. Verification with ROS2 MoveIt is still work in progress. Before this, we have verified the grasp detection with MoveIt Melodic branch (tag 0.10.8) and our visual pick & place application to be shared as [MoveIt Example Apps](https://github.com/ros-planning/moveit_example_apps). 11 | * Install ROS2 packages 12 | [ros-dashing-desktop](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians) 13 | 14 | * Install non ROS packages 15 | ```bash 16 | sudo apt-get install libpcl-dev libeigen3-dev 17 | ``` 18 | 19 | * Install [Intel OpenVINO Toolkit](install_openvino.md) 20 | * Install [GPD](install_gpd.md) 21 | 22 | ## Build Grasp Library 23 | ```bash 24 | # get the source codes 25 | mkdir ~/ros2_ws/src -p 26 | cd ~/ros2_ws/src 27 | git clone https://github.com/intel/ros2_grasp_library.git 28 | # copy GPD models 29 | cp -a /models ros2_grasp_library/gpd 30 | # build 31 | cd .. 32 | source /opt/ros/dashing/setup.bash 33 | colcon build --symlink-install --packages-select grasp_msgs moveit_msgs grasp_ros2 34 | source ./install/local_setup.bash 35 | ``` 36 | 37 | ## Launch Grasp Library 38 | ```bash 39 | # Terminal 1, Optionally, launch Rviz2 to illustrate detection results. 40 | ros2 run rviz2 rviz2 -d src/ros2_grasp_library/grasp_ros2/rviz2/grasp.rviz 41 | # Note You may customize the ".rviz" file for your own camera, for example: 42 | # To change to fixed frame: "Global Options -> Fixed Frame" 43 | # To change the point cloud topic: "Point Cloud 2 -> Topic" 44 | 45 | # Terminal 2, launch RGBD camera 46 | # e.g. launch [ROS2 Realsenes](https://github.com/intel/ros2_intel_realsense/tree/refactor) 47 | # or, with a ros-bridge, launch any ROS OpenNI RGBD cameras, like [ROS Realsense](https://github.com/intel-ros/realsense) 48 | ros2 run realsense_node realsense_node 49 | 50 | # Terminal 3, launch Grasp Library 51 | ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/grasp_ros2_params.yaml 52 | ``` 53 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/grasp_ros2/tutorials_2_grasp_ros2_test.md: -------------------------------------------------------------------------------- 1 | # Grasp Library Tests and Exampels 2 | 3 | This tutorial documents Grasp Library tests which also serve as example codes for the usage of Grasp Library. 4 | 5 | ## Grasp Library Tests 6 | Test Suites enabled: 7 | * ROS2 built-in tests for static code scanning like, copyright tests, cppcheck tests, cpplint tests, lint_cmake tests, uncrustify tests, xmllint tests. 8 | * Grasp ROS2 basic functional tests: tgrasp_ros2, basic tests cover ROS2 topic and ROS2 service of Grasp Library. 9 | 10 | Before test, make sure you have setup the environment to build the Grasp Library, following tutorials [Grasp Library with RGBD Camera](tutorials_1_grasp_library_with_camera.md). The tests take inputs from a pre-stored PointCloud file (.pcd). Thus it's unnecessary to launch an RGBD camera. 11 | ```bash 12 | # Terminal 1, launch Grasp Library 13 | ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/test_grasp_ros2.yaml 14 | # Terminal 2, run tests 15 | colcon test --packages-select grasp_msgs grasp_ros2 16 | ``` 17 | For failed cases check detailed logs at "log/latest_test/grasp_ros2/stdout.log". 18 | 19 | ## Grasp Library Examples 20 | The [grasp test codes](../grasp_ros2/tests/tgrasp_ros2.cpp) also demonstrate how to use this Grasp Library for grasp detection and grasp planning. 21 | 22 | ### Grasp Detection Example (Non-MoveIt App) 23 | This example creats ROS2 subscription to the "Detected Grasps" topic and get the detection results from callback. Grasp Library is expected to work in 'auto_mode=true', sensor-driven grasp detection, see example launch options [here](../grasp_ros2/cfg/grasp_ros2_params.yaml). 24 | 25 | ```bash 26 | #include 27 | #include 28 | #include "grasp_ros2/consts.hpp" 29 | 30 | static rclcpp::Node::SharedPtr node = nullptr; 31 | 32 | static void topic_cb(const grasp_msgs::msg::GraspConfigList::SharedPtr msg) 33 | { 34 | RCLCPP_INFO(node->get_logger(), "Grasp Callback Received"); 35 | } 36 | 37 | int main(int argc, char * argv[]) 38 | { 39 | // init ROS2 40 | rclcpp::init(argc, argv); 41 | // create ROS2 node 42 | node = rclcpp::Node::make_shared("GraspDetectionExample"); 43 | // subscribe to the "Detected Grasps" topic 44 | auto sub = node->create_subscription 45 | (Consts::kTopicDetectedGrasps, rclcpp::QoS(rclcpp::KeepLast(1)), topic_cb); 46 | // create ROS2 executor to process any pending in/out messages 47 | rclcpp::spin(node); 48 | 49 | node = nullptr; 50 | rclcpp::shutdown(); 51 | return 0; 52 | } 53 | ``` 54 | 55 | ### Grasp Planning Example (MoveIt App) 56 | This example creates ROS2 client for the "plan_grasps" service and get the palnning results from async service response. Grasp Library is expected to work in 'auto_mode=false', service-driven grasp detection, see launch option example [here](../grasp_ros2/cfg/test_grasp_ros2.yaml). 57 | 58 | ```bash 59 | #include 60 | #include 61 | #include 62 | #include "grasp_ros2/consts.hpp" 63 | 64 | static rclcpp::Node::SharedPtr node = nullptr; 65 | static std::shared_ptr result = nullptr; 66 | 67 | int main(int argc, char * argv[]) 68 | { 69 | // init ROS2 70 | rclcpp::init(argc, argv); 71 | // create ROS2 node 72 | node = rclcpp::Node::make_shared("GraspPlanningExample"); 73 | // create ROS2 client for MoveIt "plan_grasps" service 74 | auto client = node->create_client("plan_grasps"); 75 | // wait for ROS2 service ready 76 | while (!client->wait_for_service(std::chrono::seconds(1))) { 77 | if (!rclcpp::ok()) { 78 | RCLCPP_ERROR(node->get_logger(), "Client interrupted"); 79 | return 1; 80 | } 81 | RCLCPP_INFO(node->get_logger(), "Wait for service"); 82 | } 83 | // fill in a request 84 | auto request = std::make_shared(); 85 | // send async request 86 | auto result_future = client->async_send_request(request); 87 | RCLCPP_INFO(node->get_logger(), "Request sent"); 88 | // wait for response 89 | if (rclcpp::spin_until_future_complete(node, result_future) != 90 | rclcpp::executor::FutureReturnCode::SUCCESS) 91 | { 92 | RCLCPP_ERROR(node->get_logger(), "Request failed"); 93 | return 1; 94 | } 95 | // get grasp planning results from response 96 | result = result_future.get(); 97 | RCLCPP_INFO(node->get_logger(), "Response received %d", result->error_code.val); 98 | 99 | node = nullptr; 100 | rclcpp::shutdown(); 101 | return 0; 102 | } 103 | ``` 104 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/handeye_calibration.rst: -------------------------------------------------------------------------------- 1 | Hand-eye Calibration 2 | ===================== 3 | 4 | Hand-eye calibration is used to get the camera pose with respect to the robot. 5 | 6 | - `handeye_target_detection `_ 7 | 8 | - `handeye_dashboard `_ 9 | 10 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/overview.rst: -------------------------------------------------------------------------------- 1 | Overview 2 | ======== 3 | 4 | .. _GraspPlanning: http://docs.ros.org/api/moveit_msgs/html/srv/GraspPlanning.html 5 | .. _GPD: https://github.com/atenpas/gpd 6 | .. _OpenVINO™: https://software.intel.com/en-us/openvino-toolkit 7 | .. _Grasp: http://docs.ros.org/api/moveit_msgs/html/msg/Grasp.html 8 | 9 | ROS2 Grasp Library consists of 10 | 11 | .. image:: ../_static/images/ros2_grasp_library.png 12 | :width: 523 px 13 | :height: 311 px 14 | :align: center 15 | 16 | - A ROS2 Grasp Planner providing grasp planning service, as an extensible capability of MoveIt (moveit_msgs::srv::`GraspPlanning`_), translating grasp detection results into MoveIt Interfaces (moveit_msgs::msg::`Grasp`_). A ROS2 Grasp Detctor abstracting interfaces for grasp detection results 17 | 18 | - A ROS2 hand-eye calibration module generating transformation from camera frame to robot frame 19 | 20 | - Robot interfaces controlling the phsical robot to move, pick, place, as well as to feedback robot states 21 | 22 | - ROS2 example applications demonstrating how to use this ROS2 Grasp Library in advanced industrial usages for intelligent visual grasp 23 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/random_pick.rst: -------------------------------------------------------------------------------- 1 | Random Pick (OpenVINO Grasp Detection) 2 | ====================================== 3 | 4 | Overview 5 | -------- 6 | 7 | A simple application demonstrating how to pick up objects from clutter scenarios with an industrial robot arm. 8 | The application interact with Grasp Planner and Robot Interface from this Grasp Library. 9 | 10 | The Grasp Planner takes grasp detection results from `OpenVINO GPD `_, 11 | transforms the grasp pose from camera view 12 | to the robot view with the `Hand-Eye Calibration`_, 13 | translates the Grasp Pose into `moveit_msgs Grasp `_. 14 | 15 | The Robot Interface takes the grasp poses and place poses, to pick and place the object. 16 | 17 | Watch this `demo_video `_ to see the output of this application. 18 | 19 | .. raw:: html 20 | 21 | 22 | 23 | 24 | Requirement 25 | ----------- 26 | 27 | Before running the code, make sure you have followed the instructions below 28 | to setup the environment. 29 | 30 | - Hardware 31 | 32 | - Host running ROS2 33 | 34 | - RGBD sensor 35 | 36 | - `Robot Arm `_ 37 | 38 | - `Robot Gripper`_ 39 | 40 | - Software 41 | 42 | - `ROS2 `_ 43 | 44 | - `Grasp Planner `_ 45 | 46 | - `Robot Interface `_ 47 | 48 | - `Hand-Eye Calibration `_ 49 | 50 | - RGBD Sensor 51 | 52 | - `realsense `_ 53 | 54 | Download and Build the Application 55 | ---------------------------------- 56 | 57 | Within your catkin workspace, download and compile the example code 58 | 59 | :: 60 | 61 | cd /src 62 | 63 | git clone https://github.com/intel/ros2_grasp_library.git 64 | 65 | cd .. 66 | 67 | colcon build --symlink-install 68 | 69 | - Build Options 70 | 71 | - BUILD_RANDOM_PICK (**ON** | OFF) 72 | Switch on/off building of this application 73 | 74 | 75 | Launch the Application with Real Robot and Camera 76 | ------------------------------------------------- 77 | 78 | - Publish handeye transform, refer to `Hand-Eye Calibration`_ 79 | 80 | - Launch UR description 81 | 82 | :: 83 | 84 | ros2 launch ur_description view_ur5_ros2.launch.py 85 | 86 | #load rviz2 configure file "src/ros2_grasp_library/grasp_apps/random_pick/rviz2/random_pick.rviz" 87 | 88 | - Launch RGBD sensor 89 | 90 | :: 91 | 92 | ros2 run realsense_node realsense_node 93 | 94 | - Launch random pick app 95 | 96 | :: 97 | 98 | ros2 run random_pick random_pick 99 | 100 | - Launch grasp planner 101 | 102 | :: 103 | 104 | ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/random_pick.yaml 105 | 106 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/recognize_pick.rst: -------------------------------------------------------------------------------- 1 | Recognize Pick (OpenVINO Grasp Detection + OpenVINO Object Segmentation) 2 | ======================================================================== 3 | 4 | Overview 5 | -------- 6 | 7 | A simple application demonstrating how to pick up recognized objects with an industrial robot arm. 8 | The application interact with Grasp Planner and Robot Interface from this Grasp Library. 9 | 10 | Comparing against the `random picking `_ application, this recognition picking takes the place commands published from the `place_publisher` which specifying the name the object to pick and the position to place. 11 | 12 | The Grasp Detector then takes the object segmentation results from the `OpenVINO Mask-rcnn `_ to identify the location of the object in the point cloud image and generates grasp poses for that specific object. 13 | 14 | Watch this `demo_video `_ to see the output of this application. 15 | 16 | .. raw:: html 17 | 18 | 19 | 20 | Requirement 21 | ----------- 22 | 23 | Before running the code, make sure you have followed the instructions below 24 | to setup the environment. 25 | 26 | - Hardware 27 | 28 | - Host running ROS2 29 | 30 | - RGBD sensor 31 | 32 | - `Robot Arm `_ 33 | 34 | - `Robot Gripper`_ 35 | 36 | - Software 37 | 38 | - `ROS2 `_ 39 | 40 | - `Grasp Planner `_ 41 | 42 | - `Robot Interface `_ 43 | 44 | - `Hand-Eye Calibration `_ 45 | 46 | - `ROS2 OpenVINO `_ 47 | 48 | - RGBD Sensor 49 | 50 | - `realsense `_ 51 | 52 | Download and Build the Application 53 | ---------------------------------- 54 | 55 | Within your catkin workspace, download and compile the example code 56 | 57 | :: 58 | 59 | cd /src 60 | 61 | git clone https://github.com/intel/ros2_grasp_library.git 62 | 63 | cd .. 64 | 65 | colcon build --symlink-install 66 | 67 | - Build Options 68 | 69 | - BUILD_RECOGNIZE_PICK (**ON** | OFF) 70 | Switch on/off building of this application 71 | 72 | 73 | Launch the Application with Real Robot and Camera 74 | ------------------------------------------------- 75 | 76 | - Publish handeye transform, refer to `Hand-Eye Calibration`_ 77 | 78 | - Publish place object 79 | 80 | :: 81 | 82 | ros2 run recognize_pick place_publisher sports_ball 83 | 84 | - Launch UR description 85 | 86 | :: 87 | 88 | ros2 launch ur_description view_ur5_ros2.launch.py 89 | 90 | #load rviz2 configure file "src/ros2_grasp_library/grasp_apps/recognize_pick/rviz2/recognize_pick.rviz" 91 | 92 | - Launch RGBD sensor 93 | 94 | :: 95 | 96 | ros2 run realsense_node realsense_node 97 | 98 | - Launch object segmentation 99 | 100 | :: 101 | 102 | ros2 launch dynamic_vino_sample pipeline_segmentation.launch.py 103 | 104 | # close the rviz2 window 105 | 106 | - Launch recognize pick app 107 | 108 | :: 109 | 110 | ros2 run recognize_pick recognize_pick 111 | 112 | - Launch grasp planner 113 | 114 | :: 115 | 116 | ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/recognize_pick.yaml 117 | 118 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/robot_interface.rst: -------------------------------------------------------------------------------- 1 | Robot Interface 2 | =============== 3 | 4 | - `Robot Interface `_ 5 | 6 | Robot Control Apps 7 | ------------------- 8 | .. toctree:: 9 | :maxdepth: 2 10 | 11 | ./draw_x 12 | 13 | ./fixed_position_pick 14 | -------------------------------------------------------------------------------- /grasp_tutorials/doc/template.rst: -------------------------------------------------------------------------------- 1 | [App Tutorial Template] 2 | ======================= 3 | 4 | Overview 5 | -------- 6 | *(Describe what this application is in one topic sentence, 7 | followed by a paragraph telling what this application does in details. E.g.)* 8 | 9 | A template of application tutorial. 10 | The application tutorial contains an overview of the application, 11 | requirements on hardware and software, 12 | guidance to download/build/launch the application, 13 | expected output from the application, and customization notes. 14 | 15 | Requirements 16 | ------------ 17 | *(Describe the hardware and software requred 18 | to setup the environment for this application. 19 | Provide hyperlinkage to the procurement info or installation guides. E.g.)* 20 | 21 | - Hardware 22 | 23 | - Host running ROS2 24 | 25 | - `Robot Arm `_ (optional) 26 | 27 | - Software 28 | 29 | - `ROS2 Dashing `_ Desktop 30 | 31 | - `robot_interface`_ 32 | 33 | .. _robot_interface: https://github.com/intel/ros2_grasp_library/tree/master/grasp_utils/robot_interface 34 | 35 | Download and Build the Application 36 | ---------------------------------- 37 | *(Describe how to download and build the application. 38 | List build options specific to this application. E.g.)* 39 | 40 | :: 41 | 42 | cd /src 43 | 44 | git clone https://github.com/intel/ros2_grasp_library.git 45 | 46 | cd .. 47 | 48 | colcon build --symlink-install --ament-cmake-args -DBUILD_RANDOM_PICK=ON 49 | 50 | - Build Options 51 | 52 | - BUILD_RANDOM_PICK (ON | **OFF** ) 53 | Switch on/off building of this application 54 | 55 | Launch the Application 56 | ---------------------- 57 | *(Describe how to launch the application. 58 | Provide hyperlinkage to launch robot contollers. 59 | List launch options specific to this application. E.g.)* 60 | 61 | - Launch this application 62 | 63 | :: 64 | 65 | ros2 launch template template 66 | 67 | - Launch Options 68 | 69 | - grasp_xyz (double | **"0.545 0.107 0.15"**) 70 | Specify pick position in the "base" frame 71 | 72 | - place_xyz (double | **"-0.107 -0.545 -0.10"**) 73 | Specify place position in the "base" frame 74 | 75 | Expected Outputs 76 | ---------------- 77 | *(Describe expected outputs from this application. 78 | Illustrate with screen snapshot when necessary. E.g.)* 79 | 80 | You should see Rviz output like this: 81 | 82 | .. image:: ../_static/images/pick_place.png 83 | 84 | Customization Notes 85 | ------------------- 86 | *(List possible customization items. 87 | Guide how to customize the application 88 | on new environment and new robots. E.g.)* 89 | 90 | - **Change the pick position** 91 | Use launch option "grasp_xyz" to change the pick position. 92 | 93 | - **Change the place position** 94 | Use launch option "place_xyz" to change to place position. 95 | -------------------------------------------------------------------------------- /grasp_tutorials/index.rst: -------------------------------------------------------------------------------- 1 | Welcome to ROS2 Grasp Library Tutorials 2 | ======================================= 3 | 4 | ROS2 Grasp Library is a ROS2 intelligent visual grasp solution for advanced industrial usages, with OpenVINO™ grasp detection and MoveIt Grasp Planning. These tutorials aim to help quickly bringup the solution in a new working environment. 5 | 6 | The tutorials introduce how to 7 | 8 | - Install, build, and launch the ROS2 Grasp Planner and Detector 9 | 10 | - Use launch options to customize in a new workspace 11 | 12 | - Bring up the intelligent visual grasp solution on a new robot 13 | 14 | - Do hand-eye calibration for a new camera setup 15 | 16 | - Launch the example applications 17 | 18 | Contents: 19 | --------- 20 | .. toctree:: 21 | :maxdepth: 2 22 | 23 | doc/overview 24 | 25 | doc/getting_start 26 | 27 | doc/grasp_planner 28 | 29 | doc/robot_interface 30 | 31 | doc/bringup_robot 32 | 33 | doc/handeye_calibration 34 | 35 | doc/random_pick 36 | 37 | doc/recognize_pick 38 | 39 | doc/grasp_api 40 | 41 | doc/template 42 | -------------------------------------------------------------------------------- /grasp_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | grasp_tutorials 5 | 0.5.0 6 | Instructions and demo code for developing intelligent manipulation app with this ROS2 Grasp Library 7 | Apache License 2.0 8 | Sharron Liu 9 | Yu Yan 10 | Sharron Liu 11 | Yu Yan 12 | 13 | ament_cmake 14 | rclcpp 15 | rclcpp 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/README.md: -------------------------------------------------------------------------------- 1 | # handeye_dashboard 2 | 3 | ## 1.Prerequisite 4 | 5 | * System install 6 | * Install [ROS2 Dashing](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/) 7 | * Install [handeye](https://github.com/RoboticsYY/handeye) 8 | * Install [criutils](https://github.com/RoboticsYY/criutils) 9 | * Install [baldor](https://github.com/RoboticsYY/baldor) 10 | * Install [handeye_tf_service](https://github.com/intel/ros2_grasp_library/tree/master/grasp_utils/handeye_tf_service) 11 | * Install [handeye_target_detection](https://github.com/intel/ros2_grasp_library/tree/master/grasp_utils/handeye_target_detection) 12 | 13 | ## 2. Build and install 14 | 15 | ```shell 16 | sudo apt install python3-numpy python3-scipy 17 | ``` 18 | 19 | Build with the `ros2_grasp_library` package. Installation instructions refer to [here](https://github.com/intel/ros2_grasp_library/blob/master/grasp_tutorials/doc/grasp_ros2/tutorials_1_grasp_ros2_with_camera.md). 20 | 21 | ## 3.Run 22 | 23 | ### 3.1 Bring up realsense camera 24 | 25 | ```shell 26 | ros2 run realsense_node realsense_node __params:=`ros2 pkg prefix realsense_examples`/share/realsense_examples/config/d435.yaml 27 | ``` 28 | 29 | > Note: other cameras can be used, only need to check that the image topic and camera info topic are published 30 | 31 | ### 3.2 Bring up calibration board detection 32 | 33 | ```shell 34 | ros2 launch handeye_target_detection pose_estimation.launch.py 35 | ``` 36 | 37 | For detailed information, please refer to the package [handeye_target_detection](https://github.com/intel/ros2_grasp_library/tree/master/grasp_utils/handeye_target_detection) 38 | 39 | If runing successfully, you should see something similar to: 40 | 41 | 42 | 43 | The detection result is displayed on the left panel of the Rviz2. 44 | 45 | ### 3.3 Bring up UR5 robot 46 | 47 | ```shell 48 | # Terminal 1 (robot frames tf update) 49 | ros2 launch robot_interface ur_test.launch.py move:=false 50 | 51 | # Terminal 2 (robot state display in Rviz2) 52 | ros2 launch ur_description view_ur5_ros2.launch.py 53 | ``` 54 | 55 | The realtime robot state is displayed: 56 | 57 | 58 | 59 | > Note: any robot can be used, only ensure that the robot ROS2 driver is publishing the joint states and link TFs at rate of at least 125Hz 60 | 61 | ### 3.4 Bring up calibration dashboard 62 | 63 | ```shell 64 | ros2 launch handeye_dashboard handeye_dashboard.launch.py 65 | ``` 66 | 67 | If running successfully, a rqt dashboard similar to the below photo should show up: 68 | 69 | 70 | On the panel of the dashboard, user can input the names of `Camera-Frame`, `Object-Frame`, `Robot-Base-Frame` and `End-Effector-Frame`. The calibration will lookup the TF transforms: 71 | 72 | * From `Camera-Frame` to `Object-Frame` 73 | * From `Robot-Base-Frame` to `End-Effector-Frame` 74 | 75 | The calibration process is controlled by the four buttons on the left panel of the dashboard: 76 | 77 | * Step 0: Select camera mount type, `attached on robot` or `fixed beside robot`. 78 | * Step 1: Use the first button to take snapshots of the two transforms. 79 | * Step 2: After enough samples are taken, use the second button to save the snapshots and make the AX=XB calculation. 80 | * Step 3: Use the fourth button to publish the static TF transform between `Camera-Frame` and `Robot-Base-Frame`. Please check TF or point cloud on Rviz2 to make sure the camera pose published. 81 | 82 | > Note: Be careful with the third button, it is used to clear the snapshots and the calculation result. 83 | 84 | ### 3.5 Publish calibration result 85 | 86 | Please check the result at `/tmp/camera-robot.txt`: 87 | 88 | ```yaml 89 | camera-robot pose: 90 | Translation: [-0.032727495589941216, -0.09304065368400717, 0.0003508296697299189] 91 | Rotation: in Quaternion [0.9997471812284859, 0.01090594636560865, -0.009141740972837598, -0.0174086912647742] 92 | ``` 93 | 94 | The result can be published without launching the whole GUI on command line: 95 | 96 | ```shell 97 | #ros2 run tf2_ros static_transform_publisher 98 | # NOTE the quaternion stored in "camera_robot.txt" is 99 | 100 | ros2 run tf2_ros static_transform_publisher -0.032727495589941216, -0.09304065368400717, 0.0003508296697299189 0.01090594636560865, -0.009141740972837598, -0.0174086912647742, 0.9997471812284859 base camera_link 101 | ``` 102 | 103 | ## 4.Result 104 | 105 | A result with the camera mounted on the robot end-effector looks like this: 106 | 107 | 108 | 109 | A video of the calibration process can be found at: 110 | [handeye_calibration_demo](https://videoportal.intel.com/media/Industrial-robot-hand-eye-calibration/0_8ddlp0p1) 111 | 112 | ###### *Any security issue should be reported using process at https://01.org/security* 113 | -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/data/camera-robot.json: -------------------------------------------------------------------------------- 1 | [[-0.036301454848076786, 0.9992976851533406, -0.009291976274627967, 0.04561821843056508], [-0.6780505333941169, -0.0314598013057813, -0.7343417154606937, 0.9203069217128195], [-0.7341183000987421, -0.020357243157558547, 0.6787163649943599, 0.17330773243084327], [0.0, 0.0, 0.0, 1.0]] -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/doc/images/handeye_calibration_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/doc/images/handeye_calibration_result.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/doc/images/handeye_dashboard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/doc/images/handeye_dashboard.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/doc/images/handeye_target_detection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/doc/images/handeye_target_detection.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/doc/images/robot_state.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/doc/images/robot_state.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/images/Intel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/images/Intel.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/images/UR5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/images/UR5.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/images/capture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/images/capture.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/images/tool-calibration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/images/tool-calibration.png -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/launch/handeye_dashboard.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | def generate_launch_description(): 8 | 9 | # URDF file to be loaded by Robot State Publisher 10 | rqt_config = os.path.join( 11 | get_package_share_directory('handeye_dashboard'), 12 | 'config', 'Default.perspective' 13 | ) 14 | 15 | return LaunchDescription( [ 16 | # Robot State Publisher 17 | Node(package='handeye_tf_service', node_executable='handeye_tf_server', 18 | output='screen'), 19 | 20 | # Rviz2 21 | Node(package='rqt_gui', node_executable='rqt_gui', 22 | output='screen', arguments=['--perspective-file', rqt_config]), 23 | ]) -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | handeye_dashboard 5 | 0.1.0 6 | The handeye_dashboard package 7 | Yu Yan 8 | Apache License 2.0 9 | Yu Yan 10 | 11 | rclpy 12 | rqt_gui 13 | rqt_gui_py 14 | python_qt_binding 15 | tf2 16 | tf2_ros 17 | handeye 18 | 19 | 20 | ament_python 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Dashboard for the Hand-Eye Calibration 5 | 6 | 7 | 8 | 9 | images/Intel.png 10 | Plugins created by Intel OTC-Robotics 11 | 12 | 13 | images/tool-calibration.png 14 | Dashboard for the hand-eye calibration 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/resource/handeye_dashboard: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/resource/handeye_dashboard -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | from setuptools import setup 3 | from setuptools import find_packages 4 | 5 | package_name = 'handeye_dashboard' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.1.0', 10 | packages=find_packages('src', exclude=['test']), 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | ('share/' + package_name, ['plugin.xml']), 16 | ('share/' + package_name + '/images', glob('images/*.png')), 17 | ('share/' + package_name + '/data', glob('data/*.json')), 18 | ('share/' + package_name + '/launch', glob('launch/*.launch.py')), 19 | ('share/' + package_name + '/config', glob('config/*.perspective')), 20 | ], 21 | install_requires=['setuptools'], 22 | zip_safe=True, 23 | author='Yu Yan', 24 | author_email='yu.yan@intel.com', 25 | maintainer='Yu Yan', 26 | maintainer_email='yu.yan@intel.com', 27 | keywords=['ROS'], 28 | classifiers=[ 29 | 'Intended Audience :: Developers', 30 | 'License :: OSI Approved :: BSD 3-Clause License', 31 | 'Programming Language :: Python', 32 | 'Topic :: Software Development', 33 | ], 34 | description=( 35 | 'The handeye_dashboard package.' 36 | ), 37 | license='Apache License 2.0', 38 | tests_require=['pytest'], 39 | package_dir={'':'src'}, 40 | entry_points={ 41 | 'console_scripts': [ 42 | 'handeye_dashboard = handeye_dashboard.main:main', 43 | ], 44 | }, 45 | ) 46 | -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/src/handeye_dashboard/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_dashboard/src/handeye_dashboard/__init__.py -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/src/handeye_dashboard/main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | def main(): 8 | sys.exit(Main().main(sys.argv, standalone='handeye_dashboard.plugin.HandEyeCalibration')) 9 | 10 | if __name__ == '__main__': 11 | main() -------------------------------------------------------------------------------- /grasp_utils/handeye_dashboard/src/handeye_dashboard/plugin.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from .handeye_calibration import HandEyeCalibration 3 | -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 100 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 70 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: 55 | AfterClass: 'true' 56 | AfterControlStatement: 'true' 57 | AfterEnum : 'true' 58 | AfterFunction : 'true' 59 | AfterNamespace : 'true' 60 | AfterStruct : 'true' 61 | AfterUnion : 'true' 62 | BeforeCatch : 'true' 63 | BeforeElse : 'true' 64 | IndentBraces : 'false' 65 | ... -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel 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 | cmake_minimum_required(VERSION 3.5) 16 | project(handeye_target_detection) 17 | 18 | # Default to C99 19 | if(NOT CMAKE_C_STANDARD) 20 | set(CMAKE_C_STANDARD 99) 21 | endif() 22 | 23 | # Default to C++14 24 | if(NOT CMAKE_CXX_STANDARD) 25 | set(CMAKE_CXX_STANDARD 14) 26 | endif() 27 | 28 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 29 | add_compile_options(-Wall -Wextra -Wpedantic) 30 | endif() 31 | 32 | set (WITH_OPENCL OFF) 33 | 34 | # find dependencies 35 | find_package(ament_cmake REQUIRED) 36 | find_package(tf2 REQUIRED) 37 | find_package(tf2_msgs REQUIRED) 38 | find_package(tf2_ros REQUIRED) 39 | find_package(std_msgs REQUIRED) 40 | find_package(cv_bridge REQUIRED) 41 | find_package(sensor_msgs REQUIRED) 42 | find_package(image_transport REQUIRED) 43 | 44 | find_package(OpenCV REQUIRED) 45 | if((${OpenCV_VERSION} LESS 3.3)) 46 | message(WARNING "handeye_target_detection works better with OpenCV version >= 3.3") 47 | endif() 48 | 49 | # Set include directory path 50 | include_directories( 51 | include 52 | ${rclcpp_INCLUDE_DIRS} 53 | ${Boost_INCLUDE_DIRS} 54 | ) 55 | 56 | set(SOURCES_POSEESTIMATION 57 | src/pose_estimation_node.cpp 58 | src/pose_estimator.cpp 59 | ) 60 | 61 | add_executable(pose_estimation ${SOURCES_POSEESTIMATION}) 62 | ament_target_dependencies(pose_estimation rclcpp tf2 tf2_msgs tf2_ros std_msgs sensor_msgs cv_bridge image_transport) 63 | target_link_libraries(pose_estimation 64 | ${rclcpp_LIBRARIES} 65 | ${OpenCV_LIBS} 66 | ) 67 | 68 | # Install target files 69 | install( 70 | TARGETS pose_estimation 71 | DESTINATION lib/${PROJECT_NAME}/ 72 | ) 73 | 74 | # Install header files 75 | install( 76 | DIRECTORY include/ 77 | DESTINATION include 78 | ) 79 | 80 | # Install launch files. 81 | install(DIRECTORY 82 | launch cfg data 83 | DESTINATION share/${PROJECT_NAME}/ 84 | ) 85 | 86 | if(BUILD_TESTING) 87 | find_package(ament_lint_auto REQUIRED) 88 | ament_lint_auto_find_test_dependencies() 89 | endif() 90 | 91 | ament_package() 92 | -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/cfg/handeye.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Image1 10 | Splitter Ratio: 0.5 11 | Tree Height: 265 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | Visualization Manager: 26 | Class: "" 27 | Displays: 28 | - Alpha: 0.5 29 | Cell Size: 1 30 | Class: rviz_default_plugins/Grid 31 | Color: 160; 160; 164 32 | Enabled: true 33 | Line Style: 34 | Line Width: 0.029999999329447746 35 | Value: Lines 36 | Name: Grid 37 | Normal Cell Count: 0 38 | Offset: 39 | X: 0 40 | Y: 0 41 | Z: 0 42 | Plane: XY 43 | Plane Cell Count: 10 44 | Reference Frame: 45 | Value: true 46 | - Class: rviz_default_plugins/Image 47 | Enabled: true 48 | Max Value: 1 49 | Median window: 5 50 | Min Value: 0 51 | Name: Image 52 | Normalize Range: true 53 | Queue Size: 10 54 | Topic: /image/detected 55 | Unreliable: false 56 | Value: true 57 | - Class: rviz_default_plugins/TF 58 | Enabled: true 59 | Frame Timeout: 15 60 | Frames: 61 | All Enabled: true 62 | Marker Scale: 1 63 | Name: TF 64 | Show Arrows: true 65 | Show Axes: true 66 | Show Names: false 67 | Tree: 68 | {} 69 | Update Interval: 0 70 | Value: true 71 | Enabled: true 72 | Global Options: 73 | Background Color: 48; 48; 48 74 | Fixed Frame: camera_color_optical_frame 75 | Frame Rate: 30 76 | Name: root 77 | Tools: 78 | - Class: rviz_default_plugins/MoveCamera 79 | - Class: rviz_default_plugins/Select 80 | - Class: rviz_default_plugins/FocusCamera 81 | - Class: rviz_default_plugins/Measure 82 | Line color: 128; 128; 0 83 | - Class: rviz_default_plugins/SetInitialPose 84 | Topic: /initialpose 85 | - Class: rviz_default_plugins/SetGoal 86 | Topic: /move_base_simple/goal 87 | - Class: rviz_default_plugins/PublishPoint 88 | Single click: true 89 | Topic: /clicked_point 90 | Transformation: 91 | Current: 92 | Class: rviz_default_plugins/TF 93 | Value: true 94 | Views: 95 | Current: 96 | Class: rviz_default_plugins/Orbit 97 | Distance: 2.5901811122894287 98 | Enable Stereo Rendering: 99 | Stereo Eye Separation: 0.05999999865889549 100 | Stereo Focal Distance: 1 101 | Swap Stereo Eyes: false 102 | Value: false 103 | Focal Point: 104 | X: 0 105 | Y: 0 106 | Z: 0 107 | Focal Shape Fixed Size: true 108 | Focal Shape Size: 0.05000000074505806 109 | Invert Z Axis: false 110 | Name: Current View 111 | Near Clip Distance: 0.009999999776482582 112 | Pitch: 0.2152027189731598 113 | Target Frame: 114 | Value: Orbit (rviz) 115 | Yaw: 2.8704030513763428 116 | Saved: ~ 117 | Window Geometry: 118 | Displays: 119 | collapsed: false 120 | Height: 846 121 | Hide Left Dock: false 122 | Hide Right Dock: true 123 | Image: 124 | collapsed: false 125 | QMainWindow State: 000000ff00000000fd000000040000000000000211000002f8fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000144000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000185000001ae0000002800ffffff000000010000010f000002f8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002f8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000299000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 126 | Selection: 127 | collapsed: false 128 | Tool Properties: 129 | collapsed: false 130 | Views: 131 | collapsed: true 132 | Width: 1200 133 | X: 60 134 | Y: 60 135 | -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/detected/aruco/3X4_aruco.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/detected/aruco/3X4_aruco.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/detected/aruco/5X7_aruco.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/detected/aruco/5X7_aruco.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/detected/charuco/charuco.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/detected/charuco/charuco.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/detected/chessboard/chessboard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/detected/chessboard/chessboard.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/detected/circlegrid/3X5_circles_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/detected/circlegrid/3X5_circles_grid.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/detected/circlegrid/4X11_circles_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/detected/circlegrid/4X11_circles_grid.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/pattern/aruco_3X4_DICT_4X4_50.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/pattern/aruco_3X4_DICT_4X4_50.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/pattern/aruco_5X7_DICT_6X6_250.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/pattern/aruco_5X7_DICT_6X6_250.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/pattern/asymmetric_circles_grid_3X5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/pattern/asymmetric_circles_grid_3X5.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/pattern/asymmetric_circles_grid_4X11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/pattern/asymmetric_circles_grid_4X11.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/pattern/charuco_5X7_DICT_6X6_250.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/pattern/charuco_5X7_DICT_6X6_250.jpg -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/data/pattern/chessboard_9X6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/ros2_grasp_library/980b7ddd4348d9aa78c6e84c42da1984746867db/grasp_utils/handeye_target_detection/data/pattern/chessboard_9X6.png -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/include/PoseEstimator.h: -------------------------------------------------------------------------------- 1 | /** Copyright (c) 2019 Intel 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 POSEESTIMATOR_H 17 | #define POSEESTIMATOR_H 18 | 19 | // ROS include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // OpenCV include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | // System include 39 | #include 40 | #include 41 | 42 | namespace tf2 43 | { 44 | void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::msg::TransformStamped& msg, 45 | builtin_interfaces::msg::Time stamp, const std::string& frame_id, 46 | const std::string& child_frame_id); 47 | } 48 | 49 | class PoseEstimator 50 | { 51 | public: 52 | PoseEstimator(std::shared_ptr& node, std::string pattern, std::string image_topic, 53 | std::string camera_info_topic, std::string publish_image_topic, int width, int height, 54 | std::string dictionary, double chessboard_square_size, double circle_grid_seperation, 55 | double aruco_board_marker_size, double aruco_board_marker_seperation, double charuco_board_marker_size, 56 | double charuco_board_square_size); 57 | 58 | ~PoseEstimator() 59 | { 60 | } 61 | 62 | void imageCB_CHESSBOARD(const sensor_msgs::msg::Image::ConstSharedPtr& msg); 63 | void imageCB_ASYMMETRIC_CIRCLES_GRID(const sensor_msgs::msg::Image::ConstSharedPtr& msg); 64 | void imageCB_ARUCO(const sensor_msgs::msg::Image::ConstSharedPtr& msg); 65 | void imageCB_CHARUCO(const sensor_msgs::msg::Image::ConstSharedPtr& msg); 66 | void caminfoCB(const sensor_msgs::msg::CameraInfo::SharedPtr msg); 67 | void draw(cv::Mat img, std::vector corners, cv::Mat imgpts); 68 | void rotationVectorToTF2Quaternion(tf2::Quaternion&, cv::Vec3d&); 69 | 70 | private: 71 | // ROS variables 72 | std::shared_ptr node_; 73 | image_transport::ImageTransport it_; 74 | image_transport::Subscriber image_sub_; 75 | image_transport::Publisher image_pub_; 76 | rclcpp::Subscription::SharedPtr camerainfo_sub_; 77 | tf2_ros::TransformBroadcaster broadcaster_; 78 | 79 | // Native variables 80 | cv::Mat camera_matrix_; 81 | cv::Mat dist_coeffs_; 82 | bool run_; 83 | std::string image_topic_; 84 | std::string camera_info_topic_; 85 | std::string publish_image_topic_; 86 | int width_; 87 | int height_; 88 | double chessboard_square_size_; 89 | double circle_grid_seperation_; 90 | double aruco_board_marker_size_; 91 | double aruco_board_marker_seperation_; 92 | double charuco_board_marker_size_; 93 | double charuco_board_square_size_; 94 | enum Patterns 95 | { 96 | NOT_EXISTING, 97 | CHESSBOARD, 98 | ASYMMETRIC_CIRCLES_GRID, 99 | CHARUCO, 100 | ARUCO 101 | }; 102 | Patterns calibration_pattern_; 103 | std::map pattern_map_; 104 | cv::aruco::PREDEFINED_DICTIONARY_NAME dictionary_; 105 | std::map disctionary_map_; 106 | std::string path_; 107 | }; 108 | 109 | #endif -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/launch/pose_estimation.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | import launch 18 | import launch.actions 19 | import launch.substitutions 20 | import launch_ros.actions 21 | from ament_index_python.packages import get_package_share_directory 22 | 23 | def generate_launch_description(): 24 | 25 | # .yaml file for configuring the parameters 26 | yaml = os.path.join( 27 | get_package_share_directory('handeye_target_detection'), 28 | 'launch', 'pose_estimation.yaml' 29 | ) 30 | 31 | rviz = os.path.join( 32 | get_package_share_directory('handeye_target_detection'), 33 | 'cfg', 'handeye.rviz' 34 | ) 35 | 36 | return launch.LaunchDescription([ 37 | 38 | launch_ros.actions.Node( 39 | package='handeye_target_detection', node_executable='pose_estimation', 40 | output='screen', arguments=['__params:='+yaml]), 41 | 42 | launch_ros.actions.Node( 43 | package='rviz2', node_executable='rviz2', 44 | output='screen', arguments=['-d', rviz]), 45 | ]) -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/launch/pose_estimation.yaml: -------------------------------------------------------------------------------- 1 | pose_estimation: 2 | ros__parameters: 3 | pattern: "ARUCO" 4 | image_topic: "/camera/color/image_raw" 5 | camera_info_topic: "/camera/color/camera_info" 6 | publish_image_topic: "/image/detected" 7 | width: 3 8 | height: 4 9 | dictionary: "DICT_4X4_50" 10 | chessboard_square_size: 0.026 11 | circle_grid_seperation: 0.035 12 | aruco_board_marker_size: 0.0256 13 | aruco_board_marker_seperation: 0.0066 14 | charuco_board_marker_size: 0.022 15 | charuco_board_square_size: 0.03 -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | handeye_target_detection 5 | 0.1.0 6 | Recognize the calibration pattern pose with RGBD camera. 7 | yanyu 8 | yanyu 9 | Apache License 2.0 10 | 11 | ament_cmake 12 | rclcpp 13 | std_msgs 14 | sensor_msgs 15 | cv_bridge 16 | image_transport 17 | rospy 18 | tf 19 | 20 | rclcpp 21 | ament_cmake_clang_format 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /grasp_utils/handeye_target_detection/src/pose_estimation_node.cpp: -------------------------------------------------------------------------------- 1 | /** Copyright (c) 2019 Intel 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 "PoseEstimator.h" 19 | 20 | using namespace std::chrono_literals; 21 | 22 | int main(int argc, char** argv) 23 | { 24 | // Start the ros node 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared( 27 | "pose_estimation", 28 | rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)); 29 | 30 | // Initialize parameter client 31 | auto parameters_client = std::make_shared(node); 32 | while (!parameters_client->wait_for_service(1s)) 33 | { 34 | if (!rclcpp::ok()) 35 | { 36 | RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); 37 | rclcpp::shutdown(); 38 | } 39 | RCLCPP_INFO(node->get_logger(), "service not available, waiting again..."); 40 | } 41 | 42 | // Get parameters 43 | std::string pattern = parameters_client->get_parameter("pattern", "ARUCO"); 44 | std::string image_topic = parameters_client->get_parameter("image_topic", "/camera/color/image_raw"); 45 | std::string camera_info_topic = 46 | parameters_client->get_parameter("camera_info_topic", "/camera/color/camera_info"); 47 | std::string publish_image_topic = 48 | parameters_client->get_parameter("publish_image_topic", "/image/detected"); 49 | int width = parameters_client->get_parameter("width", 5); 50 | int height = parameters_client->get_parameter("height", 7); 51 | std::string dictionary = parameters_client->get_parameter("dictionary", "DICT_4X4_50"); 52 | double chessboard_square_size = parameters_client->get_parameter("chessboard_square_size", 0.026); 53 | double circle_grid_seperation = parameters_client->get_parameter("circle_grid_seperation", 0.035); 54 | double aruco_board_marker_size = parameters_client->get_parameter("aruco_board_marker_size", 0.035); 55 | double aruco_board_marker_seperation = parameters_client->get_parameter("aruco_board_marker_seperation", 0.007); 56 | double charuco_board_marker_size = parameters_client->get_parameter("charuco_board_marker_size", 0.022); 57 | double charuco_board_square_size = parameters_client->get_parameter("charuco_board_square_size", 0.037); 58 | 59 | PoseEstimator pe(node, pattern, image_topic, camera_info_topic, publish_image_topic, width, height, dictionary, 60 | chessboard_square_size, circle_grid_seperation, aruco_board_marker_size, 61 | aruco_board_marker_seperation, charuco_board_marker_size, charuco_board_square_size); 62 | 63 | rclcpp::spin(node); 64 | RCLCPP_INFO(node->get_logger(), "Node calibration_pattern_pose_estimation exited."); 65 | return 0; 66 | } -------------------------------------------------------------------------------- /grasp_utils/handeye_tf_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(handeye_tf_service) 4 | 5 | # Default to C++14 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 14) 8 | endif() 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(rclcpp REQUIRED) 15 | find_package(tf2_ros REQUIRED) 16 | find_package(ament_cmake REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(builtin_interfaces REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | find_package(rosidl_default_generators REQUIRED) 21 | 22 | set(srv_files 23 | "srv/HandeyeTF.srv") 24 | 25 | rosidl_generate_interfaces(${PROJECT_NAME} 26 | ${srv_files} 27 | DEPENDENCIES geometry_msgs builtin_interfaces std_msgs 28 | ) 29 | 30 | ament_export_dependencies(rosidl_default_runtime) 31 | 32 | add_executable(handeye_tf_server 33 | src/handeye_tf_server.cpp 34 | ) 35 | 36 | ament_target_dependencies(handeye_tf_server 37 | rclcpp tf2_ros 38 | ) 39 | 40 | get_default_rmw_implementation(rmw_implementation) 41 | find_package("${rmw_implementation}" REQUIRED) 42 | get_rmw_typesupport(typesupport_impls "${rmw_implementation}" LANGUAGE "cpp") 43 | 44 | foreach(typesupport_impl ${typesupport_impls}) 45 | rosidl_target_interfaces(handeye_tf_server 46 | ${PROJECT_NAME} ${typesupport_impl} 47 | ) 48 | endforeach() 49 | 50 | install(TARGETS handeye_tf_server 51 | DESTINATION lib/${PROJECT_NAME}) 52 | 53 | ament_package() -------------------------------------------------------------------------------- /grasp_utils/handeye_tf_service/README.md: -------------------------------------------------------------------------------- 1 | # handeye_tf_service -------------------------------------------------------------------------------- /grasp_utils/handeye_tf_service/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | handeye_tf_service 5 | 0.1.0 6 | Provide TF get function for handeye. 7 | Yu Yan 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rosidl_default_generators 13 | 14 | rclcpp 15 | geometry_msgs 16 | std_msgs 17 | tf2_ros 18 | builtin_interfaces 19 | 20 | rosidl_default_runtime 21 | 22 | rosidl_interface_packages 23 | 24 | 25 | ament_cmake 26 | 27 | -------------------------------------------------------------------------------- /grasp_utils/handeye_tf_service/src/handeye_tf_server.cpp: -------------------------------------------------------------------------------- 1 | /** Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | #include 21 | #include 22 | #include 23 | 24 | using HandeyeTF = handeye_tf_service::srv::HandeyeTF; 25 | using namespace std::chrono_literals; 26 | 27 | class ServerNode : public rclcpp::Node 28 | { 29 | public: 30 | explicit ServerNode(const rclcpp::NodeOptions & options) 31 | : Node("handeye_tf_server", options), broadcaster_(this) 32 | { 33 | // Init tf message 34 | tf_msg_.header.frame_id = "base"; // Used to void TF_NO_FRAME_ID error, updated by user later 35 | tf_msg_.child_frame_id = "camera_link"; 36 | // Initialize rotation to avoid TF_DENORMALIZED_QUATERNION error 37 | tf_msg_.transform.rotation.x = 0.0; 38 | tf_msg_.transform.rotation.y = 0.0; 39 | tf_msg_.transform.rotation.z = 0.0; 40 | tf_msg_.transform.rotation.w = 1.0; 41 | 42 | // Init timer 43 | timer_ = this->create_wall_timer( 44 | 100ms, std::bind(&ServerNode::timer_callback, this)); 45 | 46 | // Init tf listener 47 | clock_ = this->get_clock(); 48 | rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); 49 | tf_buffer_ = std::make_shared(clock_); 50 | tf_listener_ = std::make_shared(*tf_buffer_); 51 | 52 | // Service handler 53 | auto handle_service = 54 | [this](const std::shared_ptr request_header, 55 | const std::shared_ptr request, 56 | std::shared_ptr response) -> void 57 | { 58 | if (request->publish.data) // Publish the camera-robot transform 59 | { 60 | RCLCPP_INFO(this->get_logger(), "Incoming publish request\nframe_id: %s child_frame_id: %s", 61 | request->transform.header.frame_id.data(), request->transform.child_frame_id.data()); 62 | tf_msg_ = request->transform; 63 | } 64 | else // Lookup the requested transform 65 | { 66 | (void)request_header; 67 | RCLCPP_INFO(this->get_logger(), "Incoming lookup request\nframe_id: %s child_frame_id: %s", 68 | request->transform.header.frame_id.data(), request->transform.child_frame_id.data()); 69 | 70 | try 71 | { 72 | response->tf_lookup_result = tf_buffer_->lookupTransform(request->transform.header.frame_id, 73 | request->transform.child_frame_id, tf2::TimePoint()); 74 | } 75 | catch (tf2::TransformException &ex) 76 | { 77 | std::string temp = ex.what(); 78 | RCLCPP_WARN(this->get_logger(), "%s", temp.c_str()); 79 | } 80 | } 81 | }; 82 | 83 | // Create a service that will use the callback function to handle requests. 84 | srv_ = create_service("handeye_tf_service", handle_service); 85 | RCLCPP_INFO(this->get_logger(), "Handeye TF service created."); 86 | } 87 | 88 | private: 89 | void timer_callback() 90 | { 91 | broadcaster_.sendTransform(tf_msg_); 92 | } 93 | 94 | // Handeye service 95 | rclcpp::Service::SharedPtr srv_; 96 | 97 | // Variables used for looking up tf transforms 98 | std::shared_ptr tf_buffer_; 99 | std::shared_ptr tf_listener_; 100 | 101 | // Timer used for static transform publish 102 | rclcpp::TimerBase::SharedPtr timer_; 103 | // TF message for camera w.r.t robot transform 104 | geometry_msgs::msg::TransformStamped tf_msg_; 105 | // TF broadcaster 106 | tf2_ros::StaticTransformBroadcaster broadcaster_; 107 | rclcpp::Clock::SharedPtr clock_; 108 | }; 109 | 110 | int main(int argc, char ** argv) 111 | { 112 | rclcpp::init(argc, argv); 113 | auto node = std::make_shared(rclcpp::NodeOptions()); 114 | rclcpp::spin(node); 115 | rclcpp::shutdown(); 116 | return 0; 117 | } -------------------------------------------------------------------------------- /grasp_utils/handeye_tf_service/srv/HandeyeTF.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/TransformStamped transform 2 | std_msgs/Bool publish 3 | --- 4 | geometry_msgs/TransformStamped tf_lookup_result -------------------------------------------------------------------------------- /grasp_utils/robot_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel 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 | cmake_minimum_required(VERSION 3.5) 16 | project(robot_interface) 17 | 18 | # Default to C99 19 | if(NOT CMAKE_C_STANDARD) 20 | set(CMAKE_C_STANDARD 99) 21 | endif() 22 | 23 | # Default to C++14 24 | if(NOT CMAKE_CXX_STANDARD) 25 | set(CMAKE_CXX_STANDARD 14) 26 | endif() 27 | 28 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 29 | add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) 30 | endif() 31 | 32 | # find dependencies 33 | find_package(ament_cmake REQUIRED) 34 | find_package(tf2 REQUIRED) 35 | find_package(tf2_eigen REQUIRED) 36 | find_package(tf2_ros REQUIRED) 37 | find_package(eigen3_cmake_module REQUIRED) 38 | find_package(Eigen3 REQUIRED) 39 | find_package(rclcpp REQUIRED) 40 | find_package(sensor_msgs REQUIRED) 41 | find_package(geometry_msgs REQUIRED) 42 | find_library( 43 | ur_modern_driver_LIBRARIES 44 | NAMES ur_driver_lib 45 | HINTS /usr/local/lib) 46 | find_path(ur_modern_driver_INCLUDE_DIRS ur_modern_driver/tcp_socket.h) 47 | 48 | # Eigen 3.2 (Wily) only provides EIGEN3_INCLUDE_DIR, not EIGEN3_INCLUDE_DIRS 49 | if(NOT EIGEN3_INCLUDE_DIRS) 50 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 51 | endif() 52 | 53 | # Set include directory path 54 | include_directories( 55 | include 56 | ${rclcpp_INCLUDE_DIRS} 57 | ${tf2_INCLUDE_DIRS} 58 | ${tf2_eigen_INCLUDE_DIRS} 59 | ${tf2_ros_INCLUDE_DIRS} 60 | ${Eigen3_INCLUDE_DIRS} 61 | ${sensor_msgs_INCLUDE_DIRS} 62 | ${geometry_msgs_INCLUDE_DIRS} 63 | ${ur_modern_driver_INCLUDE_DIRS}) 64 | 65 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) 66 | 67 | # Add robot interface library 68 | set(${PROJECT_NAME}_SOURCES 69 | src/control_base.cpp 70 | src/control_ur.cpp 71 | ) 72 | add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) 73 | ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs geometry_msgs tf2_ros) 74 | target_link_libraries(${PROJECT_NAME} ${ur_modern_driver_LIBRARIES}) 75 | 76 | # Add test of UR robot interface library 77 | set(TEST_SOURCE 78 | test/ur_test.cpp) 79 | 80 | add_executable(ur_test_move_command test/ur_test_move_command.cpp) 81 | ament_target_dependencies(ur_test_move_command rclcpp sensor_msgs geometry_msgs) 82 | target_link_libraries(ur_test_move_command ${PROJECT_NAME} ${ur_modern_driver_LIBRARIES}) 83 | 84 | add_executable(ur_test_state_publish test/ur_test_state_publish.cpp) 85 | ament_target_dependencies(ur_test_state_publish rclcpp sensor_msgs geometry_msgs) 86 | target_link_libraries(ur_test_state_publish ${PROJECT_NAME} ${ur_modern_driver_LIBRARIES}) 87 | 88 | ament_export_include_directories(include ${Eigen3_INCLUDE_DIRS}) 89 | ament_export_interfaces(${PROJECT_NAME} HAS_LIBRARY_TARGET) 90 | ament_export_libraries(${PROJECT_NAME} ${ur_modern_driver_LIBRARIES}) 91 | ament_export_dependencies(rclcpp) 92 | ament_export_dependencies(sensor_msgs) 93 | ament_export_dependencies(geometry_msgs) 94 | ament_export_dependencies(tf2_ros) 95 | ament_export_dependencies(eigen3_cmake_module) 96 | ament_export_dependencies(Eigen3) 97 | 98 | # Install library 99 | install( 100 | TARGETS ${PROJECT_NAME} 101 | EXPORT ${PROJECT_NAME} 102 | ARCHIVE DESTINATION lib 103 | LIBRARY DESTINATION lib 104 | RUNTIME DESTINATION bin 105 | INCLUDES DESTINATION include 106 | ) 107 | 108 | # Install executables 109 | install(TARGETS ur_test_move_command ur_test_state_publish 110 | DESTINATION lib/${PROJECT_NAME}) 111 | 112 | # Install header files 113 | install( 114 | DIRECTORY include/ 115 | DESTINATION include 116 | ) 117 | 118 | # Install launch files. 119 | install(DIRECTORY 120 | launch 121 | DESTINATION share/${PROJECT_NAME}/ 122 | ) 123 | 124 | if(BUILD_TESTING) 125 | find_package(ament_lint_auto REQUIRED) 126 | # the following line skips the linter which checks for copyrights 127 | # uncomment the line when a copyright and license is not present in all source files 128 | #set(ament_cmake_copyright_FOUND TRUE) 129 | # the following line skips cpplint (only works in a git repo) 130 | # uncomment the line when this package is not in a git repo 131 | #set(ament_cmake_cpplint_FOUND TRUE) 132 | ament_lint_auto_find_test_dependencies() 133 | endif() 134 | 135 | ament_package() 136 | -------------------------------------------------------------------------------- /grasp_utils/robot_interface/README.md: -------------------------------------------------------------------------------- 1 | # robot_interface 2 | 3 | ROS2 package to use robot native interface 4 | 5 | ## Install 6 | 7 | Install dependency **ur_modern_driver**: 8 | 9 | ```shell 10 | git clone -b libur_modern_driver https://github.com/RoboticsYY/ur_modern_driver.git 11 | cd ur_modern_driver/libur_modern_driver 12 | mkdir build && cd build 13 | cmake .. && sudo make install 14 | ``` 15 | 16 | Install dependency **ros2_ur_description**: 17 | 18 | ```shell 19 | mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src 20 | git clone https://github.com/RoboticsYY/ros2_ur_description.git 21 | cd .. && colcon build 22 | ``` 23 | 24 | Install dependence **eigen3-cmake-module**: 25 | ```shell 26 | sudo apt install ros-dashing-eigen3-cmake-module 27 | ``` 28 | 29 | Install **robot_interface**: 30 | 31 | The installation should refer to the installation of **ros2_grasp_library**. 32 | 33 | ## Launch 34 | 35 | Launch the UR robot control test executable: 36 | 37 | ```shell 38 | ros2 launch robot_interface ur_test.launch.py move:=true 39 | ``` 40 | 41 | Launch the Rivz2 display: 42 | 43 | ```shell 44 | ros2 launch ur_description view_ur5_ros2.launch.py 45 | ``` 46 | 47 | ## Generate Document 48 | 49 | ```shell 50 | cd /grasp_utils/robot_interface 51 | 52 | doxygen Doxyfile 53 | ``` 54 | -------------------------------------------------------------------------------- /grasp_utils/robot_interface/include/robot_interface/control_ur.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | * @file ur_control.hpp 17 | */ 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "ur_modern_driver/log.h" 26 | #include "ur_modern_driver/pipeline.h" 27 | #include "ur_modern_driver/ur/commander.h" 28 | #include "ur_modern_driver/ur/factory.h" 29 | #include "ur_modern_driver/ur/messages.h" 30 | #include "ur_modern_driver/ur/parser.h" 31 | #include "ur_modern_driver/ur/producer.h" 32 | #include "ur_modern_driver/ur/rt_state.h" 33 | #include "ur_modern_driver/ur/state.h" 34 | 35 | static const std::vector JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", 36 | "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" }; 37 | static const std::string HOST = "192.168.0.5"; 38 | static const bool SHUTDOWN_ON_DISCONNECT = true; 39 | static const int UR_SECONDARY_PORT = 30002; 40 | static const int UR_RT_PORT = 30003; 41 | 42 | struct ProgArgs 43 | { 44 | public: 45 | std::string host; 46 | std::vector joint_names; 47 | bool shutdown_on_disconnect; 48 | }; 49 | 50 | class IgnorePipelineStoppedNotifier : public INotifier 51 | { 52 | public: 53 | void started(std::string name) 54 | { 55 | LOG_INFO("Starting pipeline %s", name.c_str()); 56 | } 57 | void stopped(std::string name) 58 | { 59 | LOG_INFO("Stopping pipeline %s", name.c_str()); 60 | } 61 | }; 62 | 63 | class ShutdownOnPipelineStoppedNotifier : public INotifier 64 | { 65 | public: 66 | void started(std::string name) 67 | { 68 | LOG_INFO("Starting pipeline %s", name.c_str()); 69 | } 70 | void stopped(std::string name) 71 | { 72 | LOG_INFO("Shutting down on stopped pipeline %s", name.c_str()); 73 | rclcpp::shutdown(); 74 | exit(1); 75 | } 76 | }; 77 | 78 | class URControl: public ArmControlBase, public URRTPacketConsumer 79 | { 80 | public: 81 | URControl(const std::string node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 82 | : ArmControlBase(node_name, options), gripper_powered_up_(false) 83 | { 84 | for (auto const& joint : JOINTS) 85 | { 86 | joint_names_.push_back(joint); 87 | } 88 | } 89 | 90 | ~URControl() 91 | { 92 | rt_pl_->stop(); 93 | state_pl_->stop(); 94 | factory_.reset(nullptr); 95 | notifier_ = nullptr; 96 | LOG_INFO("UR control interface shut down."); 97 | } 98 | 99 | // Overload ArmControlBase functions 100 | virtual bool moveToTcpPose(double x, double y, double z, 101 | double alpha, double beta, double gamma, 102 | double vel, double acc); 103 | 104 | virtual bool moveToJointValues(const std::vector& joint_values, double vel, double acc); 105 | 106 | virtual bool open(const double distance = 0); 107 | 108 | virtual bool close(const double distance = 0); 109 | 110 | // Send URScript to ur robot controller 111 | bool urscriptInterface(const std::string command_script); 112 | 113 | // Start socket communication loop 114 | bool startLoop(); 115 | 116 | // Overload URRTPacketConsumer functions 117 | virtual bool consume(RTState_V1_6__7& state); 118 | virtual bool consume(RTState_V1_8& state); 119 | virtual bool consume(RTState_V3_0__1& state); 120 | virtual bool consume(RTState_V3_2__3& state); 121 | 122 | virtual void setupConsumer() 123 | { 124 | } 125 | virtual void teardownConsumer() 126 | { 127 | } 128 | virtual void stopConsumer() 129 | { 130 | } 131 | 132 | // Functions to publish joint states 133 | bool publishJoints(RTShared& packet, rclcpp::Time t); 134 | bool publish(RTShared& packet); 135 | 136 | // Function to get tool pose 137 | bool getTcpPose(RTShared& packet); 138 | 139 | // Function to get current joint values 140 | bool getJointValues(RTShared& packet); 141 | 142 | // Parse parameters 143 | void parseArgs(); 144 | 145 | private: 146 | 147 | ProgArgs args_; 148 | std::string local_ip_; 149 | std::unique_ptr factory_; 150 | 151 | // Robot rt message 152 | std::unique_ptr> rt_parser_; 153 | std::unique_ptr rt_stream_; 154 | std::unique_ptr> rt_prod_; 155 | std::unique_ptr rt_commander_; 156 | vector *> rt_vec_; 157 | std::unique_ptr> rt_cons_; 158 | std::unique_ptr> rt_pl_; 159 | 160 | INotifier *notifier_; 161 | 162 | // Robot state message 163 | std::unique_ptr> state_parser_; 164 | std::unique_ptr state_stream_; 165 | std::unique_ptr> state_prod_; 166 | vector *> state_vec_; 167 | std::unique_ptr> state_cons_; 168 | std::unique_ptr> state_pl_; 169 | 170 | bool gripper_powered_up_; 171 | }; -------------------------------------------------------------------------------- /grasp_utils/robot_interface/launch/ur_test.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | import launch 18 | import launch.actions 19 | import launch.substitutions 20 | import launch_ros.actions 21 | from ament_index_python.packages import get_package_share_directory 22 | 23 | def generate_launch_description(): 24 | 25 | # .yaml file for configuring the parameters 26 | yaml = os.path.join( 27 | get_package_share_directory('robot_interface'), 28 | 'launch', 'ur_test.yaml' 29 | ) 30 | 31 | return launch.LaunchDescription([ 32 | 33 | launch.actions.DeclareLaunchArgument( 34 | "move", 35 | default_value=["true"], 36 | description="If using the move command test" 37 | ), 38 | 39 | launch_ros.actions.Node( 40 | package='robot_interface', 41 | node_executable='ur_test_state_publish', 42 | output='screen', arguments=['__params:='+yaml], 43 | condition=launch.conditions.UnlessCondition(launch.substitutions.LaunchConfiguration("move"))), 44 | 45 | launch_ros.actions.Node( 46 | package='robot_interface', 47 | node_executable='ur_test_move_command', 48 | output='screen', arguments=['__params:='+yaml], 49 | condition=launch.conditions.IfCondition(launch.substitutions.LaunchConfiguration("move"))), 50 | ]) -------------------------------------------------------------------------------- /grasp_utils/robot_interface/launch/ur_test.yaml: -------------------------------------------------------------------------------- 1 | ur_test: 2 | ros__parameters: 3 | host: "192.168.0.5" 4 | shutdown_on_disconnect: true 5 | joint_names: ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] -------------------------------------------------------------------------------- /grasp_utils/robot_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_interface 5 | 0.5.0 6 | Native robot motion control interface 7 | Yu Yan 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | eigen3_cmake_module 12 | 13 | eigen3_cmake_module 14 | 15 | rclcpp 16 | tf2 17 | tf2_eigen 18 | eigen 19 | 20 | rclcpp 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | eigen 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /grasp_utils/robot_interface/test/ur_test_move_command.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | * @file ur_test.cpp 17 | */ 18 | 19 | #include 20 | 21 | int main(int argc, char * argv[]) 22 | { 23 | rclcpp::init(argc, argv); 24 | 25 | std::shared_ptr arm_control(new URControl("ur_test", rclcpp::NodeOptions() 26 | .allow_undeclared_parameters(true) 27 | .automatically_declare_parameters_from_overrides(true))); 28 | 29 | arm_control->parseArgs(); 30 | arm_control->startLoop(); 31 | 32 | rclcpp::sleep_for(std::chrono::seconds(2)); 33 | 34 | while(rclcpp::ok()) 35 | { 36 | geometry_msgs::msg::PoseStamped pose_stamped; 37 | pose_stamped.header.frame_id = "base"; 38 | pose_stamped.header.stamp = arm_control->now(); 39 | pose_stamped.pose.position.x = -0.068673; 40 | pose_stamped.pose.position.y = -0.595636; 41 | pose_stamped.pose.position.z = 0.201606; 42 | pose_stamped.pose.orientation.x = -0.311507; 43 | pose_stamped.pose.orientation.y = 0.950216; 44 | pose_stamped.pose.orientation.z = -0.004305; 45 | pose_stamped.pose.orientation.w = 0.005879; 46 | 47 | arm_control->moveToTcpPose(pose_stamped, 0.3, 0.3); 48 | 49 | pose_stamped.header.frame_id = "base"; 50 | pose_stamped.header.stamp = arm_control->now(); 51 | pose_stamped.pose.position.x = -0.157402; 52 | pose_stamped.pose.position.y = -0.679509; 53 | pose_stamped.pose.position.z = 0.094437; 54 | pose_stamped.pose.orientation.x = 0.190600; 55 | pose_stamped.pose.orientation.y = 0.948295; 56 | pose_stamped.pose.orientation.z = 0.239947; 57 | pose_stamped.pose.orientation.w = 0.082662; 58 | 59 | arm_control->pick(pose_stamped, 1.05, 1.4, 0.5, 0.1); 60 | arm_control->place(pose_stamped, 1.05, 1.4, 0.5, 0.1); 61 | 62 | arm_control->moveToTcpPose(-0.350, -0.296, 0.12, 3.14159, 0, 0, 0.3, 0.3); 63 | 64 | arm_control->moveToJointValues(std::vector{0.87, -1.44, 1.68, -1.81, -1.56, 0}, 1.05, 1.4); 65 | 66 | arm_control->pick(-0.153, -0.433, 0.145, 2.8, -0.144, 0.0245, 1.05, 1.4, 0.5, 0.1); 67 | arm_control->place(-0.350, -0.296, 0.145, 3.14159, 0, 0, 1.05, 1.4, 0.5, 0.1); 68 | } 69 | rclcpp::shutdown(); 70 | return 0; 71 | } -------------------------------------------------------------------------------- /grasp_utils/robot_interface/test/ur_test_state_publish.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation. All Rights Reserved 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 | * @file ur_test.cpp 17 | */ 18 | 19 | #include 20 | 21 | int main(int argc, char * argv[]) 22 | { 23 | rclcpp::init(argc, argv); 24 | 25 | std::shared_ptr arm_control(new URControl("ur_test", rclcpp::NodeOptions() 26 | .allow_undeclared_parameters(true) 27 | .automatically_declare_parameters_from_overrides(true))); 28 | 29 | arm_control->parseArgs(); 30 | arm_control->startLoop(); 31 | 32 | rclcpp::spin(arm_control); 33 | rclcpp::shutdown(); 34 | return 0; 35 | } -------------------------------------------------------------------------------- /moveit_msgs_light/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(moveit_msgs) 4 | 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | find_package(builtin_interfaces REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(shape_msgs REQUIRED) 19 | find_package(trajectory_msgs REQUIRED) 20 | 21 | set(msg_files 22 | "msg/CollisionObject.msg" 23 | "msg/Grasp.msg" 24 | "msg/GripperTranslation.msg" 25 | "msg/MoveItErrorCodes.msg" 26 | "msg/ObjectType.msg" 27 | "msg/PlaceLocation.msg" 28 | ) 29 | 30 | set(srv_files 31 | "srv/GraspPlanning.srv" 32 | ) 33 | 34 | rosidl_generate_interfaces(${PROJECT_NAME} 35 | ${msg_files} 36 | ${srv_files} 37 | DEPENDENCIES builtin_interfaces std_msgs geometry_msgs shape_msgs trajectory_msgs 38 | ADD_LINTER_TESTS 39 | ) 40 | 41 | # todo install ros1_bridge mapping rules 42 | 43 | ament_export_dependencies(rosidl_default_runtime) 44 | 45 | ament_package() 46 | -------------------------------------------------------------------------------- /moveit_msgs_light/README.md: -------------------------------------------------------------------------------- 1 | This's a temporary solution for ROS2 interface, till MoveIt is ported to ROS2. 2 | -------------------------------------------------------------------------------- /moveit_msgs_light/msg/CollisionObject.msg: -------------------------------------------------------------------------------- 1 | # a header, used for interpreting the poses 2 | std_msgs/Header header 3 | 4 | # the id of the object (name used in MoveIt) 5 | string id 6 | 7 | # The object type in a database of known objects 8 | moveit_msgs/ObjectType type 9 | 10 | # the the collision geometries associated with the object; 11 | # their poses are with respect to the specified header 12 | 13 | # solid geometric primitives 14 | shape_msgs/SolidPrimitive[] primitives 15 | geometry_msgs/Pose[] primitive_poses 16 | 17 | # meshes 18 | shape_msgs/Mesh[] meshes 19 | geometry_msgs/Pose[] mesh_poses 20 | 21 | # bounding planes (equation is specified, but the plane can be oriented using an additional pose) 22 | shape_msgs/Plane[] planes 23 | geometry_msgs/Pose[] plane_poses 24 | 25 | # Adds the object to the planning scene. If the object previously existed, it is replaced. 26 | byte ADD=0 27 | 28 | # Removes the object from the environment entirely (everything that matches the specified id) 29 | byte REMOVE=1 30 | 31 | # Append to an object that already exists in the planning scene. If the does not exist, it is added. 32 | byte APPEND=2 33 | 34 | # If an object already exists in the scene, new poses can be sent (the geometry arrays must be left empty) 35 | # if solely moving the object is desired 36 | byte MOVE=3 37 | 38 | # Operation to be performed 39 | byte operation 40 | -------------------------------------------------------------------------------- /moveit_msgs_light/msg/Grasp.msg: -------------------------------------------------------------------------------- 1 | # This message contains a description of a grasp that would be used 2 | # with a particular end-effector to grasp an object, including how to 3 | # approach it, grip it, etc. This message does not contain any 4 | # information about a "grasp point" (a position ON the object). 5 | # Whatever generates this message should have already combined 6 | # information about grasp points with information about the geometry 7 | # of the end-effector to compute the grasp_pose in this message. 8 | 9 | # A name for this grasp 10 | string id 11 | 12 | # The internal posture of the hand for the pre-grasp 13 | # only positions are used 14 | trajectory_msgs/JointTrajectory pre_grasp_posture 15 | 16 | # The internal posture of the hand for the grasp 17 | # positions and efforts are used 18 | trajectory_msgs/JointTrajectory grasp_posture 19 | 20 | # The position of the end-effector for the grasp. This is the pose of 21 | # the "parent_link" of the end-effector, not actually the pose of any 22 | # link *in* the end-effector. Typically this would be the pose of the 23 | # most distal wrist link before the hand (end-effector) links began. 24 | geometry_msgs/PoseStamped grasp_pose 25 | 26 | # The estimated probability of success for this grasp, or some other 27 | # measure of how "good" it is. 28 | float64 grasp_quality 29 | 30 | # The approach direction to take before picking an object 31 | GripperTranslation pre_grasp_approach 32 | 33 | # The retreat direction to take after a grasp has been completed (object is attached) 34 | GripperTranslation post_grasp_retreat 35 | 36 | # The retreat motion to perform when releasing the object; this information 37 | # is not necessary for the grasp itself, but when releasing the object, 38 | # the information will be necessary. The grasp used to perform a pickup 39 | # is returned as part of the result, so this information is available for 40 | # later use. 41 | GripperTranslation post_place_retreat 42 | 43 | # the maximum contact force to use while grasping (<=0 to disable) 44 | float32 max_contact_force 45 | 46 | # an optional list of obstacles that we have semantic information about 47 | # and that can be touched/pushed/moved in the course of grasping 48 | string[] allowed_touch_objects 49 | -------------------------------------------------------------------------------- /moveit_msgs_light/msg/GripperTranslation.msg: -------------------------------------------------------------------------------- 1 | # defines a translation for the gripper, used in pickup or place tasks 2 | # for example for lifting an object off a table or approaching the table for placing 3 | 4 | # the direction of the translation 5 | geometry_msgs/Vector3Stamped direction 6 | 7 | # the desired translation distance 8 | float32 desired_distance 9 | 10 | # the min distance that must be considered feasible before the 11 | # grasp is even attempted 12 | float32 min_distance 13 | -------------------------------------------------------------------------------- /moveit_msgs_light/msg/MoveItErrorCodes.msg: -------------------------------------------------------------------------------- 1 | int32 val 2 | 3 | # overall behavior 4 | int32 SUCCESS=1 5 | int32 FAILURE=99999 6 | 7 | int32 PLANNING_FAILED=-1 8 | int32 INVALID_MOTION_PLAN=-2 9 | int32 MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE=-3 10 | int32 CONTROL_FAILED=-4 11 | int32 UNABLE_TO_AQUIRE_SENSOR_DATA=-5 12 | int32 TIMED_OUT=-6 13 | int32 PREEMPTED=-7 14 | 15 | # planning & kinematics request errors 16 | int32 START_STATE_IN_COLLISION=-10 17 | int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-11 18 | 19 | int32 GOAL_IN_COLLISION=-12 20 | int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-13 21 | int32 GOAL_CONSTRAINTS_VIOLATED=-14 22 | 23 | int32 INVALID_GROUP_NAME=-15 24 | int32 INVALID_GOAL_CONSTRAINTS=-16 25 | int32 INVALID_ROBOT_STATE=-17 26 | int32 INVALID_LINK_NAME=-18 27 | int32 INVALID_OBJECT_NAME=-19 28 | 29 | # system errors 30 | int32 FRAME_TRANSFORM_FAILURE=-21 31 | int32 COLLISION_CHECKING_UNAVAILABLE=-22 32 | int32 ROBOT_STATE_STALE=-23 33 | int32 SENSOR_INFO_STALE=-24 34 | 35 | # kinematics errors 36 | int32 NO_IK_SOLUTION=-31 37 | -------------------------------------------------------------------------------- /moveit_msgs_light/msg/ObjectType.msg: -------------------------------------------------------------------------------- 1 | ################################################## OBJECT ID ######################################################### 2 | 3 | # Contains information about the type of a found object. Those two sets of parameters together uniquely define an 4 | # object 5 | 6 | # The key of the found object: the unique identifier in the given db 7 | string key 8 | 9 | # The db parameters stored as a JSON/compressed YAML string. An object id does not make sense without the corresponding 10 | # database. E.g., in object_recognition, it can look like: "{'type':'CouchDB', 'root':'http://localhost'}" 11 | # There is no conventional format for those parameters and it's nice to keep that flexibility. 12 | # The object_recognition_core as a generic DB type that can read those fields 13 | # Current examples: 14 | # For CouchDB: 15 | # type: 'CouchDB' 16 | # root: 'http://localhost:5984' 17 | # collection: 'object_recognition' 18 | # For SQL household database: 19 | # type: 'SqlHousehold' 20 | # host: 'wgs36' 21 | # port: 5432 22 | # user: 'willow' 23 | # password: 'willow' 24 | # name: 'household_objects' 25 | # module: 'tabletop' 26 | string db 27 | -------------------------------------------------------------------------------- /moveit_msgs_light/msg/PlaceLocation.msg: -------------------------------------------------------------------------------- 1 | # A name for this grasp 2 | string id 3 | 4 | # The internal posture of the hand for the grasp 5 | # positions and efforts are used 6 | trajectory_msgs/JointTrajectory post_place_posture 7 | 8 | # The position of the end-effector for the grasp relative to a reference frame 9 | # (that is always specified elsewhere, not in this message) 10 | geometry_msgs/PoseStamped place_pose 11 | 12 | # The approach motion 13 | GripperTranslation pre_place_approach 14 | 15 | # The retreat motion 16 | GripperTranslation post_place_retreat 17 | 18 | # an optional list of obstacles that we have semantic information about 19 | # and that can be touched/pushed/moved in the course of grasping 20 | string[] allowed_touch_objects 21 | -------------------------------------------------------------------------------- /moveit_msgs_light/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | moveit_msgs 5 | 0.5.0 6 | ROS2 messages definitions for MoveIt 7 | Sharron LIU 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | builtin_interfaces 13 | std_msgs 14 | geometry_msgs 15 | shape_msgs 16 | trajectory_msgs 17 | 18 | rosidl_default_runtime 19 | builtin_interfaces 20 | std_msgs 21 | geometry_msgs 22 | shape_msgs 23 | trajectory_msgs 24 | 25 | ament_lint_common 26 | 27 | rosidl_interface_packages 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /moveit_msgs_light/srv/GraspPlanning.srv: -------------------------------------------------------------------------------- 1 | # Requests that grasp planning be performed for the target object 2 | # returns a list of candidate grasps to be tested and executed 3 | 4 | # the planning group used 5 | string group_name 6 | 7 | # the object to be grasped 8 | CollisionObject target 9 | 10 | # the names of the relevant support surfaces (e.g. tables) in the collision map 11 | # can be left empty if no names are available 12 | string[] support_surfaces 13 | 14 | # an optional list of grasps to be evaluated by the planner 15 | Grasp[] candidate_grasps 16 | 17 | # an optional list of obstacles that we have semantic information about 18 | # and that can be moved in the course of grasping 19 | CollisionObject[] movable_obstacles 20 | 21 | --- 22 | 23 | # the list of planned grasps 24 | Grasp[] grasps 25 | 26 | # whether an error occurred 27 | MoveItErrorCodes error_code 28 | --------------------------------------------------------------------------------