diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index abe766e..f60b4dd 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-case-conflict @@ -29,7 +29,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 20.8b1 + rev: 23.3.0 hooks: - id: black @@ -38,10 +38,10 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] + args: ["-fallback-style=none", "-i"] - repo: local hooks: @@ -51,11 +51,3 @@ repos: language: pygrep entry: Moveit|MoveIt! exclude: .pre-commit-config.yaml|README.md - - - id: catkin_lint - name: catkin_lint - description: Check package.xml and cmake files - entry: catkin_lint . - language: system - always_run: true - pass_filenames: false diff --git a/README.md b/README.md index 5c2b74c..f511733 100644 --- a/README.md +++ b/README.md @@ -2,21 +2,32 @@ *Tools for robot arm hand-eye calibration.* -| **Warning to Melodic users** | -| --- | -| OpenCV 3.2, which is the version in Ubuntu 18.04, has a buggy ArUco board pose detector. Do not expect adequate results if you are using an ArUco board with OpenCV 3.2. | - MoveIt Calibration supports ArUco boards and ChArUco boards as calibration targets. Experiments have demonstrated that a ChArUco board gives more accurate results, so it is recommended. -This repository has been developed and tested on ROS Melodic and Noetic. It has not been tested on earlier ROS versions. -When building `moveit_calibration` on ROS Melodic, `rviz_visual_tools` must also be built from source. - This package was originally developed by Dr. Yu Yan at Intel, and was originally submitted as a PR to the core MoveIt repository. For background, see this [Github discussion](https://github.com/ros-planning/moveit/issues/1070). +## Instructions + +### Build from Source + +```sh +mkdir -p ws_moveit/src +cd ws_moveit +git clone https://github.com/ros-planning/moveit_calibration.git -b ros2 src/moveit_calibration +vcs import src < src/moveit_calibration/moveit_calibration.repos --skip-existing +rosdep install -r --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +### Example + + +For examples, please follow [Hand-Eye Calibration tutorial](https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/examples/hand_eye_calibration/hand_eye_calibration_tutorial.rst) from [moveit2_tutorials](https://github.com/ros-planning/moveit2_tutorials). + ## GitHub Actions - Continuous Integration -[![Format](https://github.com/ros-planning/moveit_calibration/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit_calibration/actions/workflows/format.yaml?branch=master) -[![BuildAndTest](https://github.com/ros-planning/moveit_calibration/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit_calibration/actions/workflows/ci.yaml?branch=master) -[![codecov](https://codecov.io/gh/ros-planning/moveit_calibration/branch/master/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit_calibration) +[![Format](https://github.com/ros-planning/moveit_calibration/actions/workflows/format.yaml/badge.svg?branch=ros2)](https://github.com/ros-planning/moveit_calibration/actions/workflows/format.yaml?branch=ros2) +[![BuildAndTest](https://github.com/ros-planning/moveit_calibration/actions/workflows/ci.yaml/badge.svg?branch=ros2)](https://github.com/ros-planning/moveit_calibration/actions/workflows/ci.yaml?branch=ros2) +[![codecov](https://codecov.io/gh/ros-planning/moveit_calibration/branch/ros2/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit_calibration) diff --git a/moveit_calibration_demos/.docker/Dockerfile b/moveit_calibration_demos/.docker/Dockerfile new file mode 100644 index 0000000..25816e5 --- /dev/null +++ b/moveit_calibration_demos/.docker/Dockerfile @@ -0,0 +1,49 @@ +ARG ROS_DISTRO=humble +FROM moveit/moveit2:${ROS_DISTRO}-source + +### Use bash by default +SHELL ["/bin/bash", "-c"] + +### Install Gazebo +ARG GZ_VERSION=fortress +ENV GZ_VERSION=${GZ_VERSION} +ARG IGNITION_VERSION=${GZ_VERSION} +ENV IGNITION_VERSION=${GZ_VERSION} +RUN apt-get update && \ + apt-get install -yq --no-install-recommends \ + gz-${GZ_VERSION} && \ + rm -rf /var/lib/apt/lists/* + +### Import and install dependencies, then build these dependencies (not moveit_calibration yet) +COPY ./moveit_calibration_demos/moveit_calibration_demos.repos /root/ws_moveit/src/moveit_calibration/moveit_calibration_demos/moveit_calibration_demos.repos +RUN vcs import --shallow /root/ws_moveit/src < /root/ws_moveit/src/moveit_calibration/moveit_calibration_demos/moveit_calibration_demos.repos && \ + rosdep update && \ + apt-get update && \ + rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths /root/ws_moveit/src && \ + rm -rf /var/lib/apt/lists/* && \ + source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ + source "/root/ws_moveit/install/local_setup.bash" && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --ament-cmake-args -DCMAKE_BUILD_TYPE=Release && \ + rm -rf /root/ws_moveit/log + +### Copy over the rest of moveit_calibration, then install dependencies and build +COPY ./ /root/ws_moveit/src/moveit_calibration/ +RUN rosdep update && \ + apt-get update && \ + rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths /root/ws_moveit/src && \ + rm -rf /var/lib/apt/lists/* && \ + source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ + source "/root/ws_moveit/install/local_setup.bash" && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --ament-cmake-args -DCMAKE_BUILD_TYPE=Release && \ + rm -rf /root/ws_moveit/log + +### Install panda packages again with --symlink-install (workaround, needed for SDF exporter) +RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ + source "/root/ws_moveit/install/local_setup.bash" && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --ament-cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --packages-select panda_description panda_moveit_config panda --symlink-install && \ + rm -rf /root/ws_moveit/log + +### Add workspace to the ROS entrypoint +### Source ROS workspace inside `~/.bashrc` to enable autocompletion +RUN sed -i '$i source "/root/ws_moveit/install/local_setup.bash" --' /ros_entrypoint.sh && \ + sed -i '$a source "/opt/ros/${ROS_DISTRO}/setup.bash"' ~/.bashrc diff --git a/moveit_calibration_demos/.docker/build.bash b/moveit_calibration_demos/.docker/build.bash new file mode 100755 index 0000000..edcf98e --- /dev/null +++ b/moveit_calibration_demos/.docker/build.bash @@ -0,0 +1,40 @@ +#!/usr/bin/env bash + +## Configuration +# Default Docker Hub user (used if user is not logged in) +DEFAULT_DOCKERHUB_USER="andrejorsula" + +## Determine the name of the image to run (automatically inferred from the current user and repository, or using the default if not available) +# Get the current Docker Hub user or use the default +DOCKERHUB_USER="$(docker info | sed '/Username:/!d;s/.* //')" +DOCKERHUB_USER="${DOCKERHUB_USER:-${DEFAULT_DOCKERHUB_USER}}" +# Get the name of the repository (directory) +SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" +REPOSITORY_DIR="$(dirname "$(dirname "${SCRIPT_DIR}")")" +REPOSITORY_NAME="$(basename "${REPOSITORY_DIR}")" +# Combine the user and repository name to form the image name +IMAGE_NAME="${DOCKERHUB_USER}/${REPOSITORY_NAME}" +# Determine the path to the Dockerfile +DOCKERFILE="${SCRIPT_DIR}/Dockerfile" + +## Parse TAG and forward additional build arguments +if [ "${#}" -gt "0" ]; then + if [[ "${1}" != "-"* ]]; then + IMAGE_NAME="${IMAGE_NAME}:${1}" + BUILD_ARGS=${*:2} + else + BUILD_ARGS=${*:1} + fi +fi + +## Build the image +DOCKER_BUILD_CMD=( + docker build + --file "${DOCKERFILE}" + --tag "${IMAGE_NAME}" + "${BUILD_ARGS}" + "${REPOSITORY_DIR}" +) +echo -e "\033[1;30m${DOCKER_BUILD_CMD[*]}\033[0m" | xargs +# shellcheck disable=SC2048 +exec ${DOCKER_BUILD_CMD[*]} diff --git a/moveit_calibration_demos/.docker/run.bash b/moveit_calibration_demos/.docker/run.bash new file mode 100755 index 0000000..19cf804 --- /dev/null +++ b/moveit_calibration_demos/.docker/run.bash @@ -0,0 +1,130 @@ +#!/usr/bin/env bash + +## Configuration +# Default Docker Hub user and repository name (used if inferred image is not available) +DEFAULT_DOCKERHUB_USER="andrejorsula" +DEFAULT_REPOSITORY_NAME="moveit2_calibration" +# Flags for running the container +DOCKER_RUN_OPTS="${DOCKER_RUN_OPTS:- + --name "${DEFAULT_REPOSITORY_NAME}" + --interactive + --tty + --rm + --network host + --ipc host +}" +# Flags for enabling GPU (NVIDIA) and GUI (X11) inside the container +ENABLE_GPU="${ENABLE_GPU:-true}" +ENABLE_GUI="${ENABLE_GUI:-true}" +# List of volumes to mount (can be updated by passing -v HOST_DIR:DOCKER_DIR:OPTIONS) +CUSTOM_VOLUMES=( + "/etc/localtime:/etc/localtime:ro" +) +# List of environment variables to set (can be updated by passing -e ENV=VALUE) +CUSTOM_ENVS=( +) + +## Determine the name of the image to run (automatically inferred from the current user and repository, or using the default if not available) +# Get the current Docker Hub user or use the default +DOCKERHUB_USER="$(docker info | sed '/Username:/!d;s/.* //')" +DOCKERHUB_USER="${DOCKERHUB_USER:-${DEFAULT_DOCKERHUB_USER}}" +# Get the name of the repository (directory) or use the default +SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" +REPOSITORY_DIR="$(dirname "${SCRIPT_DIR}")" +if [[ -f "${REPOSITORY_DIR}/Dockerfile" ]]; then + REPOSITORY_NAME="$(basename "${REPOSITORY_DIR}")" +else + REPOSITORY_NAME="${DEFAULT_REPOSITORY_NAME}" +fi +# Combine the user and repository name to form the image name +IMAGE_NAME="${DOCKERHUB_USER}/${REPOSITORY_NAME}" +# Determine if such image exists (either locally or on Docker Hub), otherwise use the default image name +if [[ -z "$(docker images -q "${IMAGE_NAME}")" ]] || [[ -z "$(wget -q "https://registry.hub.docker.com/v2/repositories/${IMAGE_NAME}" -O -)" ]]; then + IMAGE_NAME="${DEFAULT_DOCKERHUB_USER}/${DEFAULT_REPOSITORY_NAME}" +fi + +## Parse volumes and environment variables +while getopts ":v:e:" opt; do + case "${opt}" in + v) CUSTOM_VOLUMES+=("${OPTARG}") ;; + e) CUSTOM_ENVS+=("${OPTARG}") ;; + *) + echo >&2 "Usage: ${0} [-v HOST_DIR:DOCKER_DIR:OPTIONS] [-e ENV=VALUE] [TAG] [CMD]" + exit 2 + ;; + esac +done +shift "$((OPTIND - 1))" + +## Parse TAG and CMD positional arguments +if [ "${#}" -gt "0" ]; then + if [[ $(docker images --format "{{.Tag}}" "${IMAGE_NAME}") =~ (^|[[:space:]])${1}($|[[:space:]]) || $(wget -q "https://registry.hub.docker.com/v2/repositories/${IMAGE_NAME}/tags" -O - | grep -Poe '(?<=(\"name\":\")).*?(?=\")') =~ (^|[[:space:]])${1}($|[[:space:]]) ]]; then + # Use the first argument as a tag is such tag exists either locally or on the remote registry + IMAGE_NAME="${IMAGE_NAME}:${1}" + CMD=${*:2} + else + CMD=${*:1} + fi +fi + +## GPU +if [[ "${ENABLE_GPU}" = true ]]; then + LS_HW_DISPLAY=$(lshw -C display 2> /dev/null | grep vendor) + if [[ ${LS_HW_DISPLAY^^} =~ NVIDIA ]]; then + # Enable GPU either via NVIDIA Container Toolkit or NVIDIA Docker (depending on Docker version) + if dpkg --compare-versions "$(docker version --format '{{.Server.Version}}')" gt "19.3"; then + GPU_OPT="--gpus all" + else + GPU_OPT="--runtime nvidia" + fi + GPU_ENVS=( + NVIDIA_VISIBLE_DEVICES="all" + NVIDIA_DRIVER_CAPABILITIES="all" + ) + fi +fi + +## GUI +if [[ "${ENABLE_GUI}" = true ]]; then + # To enable GUI, make sure processes in the container can connect to the x server + XAUTH=/tmp/.docker.xauth + if [ ! -f ${XAUTH} ]; then + touch ${XAUTH} + chmod a+r ${XAUTH} + + XAUTH_LIST=$(xauth nlist "${DISPLAY}") + if [ -n "${XAUTH_LIST}" ]; then + # shellcheck disable=SC2001 + XAUTH_LIST=$(sed -e 's/^..../ffff/' <<<"${XAUTH_LIST}") + echo "${XAUTH_LIST}" | xauth -f ${XAUTH} nmerge - + fi + fi + # GUI-enabling volumes + GUI_VOLUMES=( + "${XAUTH}:${XAUTH}" + "/tmp/.X11-unix:/tmp/.X11-unix" + "/dev/input:/dev/input" + ) + # GUI-enabling environment variables + GUI_ENVS=( + DISPLAY="${DISPLAY}" + XAUTHORITY="${XAUTH}" + ) +fi + +## Run the container +DOCKER_RUN_CMD=( + docker run + "${DOCKER_RUN_OPTS}" + "${GPU_OPT}" + "${GPU_ENVS[@]/#/"--env "}" + "${GUI_VOLUMES[@]/#/"--volume "}" + "${GUI_ENVS[@]/#/"--env "}" + "${CUSTOM_VOLUMES[@]/#/"--volume "}" + "${CUSTOM_ENVS[@]/#/"--env "}" + "${IMAGE_NAME}" + "${CMD}" +) +echo -e "\033[1;30m${DOCKER_RUN_CMD[*]}\033[0m" | xargs +# shellcheck disable=SC2048 +exec ${DOCKER_RUN_CMD[*]} diff --git a/moveit_calibration_demos/CMakeLists.txt b/moveit_calibration_demos/CMakeLists.txt new file mode 100644 index 0000000..ca69f2c --- /dev/null +++ b/moveit_calibration_demos/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(moveit_calibration_demos) + +# Find dependencies +find_package(ament_cmake REQUIRED) + +# Install directories +install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME}) + +# Setup the project +ament_package() diff --git a/moveit_calibration_demos/launch/_gz_moveit_calibration.launch.py b/moveit_calibration_demos/launch/_gz_moveit_calibration.launch.py new file mode 100755 index 0000000..c25c8a1 --- /dev/null +++ b/moveit_calibration_demos/launch/_gz_moveit_calibration.launch.py @@ -0,0 +1,186 @@ +#!/usr/bin/env -S ros2 launch +"""MoveIt2 hand-eye calibration example inside Gazebo simulation (eye-in-hand variant)""" + +from math import pi +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + robot = LaunchConfiguration("robot") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + gz_verbosity = LaunchConfiguration("gz_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # TODO: Use official launch script from moveit2_tutorials once it supports this use case + # Launch move_group of MoveIt 2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare(robot), + "launch", + "gz.launch.py", + ] + ) + ), + launch_arguments=[ + ["world", world], + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("gz_verbosity", gz_verbosity), + ("log_level", log_level), + ], + ), + ] + + # List of nodes to be launched + nodes = [ + # static_transform_publisher (identity; world_moveit_calibration) + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=[ + "--frame-id", + ["world"], + "--child-frame-id", + ["world_moveit_calibration"], + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + # static_transform_publisher (identity; rgbd_camera_base_link) + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=[ + "--frame-id", + "rgbd_camera_base_link", + "--child-frame-id", + "rgbd_camera/rgbd_camera_link/rgbd_camera", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + # static_transform_publisher (rotation; rgbd_camera_optical_frame) + # Note: Necessary because Gazebo camera sensors use different coordinates (X forward) + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=[ + "--frame-id", + "rgbd_camera_optical_frame", + "--child-frame-id", + "rgbd_camera_base_link", + "--roll", + str(-pi / 2), + "--pitch", + "0.0", + "--yaw", + str(-pi / 2), + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + # ros_gz_bridge (camera pose ground truth -> ROS 2) + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/model/rgbd_camera/pose" + + "@" + + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + remappings=[("/model/rgbd_camera/pose", "/camera_pose_ground_truth")], + ), + # ros_gz_bridge (image -> ROS 2; camera info -> ROS 2) + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + ["/rgbd_camera/image@sensor_msgs/msg/Image[gz.msgs.Image"], + ["/rgbd_camera/depth_image@sensor_msgs/msg/Image[gz.msgs.Image"], + [ + "/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo", + ], + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World and model for Gazebo + DeclareLaunchArgument( + "world", + default_value="ERROR_empty_launch_argument", + description="Name or filepath of world to load.", + ), + DeclareLaunchArgument( + "robot", + default_value="panda", + description="Name or filepath of model to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value="ERROR_empty_launch_argument", + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "gz_verbosity", + default_value="3", + description="Verbosity level for Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/moveit_calibration_demos/launch/gz_eye_in_hand_aruco.launch.py b/moveit_calibration_demos/launch/gz_eye_in_hand_aruco.launch.py new file mode 100755 index 0000000..c44fd3b --- /dev/null +++ b/moveit_calibration_demos/launch/gz_eye_in_hand_aruco.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env -S ros2 launch +"""MoveIt2 hand-eye calibration example inside Gazebo simulation (eye-to-hand variant)""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + rviz_config = LaunchConfiguration("rviz_config") + + # List of included launch descriptions + launch_descriptions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("moveit_calibration_demos"), + "launch", + "_gz_moveit_calibration.launch.py", + ] + ) + ), + launch_arguments=[ + ["world", world], + ("rviz_config", rviz_config), + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World and model for Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "worlds", + "eye_in_hand_aruco.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "rviz", + "eye_in_hand_aruco.rviz", + ), + description="Path to configuration for RViz2.", + ), + ] diff --git a/moveit_calibration_demos/launch/gz_eye_in_hand_charuco.launch.py b/moveit_calibration_demos/launch/gz_eye_in_hand_charuco.launch.py new file mode 100755 index 0000000..660b1de --- /dev/null +++ b/moveit_calibration_demos/launch/gz_eye_in_hand_charuco.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env -S ros2 launch +"""MoveIt2 hand-eye calibration example inside Gazebo simulation (eye-to-hand variant)""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + rviz_config = LaunchConfiguration("rviz_config") + + # List of included launch descriptions + launch_descriptions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("moveit_calibration_demos"), + "launch", + "_gz_moveit_calibration.launch.py", + ] + ) + ), + launch_arguments=[ + ["world", world], + ("rviz_config", rviz_config), + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World and model for Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "worlds", + "eye_in_hand_charuco.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "rviz", + "eye_in_hand_charuco.rviz", + ), + description="Path to configuration for RViz2.", + ), + ] diff --git a/moveit_calibration_demos/launch/gz_eye_to_hand_aruco.launch.py b/moveit_calibration_demos/launch/gz_eye_to_hand_aruco.launch.py new file mode 100755 index 0000000..d819b81 --- /dev/null +++ b/moveit_calibration_demos/launch/gz_eye_to_hand_aruco.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env -S ros2 launch +"""MoveIt2 hand-eye calibration example inside Gazebo simulation (eye-to-hand variant)""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + rviz_config = LaunchConfiguration("rviz_config") + + # List of included launch descriptions + launch_descriptions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("moveit_calibration_demos"), + "launch", + "_gz_moveit_calibration.launch.py", + ] + ) + ), + launch_arguments=[ + ["world", world], + ("rviz_config", rviz_config), + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World and model for Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "worlds", + "eye_to_hand_aruco.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "rviz", + "eye_to_hand_aruco.rviz", + ), + description="Path to configuration for RViz2.", + ), + ] diff --git a/moveit_calibration_demos/launch/gz_eye_to_hand_charuco.launch.py b/moveit_calibration_demos/launch/gz_eye_to_hand_charuco.launch.py new file mode 100755 index 0000000..89b8d69 --- /dev/null +++ b/moveit_calibration_demos/launch/gz_eye_to_hand_charuco.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env -S ros2 launch +"""MoveIt2 hand-eye calibration example inside Gazebo simulation (eye-to-hand variant)""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + rviz_config = LaunchConfiguration("rviz_config") + + # List of included launch descriptions + launch_descriptions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("moveit_calibration_demos"), + "launch", + "_gz_moveit_calibration.launch.py", + ] + ) + ), + launch_arguments=[ + ["world", world], + ("rviz_config", rviz_config), + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World and model for Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "worlds", + "eye_to_hand_charuco.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("moveit_calibration_demos"), + "rviz", + "eye_to_hand_charuco.rviz", + ), + description="Path to configuration for RViz2.", + ), + ] diff --git a/moveit_calibration_demos/moveit_calibration_demos.repos b/moveit_calibration_demos/moveit_calibration_demos.repos new file mode 100644 index 0000000..03f6333 --- /dev/null +++ b/moveit_calibration_demos/moveit_calibration_demos.repos @@ -0,0 +1,6 @@ +repositories: + # TODO: Use config from moveit2_tutorials once it supports this use case + panda_ign_moveit2: + type: git + url: https://github.com/AndrejOrsula/panda_ign_moveit2.git + version: humble diff --git a/moveit_calibration_demos/package.xml b/moveit_calibration_demos/package.xml new file mode 100644 index 0000000..d10f8ac --- /dev/null +++ b/moveit_calibration_demos/package.xml @@ -0,0 +1,30 @@ + + + moveit_calibration_demos + 1.0.0 + Demos for MoveIt 2 Calibration inside Gazebo simulation + Andrej Orsula + Andrej Orsula + BSD + + ament_cmake + + geometry_msgs + moveit_ros_planning_interface + moveit + rviz2 + + moveit_calibration_gui + moveit_calibration_plugins + ros_gz_sim + ros_gz_bridge + panda_moveit_config + panda_description + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/moveit_calibration_demos/rviz/eye_in_hand_aruco.rviz b/moveit_calibration_demos/rviz/eye_in_hand_aruco.rviz new file mode 100644 index 0000000..4ec3c01 --- /dev/null +++ b/moveit_calibration_demos/rviz/eye_in_hand_aruco.rviz @@ -0,0 +1,476 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /HandEyeCalibration1 + Splitter Ratio: 0.506147563457489 + Tree Height: 435 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Target Detection + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /handeye_calibration/target_detection + Value: true + - Alpha: 1 + Axes Length: 0.03999999910593033 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Camera Pose (Ground Truth) + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_pose_ground_truth + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + handeye_target: + Value: false + panda_hand: + Value: false + panda_hand_tcp: + Value: false + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + rgbd_camera/rgbd_camera_link/rgbd_camera: + Value: false + rgbd_camera_base_link: + Value: false + rgbd_camera_optical_frame: + Value: true + world: + Value: false + world_moveit_calibration: + Value: false + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + world_moveit_calibration: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.25 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: false + Show Trail: false + State Display Time: 0.1s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 0.5 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.10000000149011612 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 0.5 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - ArUco dictionary: DICT_5X5_250 + Camera FOV Marker: + Marker Alpha: 0.30000001192092896 + Marker Size: 1.5 + Value: true + Class: moveit_rviz_plugin/HandEyeCalibration + Enabled: true + Move Group Namespace: "" + Name: HandEyeCalibration + Planning Scene Topic: /monitored_planning_scene + Rx: 0 + Ry: 0 + Rz: 0 + Tx: 0 + Ty: 0 + Tz: 0 + Value: true + base: "" + eef: "" + group: arm + image_topic: /rgbd_camera/image + marker border (bits): 1 + marker separation (px): 20 + marker size (px): 200 + markers, X: 3 + markers, Y: 4 + measured marker size (m): 0.200000 + measured separation (m): 0.020000 + object: "" + sensor: "" + sensor_mount_type: 1 + solver: OpenCV/Daniilidis1998 + target_type: HandEyeTarget/Aruco + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.25 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.5 + Y: 0 + Z: 0.4000000059604645 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5235999822616577 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + HandEye Calibration: + collapsed: false + Height: 1411 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Target Detection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 0 + Y: 0 diff --git a/moveit_calibration_demos/rviz/eye_in_hand_charuco.rviz b/moveit_calibration_demos/rviz/eye_in_hand_charuco.rviz new file mode 100644 index 0000000..0725c1a --- /dev/null +++ b/moveit_calibration_demos/rviz/eye_in_hand_charuco.rviz @@ -0,0 +1,481 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /HandEyeCalibration1 + Splitter Ratio: 0.506147563457489 + Tree Height: 435 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Target Detection + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /handeye_calibration/target_detection + Value: true + - Alpha: 1 + Axes Length: 0.03999999910593033 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Camera Pose (Ground Truth) + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_pose_ground_truth + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + handeye_target: + Value: false + panda_hand: + Value: false + panda_hand_tcp: + Value: false + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + rgbd_camera/rgbd_camera_link/rgbd_camera: + Value: false + rgbd_camera_base_link: + Value: false + rgbd_camera_optical_frame: + Value: true + world: + Value: false + world_moveit_calibration: + Value: false + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + rgbd_camera_optical_frame: + rgbd_camera_base_link: + rgbd_camera/rgbd_camera_link/rgbd_camera: + {} + world_moveit_calibration: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.25 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: false + Show Trail: false + State Display Time: 0.1s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 0.5 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.10000000149011612 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 0.5 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - ArUco dictionary: DICT_5X5_250 + Camera FOV Marker: + Marker Alpha: 0.30000001192092896 + Marker Size: 1.5 + Value: true + Class: moveit_rviz_plugin/HandEyeCalibration + Enabled: true + Move Group Namespace: "" + Name: HandEyeCalibration + Planning Scene Topic: /monitored_planning_scene + Rx: 0 + Ry: 0 + Rz: 0 + Tx: 0 + Ty: 0 + Tz: 0 + Value: true + base: panda_link0 + eef: panda_hand + group: arm + image_topic: /rgbd_camera/image + longest board side (m): 0.560000 + margin size (px): 2 + marker border (bits): 1 + marker size (px): 50 + measured marker size (m): 0.060000 + object: handeye_target + sensor: rgbd_camera_optical_frame + sensor_mount_type: 1 + solver: OpenCV/Daniilidis1998 + square size (px): 80 + squares, X: 5 + squares, Y: 7 + target_type: HandEyeTarget/Charuco + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.25 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.5 + Y: 0 + Z: 0.4000000059604645 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5235999822616577 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + HandEye Calibration: + collapsed: false + Height: 1411 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ee00000339fc020000000afb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000022f000003390000016900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff000000010000011000000339fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000022f00000339000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000a00000001eefc0100000005fb000000100044006900730070006c0061007900730100000000000001ea0000015600fffffffb0000002000540061007200670065007400200044006500740065006300740069006f006e01000001f0000002bf0000009800fffffffb0000002600480061006e0064004500790065002000430061006c006900620072006100740069006f006e01000004b50000054b000002b700fffffffb0000000a0049006d0061006700650100000530000002500000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d00650000000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000080c0000033900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Target Detection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 0 + Y: 0 diff --git a/moveit_calibration_demos/rviz/eye_to_hand_aruco.rviz b/moveit_calibration_demos/rviz/eye_to_hand_aruco.rviz new file mode 100644 index 0000000..97ba357 --- /dev/null +++ b/moveit_calibration_demos/rviz/eye_to_hand_aruco.rviz @@ -0,0 +1,476 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /HandEyeCalibration1 + Splitter Ratio: 0.506147563457489 + Tree Height: 435 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Target Detection + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /handeye_calibration/target_detection + Value: true + - Alpha: 1 + Axes Length: 0.03999999910593033 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Camera Pose (Ground Truth) + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_pose_ground_truth + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + handeye_target: + Value: false + panda_hand: + Value: false + panda_hand_tcp: + Value: false + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + rgbd_camera/rgbd_camera_link/rgbd_camera: + Value: false + rgbd_camera_base_link: + Value: false + rgbd_camera_optical_frame: + Value: true + world: + Value: false + world_moveit_calibration: + Value: false + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + world_moveit_calibration: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.25 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: false + Show Trail: false + State Display Time: 0.1s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 0.5 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.10000000149011612 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 0.5 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - ArUco dictionary: DICT_5X5_250 + Camera FOV Marker: + Marker Alpha: 0.30000001192092896 + Marker Size: 1.5 + Value: true + Class: moveit_rviz_plugin/HandEyeCalibration + Enabled: true + Move Group Namespace: "" + Name: HandEyeCalibration + Planning Scene Topic: /monitored_planning_scene + Rx: 0 + Ry: 0 + Rz: 0 + Tx: 0 + Ty: 0 + Tz: 0 + Value: true + base: "" + eef: "" + group: arm + image_topic: /rgbd_camera/image + marker border (bits): 1 + marker separation (px): 20 + marker size (px): 200 + markers, X: 3 + markers, Y: 4 + measured marker size (m): 0.200000 + measured separation (m): 0.020000 + object: "" + sensor: "" + sensor_mount_type: 0 + solver: OpenCV/Daniilidis1998 + target_type: HandEyeTarget/Aruco + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.25 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.75 + Y: 0 + Z: 0.5 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5235999822616577 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + HandEye Calibration: + collapsed: false + Height: 1411 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Target Detection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 0 + Y: 0 diff --git a/moveit_calibration_demos/rviz/eye_to_hand_charuco.rviz b/moveit_calibration_demos/rviz/eye_to_hand_charuco.rviz new file mode 100644 index 0000000..2e2a04e --- /dev/null +++ b/moveit_calibration_demos/rviz/eye_to_hand_charuco.rviz @@ -0,0 +1,477 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /HandEyeCalibration1 + Splitter Ratio: 0.506147563457489 + Tree Height: 435 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Target Detection + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /handeye_calibration/target_detection + Value: true + - Alpha: 1 + Axes Length: 0.03999999910593033 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Camera Pose (Ground Truth) + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_pose_ground_truth + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + handeye_target: + Value: false + panda_hand: + Value: false + panda_hand_tcp: + Value: false + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + rgbd_camera/rgbd_camera_link/rgbd_camera: + Value: false + rgbd_camera_base_link: + Value: false + rgbd_camera_optical_frame: + Value: true + world: + Value: false + world_moveit_calibration: + Value: false + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + world_moveit_calibration: + {} + Update Interval: 0 + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.25 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: false + Show Trail: false + State Display Time: 0.1s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 0.5 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.10000000149011612 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 0.5 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - ArUco dictionary: DICT_5X5_250 + Camera FOV Marker: + Marker Alpha: 0.30000001192092896 + Marker Size: 1.5 + Value: true + Class: moveit_rviz_plugin/HandEyeCalibration + Enabled: true + Move Group Namespace: "" + Name: HandEyeCalibration + Planning Scene Topic: /monitored_planning_scene + Rx: 0 + Ry: 0 + Rz: 0 + Tx: 0 + Ty: 0 + Tz: 0 + Value: true + base: "" + eef: "" + group: arm + image_topic: /rgbd_camera/image + longest board side (m): 0.560000 + margin size (px): 2 + marker border (bits): 1 + marker size (px): 50 + measured marker size (m): 0.060000 + object: "" + sensor: "" + sensor_mount_type: 0 + solver: OpenCV/Daniilidis1998 + square size (px): 80 + squares, X: 5 + squares, Y: 7 + target_type: HandEyeTarget/Charuco + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.25 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.75 + Y: 0 + Z: 0.5 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5235999822616577 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + HandEye Calibration: + collapsed: false + Height: 1411 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Target Detection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 0 + Y: 0 diff --git a/moveit_calibration_demos/worlds/eye_in_hand_aruco.sdf b/moveit_calibration_demos/worlds/eye_in_hand_aruco.sdf new file mode 100644 index 0000000..e3fe91d --- /dev/null +++ b/moveit_calibration_demos/worlds/eye_in_hand_aruco.sdf @@ -0,0 +1,441 @@ + + + + + + + + 0.004 + 1.0 + + + + + 0.2 0.2 0.2 + false + + + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.2 0.2 0.2 + 1.0 1.0 1.0 + 1.0 0.5 1.0 0.0 0.5236 -2.3562 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 100 + 50 + floating + false + #777777 + + + + + + rgbd_camera/image + false + + RGB Image + true + false + false + false + false + 240 + 200 + floating + + + + + + + + rgbd_camera/depth_image + false + + Depth Map + true + false + false + false + false + 240 + 200 + floating + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 2.5 + 0.01 + 0.001 + + -1.0 1.0 -1.0 + + + + + + + true + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + model://panda + + + panda_link7 + rgbd_camera + rgbd_camera_link + + + + + + true + 0.105 0.02 0.855 0 1.5708 0.7854 + + + -0.005 0 0 0 1.5708 0 + + + 0.01 + 0.01 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + -0.025 0 0 0 0 0 + + + 0.03 0.025 0.025 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + 0.06 + + 0.0004 + 0.00145 + 0.00145 + + + + + 1.0472 + + + 554.25469 + 554.25469 + 320.5 + 240.5 + 0 + + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + + 0.1 + 10 + + + + gaussian + 0 + 0.007 + + + 1 + 30 + true + rgbd_camera + + + + + true + false + false + false + + + + + + https://fuel.gazebosim.org/1.0/AndrejOrsula/models/aruco_default + 0.33 0.0 0.001 0 0 0 + + + + diff --git a/moveit_calibration_demos/worlds/eye_in_hand_charuco.sdf b/moveit_calibration_demos/worlds/eye_in_hand_charuco.sdf new file mode 100644 index 0000000..7d3699f --- /dev/null +++ b/moveit_calibration_demos/worlds/eye_in_hand_charuco.sdf @@ -0,0 +1,441 @@ + + + + + + + + 0.004 + 1.0 + + + + + 0.2 0.2 0.2 + false + + + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.2 0.2 0.2 + 1.0 1.0 1.0 + 1.0 0.5 1.0 0.0 0.5236 -2.3562 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 100 + 50 + floating + false + #777777 + + + + + + rgbd_camera/image + false + + RGB Image + true + false + false + false + false + 240 + 200 + floating + + + + + + + + rgbd_camera/depth_image + false + + Depth Map + true + false + false + false + false + 240 + 200 + floating + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 2.5 + 0.01 + 0.001 + + -1.0 1.0 -1.0 + + + + + + + true + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + model://panda + + + panda_link7 + rgbd_camera + rgbd_camera_link + + + + + + true + 0.105 0.02 0.855 0 1.5708 0.7854 + + + -0.005 0 0 0 1.5708 0 + + + 0.01 + 0.01 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + -0.025 0 0 0 0 0 + + + 0.03 0.025 0.025 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + 0.06 + + 0.0004 + 0.00145 + 0.00145 + + + + + 1.0472 + + + 554.25469 + 554.25469 + 320.5 + 240.5 + 0 + + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + + 0.1 + 10 + + + + gaussian + 0 + 0.007 + + + 1 + 30 + true + rgbd_camera + + + + + true + false + false + false + + + + + + https://fuel.gazebosim.org/1.0/AndrejOrsula/models/charuco_default + 0.33 0.0 0.001 0 0 0 + + + + diff --git a/moveit_calibration_demos/worlds/eye_to_hand_aruco.sdf b/moveit_calibration_demos/worlds/eye_to_hand_aruco.sdf new file mode 100644 index 0000000..51be78b --- /dev/null +++ b/moveit_calibration_demos/worlds/eye_to_hand_aruco.sdf @@ -0,0 +1,441 @@ + + + + + + + + 0.004 + 1.0 + + + + + 0.2 0.2 0.2 + false + + + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.2 0.2 0.2 + 1.0 1.0 1.0 + 1.5 0.5 1.0 0.0 0.5236 -2.3562 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 100 + 50 + floating + false + #777777 + + + + + + rgbd_camera/image + false + + RGB Image + true + false + false + false + false + 240 + 200 + floating + + + + + + + + rgbd_camera/depth_image + false + + Depth Map + true + false + false + false + false + 240 + 200 + floating + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 2.5 + 0.01 + 0.001 + + -1.0 1.0 -1.0 + + + + + + + true + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + model://panda + + + panda_link7 + aruco_default + aruco_link + + + + + + true + 1.2 0.0 0.5 0 0.0 3.1416 + + + -0.005 0 0 0 1.5708 0 + + + 0.01 + 0.01 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + -0.025 0 0 0 0 0 + + + 0.03 0.025 0.025 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + 0.06 + + 0.0004 + 0.00145 + 0.00145 + + + + + 1.0472 + + + 554.25469 + 554.25469 + 320.5 + 240.5 + 0 + + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + + 0.1 + 10 + + + + gaussian + 0 + 0.007 + + + 1 + 30 + true + rgbd_camera + + + + + true + false + false + false + + + + + + https://fuel.gazebosim.org/1.0/AndrejOrsula/models/aruco_default + 0.12 0.04 0.92 0 1.5708 0.7854 + + + + diff --git a/moveit_calibration_demos/worlds/eye_to_hand_charuco.sdf b/moveit_calibration_demos/worlds/eye_to_hand_charuco.sdf new file mode 100644 index 0000000..9c9191c --- /dev/null +++ b/moveit_calibration_demos/worlds/eye_to_hand_charuco.sdf @@ -0,0 +1,441 @@ + + + + + + + + 0.004 + 1.0 + + + + + 0.2 0.2 0.2 + false + + + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.2 0.2 0.2 + 1.0 1.0 1.0 + 1.5 0.5 1.0 0.0 0.5236 -2.3562 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 100 + 50 + floating + false + #777777 + + + + + + rgbd_camera/image + false + + RGB Image + true + false + false + false + false + 240 + 200 + floating + + + + + + + + rgbd_camera/depth_image + false + + Depth Map + true + false + false + false + false + 240 + 200 + floating + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 2.5 + 0.01 + 0.001 + + -1.0 1.0 -1.0 + + + + + + + true + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + + + + 0.0 0.0 1.0 + 4.0 4.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + model://panda + + + panda_link7 + charuco_default + charuco_link + + + + + + true + 1.2 0.0 0.5 0 0.0 3.1416 + + + -0.005 0 0 0 1.5708 0 + + + 0.01 + 0.01 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + -0.025 0 0 0 0 0 + + + 0.03 0.025 0.025 + + + + 0.0 0.8 0.0 + 0.0 0.8 0.0 + 0.0 0.8 0.0 + + + + 0.06 + + 0.0004 + 0.00145 + 0.00145 + + + + + 1.0472 + + + 554.25469 + 554.25469 + 320.5 + 240.5 + 0 + + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + + 0.1 + 10 + + + + gaussian + 0 + 0.007 + + + 1 + 30 + true + rgbd_camera + + + + + true + false + false + false + + + + + + https://fuel.gazebosim.org/1.0/AndrejOrsula/models/charuco_default + 0.12 0.04 0.92 0 1.5708 0.7854 + + + + diff --git a/moveit_calibration_gui/CHANGELOG.rst b/moveit_calibration_gui/CHANGELOG.rst index e0ab544..cce31a7 100644 --- a/moveit_calibration_gui/CHANGELOG.rst +++ b/moveit_calibration_gui/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_calibration_gui ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.1.0 (forthcoming) +2.0.0 (forthcoming) +----------- +* First public release for ROS 2 + +0.1.1 +----------- +* Calculate and report reprojection error + +0.1.0 ----------- * First public release for Noetic diff --git a/moveit_calibration_gui/CMakeLists.txt b/moveit_calibration_gui/CMakeLists.txt index fc57d6f..bb528c7 100644 --- a/moveit_calibration_gui/CMakeLists.txt +++ b/moveit_calibration_gui/CMakeLists.txt @@ -1,88 +1,53 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.10.2) project(moveit_calibration_gui) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_EXTENSIONS OFF) +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -find_package(catkin REQUIRED COMPONENTS - class_loader +# find dependencies +find_package(ament_cmake_ros REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(moveit_calibration_plugins REQUIRED) +find_package(moveit_ros_visualization REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_visual_tools REQUIRED) +find_package(OpenCV REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_visual_tools REQUIRED) +find_package(tf2_eigen REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS cv_bridge - geometric_shapes + Eigen3 image_geometry + image_transport moveit_calibration_plugins - moveit_core - moveit_ros_perception - moveit_ros_planning - moveit_ros_planning_interface moveit_ros_visualization + moveit_ros_planning_interface moveit_visual_tools pluginlib - rosconsole - roscpp - rviz + rclcpp + rviz_common rviz_visual_tools tf2_eigen ) -find_package(Eigen3 REQUIRED) - -# Qt Stuff -if(rviz_QT_VERSION VERSION_LESS "5") - find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) - include(${QT_USE_FILE}) - macro(qt_wrap_ui) - qt4_wrap_ui(${ARGN}) - endmacro() -else() - find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Widgets) - macro(qt_wrap_ui) - qt5_wrap_ui(${ARGN}) - endmacro() -endif() - -set(CMAKE_INCLUDE_CURRENT_DIR ON) -set(CMAKE_AUTOMOC ON) -add_definitions(-DQT_NO_KEYWORDS) - -catkin_package( - LIBRARIES - moveit_handeye_calibration_rviz_plugin_core - INCLUDE_DIRS - handeye_calibration_rviz_plugin/include - CATKIN_DEPENDS - moveit_calibration_plugins - moveit_core - moveit_ros_perception - moveit_ros_planning - moveit_ros_planning_interface - moveit_visual_tools - roscpp - rviz - rviz_visual_tools - DEPENDS - EIGEN3 -) - -include_directories( - handeye_calibration_rviz_plugin/include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) - -include_directories(SYSTEM - ${EIGEN3_INCLUDE_DIRS} - ${QT_INCLUDE_DIR}) - +# Add project sub-libraries add_subdirectory(handeye_calibration_rviz_plugin) -install(FILES +# Export plugin descriptions to register with pluginlib +pluginlib_export_plugin_description_file( + rviz_common handeye_calibration_rviz_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +) -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) -endif() +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt b/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt index 6e55874..292078b 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/CMakeLists.txt @@ -1,37 +1,72 @@ -find_package(OpenCV REQUIRED) - +set(MOVEIT_LIB_NAME moveit_handeye_calibration_rviz_plugin) set(HEADERS include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_display.h include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_frame.h - include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h + include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h ) - -#catkin_lint: ignore_once missing_directory -include_directories(${CMAKE_CURRENT_BINARY_DIR}) - -# Plugin Source -set(SOURCE_FILES +set(SOURCE_FILES_CORE src/handeye_calibration_display.cpp src/handeye_calibration_frame.cpp - src/handeye_target_widget.cpp src/handeye_context_widget.cpp src/handeye_control_widget.cpp + src/handeye_target_widget.cpp +) +set(SOURCE_FILES_PLUGINS + src/plugin_init.cpp ) -set(MOVEIT_LIB_NAME moveit_handeye_calibration_rviz_plugin) -add_library(${MOVEIT_LIB_NAME}_core ${SOURCE_FILES} ${HEADERS}) +# Qt Stuff +find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +macro(qt_wrap_ui) + qt5_wrap_ui(${ARGN}) +endmacro() + +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTORCC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +# Core library +# TODO: Remove headers from moveit_handeye_calibration_rviz_plugin library compilation +add_library(${MOVEIT_LIB_NAME}_core SHARED ${SOURCE_FILES_CORE} ${HEADERS}) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME}_core - ${catkin_LIBRARIES} ${OpenCV_LIBS} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME}_core ${OpenCV_LIBS}) +target_include_directories(${MOVEIT_LIB_NAME}_core PUBLIC + $ + $) +ament_target_dependencies( + ${MOVEIT_LIB_NAME}_core + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) -add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) +# Plugin library +add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCE_FILES_PLUGINS}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC + $ + $) +ament_target_dependencies( + ${MOVEIT_LIB_NAME} + pluginlib +) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) +install( + TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +ament_export_include_directories( + include +) +ament_export_libraries( + {MOVEIT_LIB_NAME}_core {MOVEIT_LIB_NAME} +) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_display.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_display.h index 4006bef..e926fc2 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_display.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_display.h @@ -37,38 +37,39 @@ #pragma once // ros -#include +#include // local #include #ifndef Q_MOC_RUN -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #endif namespace moveit_rviz_plugin { -class HandEyeCalibrationDisplay : public rviz::Display +class HandEyeCalibrationFrame; +class HandEyeCalibrationDisplay : public rviz_common::Display { Q_OBJECT public: explicit HandEyeCalibrationDisplay(QWidget* parent = 0); ~HandEyeCalibrationDisplay() override; - void load(const rviz::Config& config) override; - void save(rviz::Config config) const override; + void load(const rviz_common::Config& config) override; + void save(rviz_common::Config config) const override; - rviz::StringProperty* move_group_ns_property_; - rviz::RosTopicProperty* planning_scene_topic_property_; - rviz::BoolProperty* fov_marker_enabled_property_; - rviz::FloatProperty* fov_marker_alpha_property_; - rviz::FloatProperty* fov_marker_size_property_; + rviz_common::properties::StringProperty* move_group_ns_property_; + rviz_common::properties::RosTopicProperty* planning_scene_topic_property_; + rviz_common::properties::BoolProperty* fov_marker_enabled_property_; + rviz_common::properties::FloatProperty* fov_marker_alpha_property_; + rviz_common::properties::FloatProperty* fov_marker_size_property_; private Q_SLOTS: @@ -82,7 +83,7 @@ private Q_SLOTS: void onInitialize() override; private: - rviz::PanelDockWidget* frame_dock_; + rviz_common::PanelDockWidget* frame_dock_; HandEyeCalibrationFrame* frame_; }; diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_frame.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_frame.h index a6f1381..75a08ca 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_frame.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_calibration_frame.h @@ -39,7 +39,7 @@ // qt // ros -#include +#include // local #include @@ -48,9 +48,9 @@ #include #ifndef Q_MOC_RUN -#include -#include -#include +#include +// #include Do we need this? +#include #endif namespace moveit_rviz_plugin @@ -66,12 +66,12 @@ class HandEyeCalibrationFrame : public QWidget Q_OBJECT public: - explicit HandEyeCalibrationFrame(HandEyeCalibrationDisplay* pdisplay, rviz::DisplayContext* context, + explicit HandEyeCalibrationFrame(HandEyeCalibrationDisplay* pdisplay, rviz_common::DisplayContext* context, QWidget* parent = 0); ~HandEyeCalibrationFrame() override; - virtual void loadWidget(const rviz::Config& config); - virtual void saveWidget(rviz::Config config) const; + virtual void loadWidget(const rviz_common::Config& config); + virtual void saveWidget(rviz_common::Config& config) const; protected: // ****************************************************************************************** @@ -81,12 +81,15 @@ class HandEyeCalibrationFrame : public QWidget TargetTabWidget* tab_target_; ContextTabWidget* tab_context_; ControlTabWidget* tab_control_; + rclcpp::executors::MultiThreadedExecutor executor_; + std::thread executor_thread_; private: - rviz::DisplayContext* context_; + rviz_common::DisplayContext* context_; HandEyeCalibrationDisplay* calibration_display_; rviz_visual_tools::TFVisualToolsPtr tf_tools_; + rclcpp::Node::SharedPtr node_; }; } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h index 58747eb..1f8fab4 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_context_widget.h @@ -48,21 +48,25 @@ #include // ros -#include -#include +#include +#include #include -#include +#include #include -#include -#include +#include +#include #include #include #include #include +#include +#include +#include +#include #ifndef Q_MOC_RUN -#include -#include +#include +#include #endif namespace rvt = rviz_visual_tools; @@ -86,10 +90,11 @@ class TFFrameNameComboBox : public QComboBox { Q_OBJECT public: - TFFrameNameComboBox(FRAME_SOURCE source = ROBOT_FRAME, QWidget* parent = 0) : QComboBox(parent), frame_source_(source) + TFFrameNameComboBox(rviz_common::DisplayContext* context, rclcpp::Node::SharedPtr& node, + FRAME_SOURCE source = ROBOT_FRAME, QWidget* parent = 0) + : QComboBox(parent), frame_source_(source), context_(context), node_(node) { - robot_model_loader_.reset(new robot_model_loader::RobotModelLoader("robot_description")); - frame_manager_.reset(new rviz::FrameManager()); + robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(node_, "robot_description")); } ~TFFrameNameComboBox() @@ -104,7 +109,8 @@ class TFFrameNameComboBox : public QComboBox private: FRAME_SOURCE frame_source_; - std::unique_ptr frame_manager_; + rclcpp::Node::SharedPtr node_; + rviz_common::DisplayContext* context_; robot_model_loader::RobotModelLoaderConstPtr robot_model_loader_; }; @@ -151,7 +157,8 @@ class ContextTabWidget : public QWidget { Q_OBJECT public: - explicit ContextTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* parent = Q_NULLPTR); + explicit ContextTabWidget(rclcpp::Node::SharedPtr node, HandEyeCalibrationDisplay* pdisplay, + rviz_common::DisplayContext* context, QWidget* parent = Q_NULLPTR); ~ContextTabWidget() { camera_info_.reset(); @@ -159,27 +166,28 @@ class ContextTabWidget : public QWidget tf_tools_.reset(); } - void loadWidget(const rviz::Config& config); - void saveWidget(rviz::Config& config); + void loadWidget(const rviz_common::Config& config); + void saveWidget(rviz_common::Config& config); void setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub); void updateAllMarkers(); void updateFOVPose(); - static shape_msgs::Mesh getCameraFOVMesh(const sensor_msgs::CameraInfo& camera_info, double maxdist); + static shape_msgs::msg::Mesh getCameraFOVMesh(const sensor_msgs::msg::CameraInfo& camera_info, double maxdist); - visualization_msgs::Marker getCameraFOVMarker(const Eigen::Isometry3d& pose, const shape_msgs::Mesh& mesh, - rvt::colors color, double alpha, std::string frame_id); + visualization_msgs::msg::Marker getCameraFOVMarker(const Eigen::Isometry3d& pose, const shape_msgs::msg::Mesh& mesh, + rvt::Colors color, double alpha, std::string frame_id); - visualization_msgs::Marker getCameraFOVMarker(const geometry_msgs::Pose& pose, const shape_msgs::Mesh& mesh, - rvt::colors color, double alpha, std::string frame_id); + visualization_msgs::msg::Marker getCameraFOVMarker(const geometry_msgs::msg::Pose& pose, + const shape_msgs::msg::Mesh& mesh, rvt::Colors color, double alpha, + std::string frame_id); void setCameraPose(double tx, double ty, double tz, double rx, double ry, double rz); public Q_SLOTS: - void setCameraInfo(sensor_msgs::CameraInfo camera_info); + void setCameraInfo(sensor_msgs::msg::CameraInfo camera_info); void setOpticalFrame(const std::string& frame_id); @@ -222,7 +230,7 @@ private Q_SLOTS: // Variables // ************************************************************** - sensor_msgs::CameraInfoPtr camera_info_; + sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; // Transform from camera to robot base or end-effector Eigen::Isometry3d camera_pose_; @@ -236,10 +244,12 @@ private Q_SLOTS: // Ros components // ************************************************************** + rclcpp::Node::SharedPtr node_; + rviz_common::DisplayContext* context_; moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; rviz_visual_tools::TFVisualToolsPtr tf_tools_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; }; } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h index f2a0a3b..7c039d0 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h @@ -58,16 +58,16 @@ #include #include #include -#include +#include #include #include #include -#include +#include #include #ifndef Q_MOC_RUN -#include -#include +#include +#include #endif #include @@ -114,7 +114,8 @@ class ControlTabWidget : public QWidget }; public: - explicit ControlTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* parent = Q_NULLPTR); + explicit ControlTabWidget(rclcpp::Node::SharedPtr node, HandEyeCalibrationDisplay* pdisplay, + QWidget* parent = Q_NULLPTR); ~ControlTabWidget() { tf_tools_.reset(); @@ -125,13 +126,13 @@ class ControlTabWidget : public QWidget planning_scene_monitor_.reset(); } - void loadWidget(const rviz::Config& config); - void saveWidget(rviz::Config& config); + void loadWidget(const rviz_common::Config& config); + void saveWidget(rviz_common::Config& config); void setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub); - void addPoseSampleToTreeView(const geometry_msgs::TransformStamped& camera_to_object_tf, - const geometry_msgs::TransformStamped& base_to_eef_tf, int id); + void addPoseSampleToTreeView(const geometry_msgs::msg::TransformStamped& camera_to_object_tf, + const geometry_msgs::msg::TransformStamped& base_to_eef_tf, int id); bool loadSolverPlugin(std::vector& plugins); @@ -255,10 +256,7 @@ private Q_SLOTS: // ************************************************************** // Ros components // ************************************************************** - - ros::NodeHandle nh_; - // ros::CallbackQueue callback_queue_; - // ros::AsyncSpinner spinner_; + rclcpp::Node::SharedPtr node_; std::shared_ptr tf_buffer_; tf2_ros::TransformListener tf_listener_; rviz_visual_tools::TFVisualToolsPtr tf_tools_; @@ -267,6 +265,19 @@ private Q_SLOTS: planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; moveit::planning_interface::MoveGroupInterfacePtr move_group_; moveit::planning_interface::MoveGroupInterface::PlanPtr current_plan_; + + // ************************************************************** + // Launch script exporters + // ************************************************************** + std::stringstream generateCalibrationPython(std::string& from_frame, std::string& to_frame, Eigen::Vector3d& t, + Eigen::Quaterniond& r_quat, Eigen::Vector3d& r_euler, + std::string& mount_type); + std::stringstream generateCalibrationXml(std::string& from_frame, std::string& to_frame, Eigen::Vector3d& t, + Eigen::Quaterniond& r_quat, Eigen::Vector3d& r_euler, + std::string& mount_type); + std::stringstream generateCalibrationYaml(std::string& from_frame, std::string& to_frame, Eigen::Vector3d& t, + Eigen::Quaterniond& r_quat, Eigen::Vector3d& r_euler, + std::string& mount_type); }; } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h index b7eb8a0..aa35afa 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h @@ -57,24 +57,25 @@ #include // ros -#include -#include -#include +#include +#include +#include #include -#include +#include #include -#include +#include #include #include #ifndef Q_MOC_RUN -#include -#include +#include +#include +#include #endif -Q_DECLARE_METATYPE(sensor_msgs::CameraInfo); +Q_DECLARE_METATYPE(sensor_msgs::msg::CameraInfo); Q_DECLARE_METATYPE(std::string); namespace moveit_rviz_plugin @@ -88,7 +89,7 @@ class RosTopicComboBox : public QComboBox { Q_OBJECT public: - explicit RosTopicComboBox(QWidget* parent = Q_NULLPTR) : QComboBox(parent) + explicit RosTopicComboBox(rclcpp::Node::SharedPtr node, QWidget* parent = Q_NULLPTR) : node_(node), QComboBox(parent) { } ~RosTopicComboBox() = default; @@ -104,13 +105,17 @@ class RosTopicComboBox : public QComboBox QSet message_types_; QSet image_topics_; + +private: + rclcpp::Node::SharedPtr node_; }; class TargetTabWidget : public QWidget { Q_OBJECT public: - explicit TargetTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* parent = Q_NULLPTR); + explicit TargetTabWidget(rclcpp::Node::SharedPtr node, HandEyeCalibrationDisplay* pdisplay, + QWidget* parent = Q_NULLPTR); ~TargetTabWidget() { target_.reset(); @@ -118,8 +123,8 @@ class TargetTabWidget : public QWidget camera_info_.reset(); } - void loadWidget(const rviz::Config& config); - void saveWidget(rviz::Config& config); + void loadWidget(const rviz_common::Config& config); + void saveWidget(rviz_common::Config& config); bool loadAvailableTargetPlugins(); @@ -127,10 +132,12 @@ class TargetTabWidget : public QWidget void fillDictionaryIds(std::string id = ""); - void imageCallback(const sensor_msgs::ImageConstPtr& msg); + void cameraCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info); - void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg); + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + void cameraInfoCallback(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg); private Q_SLOTS: // Called when the current item of target_type_ changed @@ -148,12 +155,9 @@ private Q_SLOTS: // Called when the item of image_topic_field_ combobox is selected void imageTopicComboboxChanged(const QString& topic); - // Called when the item of camera_info_topic_field_ combobox is selected - void cameraInfoComboBoxChanged(const QString& topic); - Q_SIGNALS: - void cameraInfoChanged(sensor_msgs::CameraInfo msg); + void cameraInfoChanged(sensor_msgs::msg::CameraInfo msg); void opticalFrameChanged(const std::string& frame_id); @@ -188,20 +192,20 @@ private Q_SLOTS: std::string optical_frame_; - sensor_msgs::CameraInfoConstPtr camera_info_; + sensor_msgs::msg::CameraInfo::ConstPtr camera_info_; // ************************************************************** // Ros components // ************************************************************** - ros::NodeHandle nh_; + rclcpp::Node::SharedPtr node_; std::unique_ptr > target_plugins_loader_; pluginlib::UniquePtr target_; image_transport::ImageTransport it_; - image_transport::Subscriber image_sub_; + image_transport::CameraSubscriber camera_sub_; image_transport::Publisher image_pub_; - ros::Subscriber camerainfo_sub_; + // tf broadcaster - tf2_ros::TransformBroadcaster tf_pub_; + std::shared_ptr tf_pub_; }; } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_display.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_display.cpp index f83760e..8261109 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_display.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_display.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include @@ -48,24 +48,25 @@ namespace moveit_rviz_plugin { HandEyeCalibrationDisplay::HandEyeCalibrationDisplay(QWidget* parent) : Display() { - move_group_ns_property_ = new rviz::StringProperty("Move Group Namespace", "", - "The name of the ROS namespace in " - "which the move_group node is running", - this, SLOT(fillPlanningGroupNameComboBox()), this); - planning_scene_topic_property_ = - new rviz::RosTopicProperty("Planning Scene Topic", "move_group/monitored_planning_scene", - ros::message_traits::datatype(), - "The topic on which the moveit_msgs::PlanningScene messages are received", this, - SLOT(fillPlanningGroupNameComboBox()), this); - - fov_marker_enabled_property_ = new rviz::BoolProperty( + move_group_ns_property_ = + new rviz_common::properties::StringProperty("Move Group Namespace", "", + "The name of the ROS namespace in " + "which the move_group node is running", + this, SLOT(fillPlanningGroupNameComboBox()), this); + planning_scene_topic_property_ = new rviz_common::properties::RosTopicProperty( + "Planning Scene Topic", "/monitored_planning_scene", + rosidl_generator_traits::data_type(), + "The topic on which the moveit_msgs::msg::PlanningScene messages are received", this, + SLOT(fillPlanningGroupNameComboBox()), this); + + fov_marker_enabled_property_ = new rviz_common::properties::BoolProperty( "Camera FOV Marker", true, "Enable marker showing camera field of view", this, SLOT(updateMarkers()), this); - fov_marker_alpha_property_ = - new rviz::FloatProperty("Marker Alpha", 0.3f, "Specifies the alpha (transparency) for the rendered marker", - fov_marker_enabled_property_, SLOT(updateMarkers()), this); - fov_marker_size_property_ = - new rviz::FloatProperty("Marker Size", 1.5f, "Specifies the size (depth in meters) for the rendered marker", - fov_marker_enabled_property_, SLOT(updateMarkers()), this); + fov_marker_alpha_property_ = new rviz_common::properties::FloatProperty( + "Marker Alpha", 0.3f, "Specifies the alpha (transparency) for the rendered marker", fov_marker_enabled_property_, + SLOT(updateMarkers()), this); + fov_marker_size_property_ = new rviz_common::properties::FloatProperty( + "Marker Size", 1.5f, "Specifies the size (depth in meters) for the rendered marker", fov_marker_enabled_property_, + SLOT(updateMarkers()), this); } HandEyeCalibrationDisplay::~HandEyeCalibrationDisplay() @@ -78,7 +79,7 @@ void HandEyeCalibrationDisplay::onInitialize() { Display::onInitialize(); - rviz::WindowManagerInterface* window_context = context_->getWindowManager(); + rviz_common::WindowManagerInterface* window_context = context_->getWindowManager(); frame_ = new HandEyeCalibrationFrame(this, context_, window_context ? window_context->getParentWindow() : nullptr); if (window_context) @@ -87,7 +88,7 @@ void HandEyeCalibrationDisplay::onInitialize() } } -void HandEyeCalibrationDisplay::save(rviz::Config config) const +void HandEyeCalibrationDisplay::save(rviz_common::Config config) const { Display::save(config); if (frame_) @@ -97,7 +98,7 @@ void HandEyeCalibrationDisplay::save(rviz::Config config) const } // Load all configuration data for this panel from the given Config object. -void HandEyeCalibrationDisplay::load(const rviz::Config& config) +void HandEyeCalibrationDisplay::load(const rviz_common::Config& config) { Display::load(config); if (frame_) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_frame.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_frame.cpp index 15d8fb0..d45757a 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_frame.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_frame.cpp @@ -38,14 +38,16 @@ #include #include +#include #include namespace moveit_rviz_plugin { -HandEyeCalibrationFrame::HandEyeCalibrationFrame(HandEyeCalibrationDisplay* pdisplay, rviz::DisplayContext* context, - QWidget* parent) +HandEyeCalibrationFrame::HandEyeCalibrationFrame(HandEyeCalibrationDisplay* pdisplay, + rviz_common::DisplayContext* context, QWidget* parent) : QWidget(parent), calibration_display_(pdisplay), context_(context) { + node_ = std::make_shared("handeye_calibration_frame"); setMinimumSize(695, 460); // Basic widget container QVBoxLayout* layout = new QVBoxLayout(); @@ -60,18 +62,18 @@ HandEyeCalibrationFrame::HandEyeCalibrationFrame(HandEyeCalibrationDisplay* pdis // Tab menu ------------------------------------------------------------ QTabWidget* tabs = new QTabWidget(this); - tab_target_ = new TargetTabWidget(calibration_display_); + tab_target_ = new TargetTabWidget(node_, calibration_display_); - tf_tools_.reset(new rviz_visual_tools::TFVisualTools(250)); + tf_tools_.reset(new rviz_visual_tools::TFVisualTools(node_, 250)); - tab_context_ = new ContextTabWidget(calibration_display_); + tab_context_ = new ContextTabWidget(node_, calibration_display_, context_); tab_context_->setTFTool(tf_tools_); - connect(tab_target_, SIGNAL(cameraInfoChanged(sensor_msgs::CameraInfo)), tab_context_, - SLOT(setCameraInfo(sensor_msgs::CameraInfo))); + connect(tab_target_, SIGNAL(cameraInfoChanged(sensor_msgs::msg::CameraInfo)), tab_context_, + SLOT(setCameraInfo(sensor_msgs::msg::CameraInfo))); connect(tab_target_, SIGNAL(opticalFrameChanged(const std::string&)), tab_context_, SLOT(setOpticalFrame(const std::string&))); - tab_control_ = new ControlTabWidget(calibration_display_); + tab_control_ = new ControlTabWidget(node_, calibration_display_); tab_control_->setTFTool(tf_tools_); tab_control_->UpdateSensorMountType(0); connect(tab_context_, SIGNAL(sensorMountTypeChanged(int)), tab_control_, SLOT(UpdateSensorMountType(int))); @@ -85,12 +87,22 @@ HandEyeCalibrationFrame::HandEyeCalibrationFrame(HandEyeCalibrationDisplay* pdis tabs->addTab(tab_control_, "Calibrate"); layout->addWidget(tabs); - ROS_INFO_STREAM("handeye calibration gui created."); + // Spin node in the background for sub callbacks + executor_.add_node(node_); + auto spin = [this]() { + while (rclcpp::ok()) + { + executor_.spin_once(); + } + }; + executor_thread_ = std::thread(spin); + + RCLCPP_INFO_STREAM(node_->get_logger(), "handeye calibration gui created."); } HandEyeCalibrationFrame::~HandEyeCalibrationFrame() = default; -void HandEyeCalibrationFrame::saveWidget(rviz::Config config) const +void HandEyeCalibrationFrame::saveWidget(rviz_common::Config& config) const { tab_target_->saveWidget(config); tab_context_->saveWidget(config); @@ -98,13 +110,13 @@ void HandEyeCalibrationFrame::saveWidget(rviz::Config config) const } // Load all configuration data for this panel from the given Config object. -void HandEyeCalibrationFrame::loadWidget(const rviz::Config& config) +void HandEyeCalibrationFrame::loadWidget(const rviz_common::Config& config) { tab_target_->loadWidget(config); tab_context_->loadWidget(config); tab_control_->loadWidget(config); - ROS_INFO_STREAM("handeye calibration gui loaded."); + RCLCPP_INFO_STREAM(node_->get_logger(), "handeye calibration gui loaded."); } } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp index 7779bb9..82866e4 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp @@ -39,13 +39,10 @@ namespace moveit_rviz_plugin { -const std::string LOGNAME = "handeye_context_widget"; - void TFFrameNameComboBox::mousePressEvent(QMouseEvent* event) { - std::vector names; - frame_manager_->update(); - frame_manager_->getTF2BufferPtr()->_getFrameStrings(names); + context_->getFrameManager()->update(); + std::vector names = context_->getFrameManager()->getAllFrameNames(); clear(); addItem(QString("")); @@ -76,8 +73,8 @@ void TFFrameNameComboBox::mousePressEvent(QMouseEvent* event) bool TFFrameNameComboBox::hasFrame(const std::string& frame_name) { std::vector names; - frame_manager_->update(); - frame_manager_->getTF2BufferPtr()->_getFrameStrings(names); + context_->getFrameManager()->update(); + moveit::planning_interface::getSharedTF()->_getFrameStrings(names); auto it = std::find(names.begin(), names.end(), frame_name); return it != names.end(); @@ -154,8 +151,14 @@ void SliderWidget::changeSlider() Q_EMIT valueChanged(value); } -ContextTabWidget::ContextTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* parent) - : QWidget(parent), calibration_display_(pdisplay), tf_listener_(tf_buffer_) +ContextTabWidget::ContextTabWidget(rclcpp::Node::SharedPtr node, HandEyeCalibrationDisplay* pdisplay, + rviz_common::DisplayContext* context, QWidget* parent) + : QWidget(parent) + , node_(node) + , context_(context) + , calibration_display_(pdisplay) + , tf_buffer_(std::make_shared(node_->get_clock())) + , tf_listener_(std::make_shared(*tf_buffer_)) { // Context setting tab ---------------------------------------------------- QHBoxLayout* layout = new QHBoxLayout(); @@ -183,23 +186,23 @@ ContextTabWidget::ContextTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* QFormLayout* frame_layout = new QFormLayout(); frame_group->setLayout(frame_layout); - frames_.insert(std::make_pair("sensor", new TFFrameNameComboBox(CAMERA_FRAME))); + frames_.insert(std::make_pair("sensor", new TFFrameNameComboBox(context_, node_, CAMERA_FRAME))); frame_layout->addRow("Sensor frame:", frames_["sensor"]); - frames_.insert(std::make_pair("object", new TFFrameNameComboBox(ENVIRONMENT_FRAME))); + frames_.insert(std::make_pair("object", new TFFrameNameComboBox(context_, node_, ENVIRONMENT_FRAME))); frame_layout->addRow("Object frame:", frames_["object"]); - frames_.insert(std::make_pair("eef", new TFFrameNameComboBox(ROBOT_FRAME))); + frames_.insert(std::make_pair("eef", new TFFrameNameComboBox(context_, node_, ROBOT_FRAME))); frame_layout->addRow("End-effector frame:", frames_["eef"]); - frames_.insert(std::make_pair("base", new TFFrameNameComboBox(ROBOT_FRAME))); + frames_.insert(std::make_pair("base", new TFFrameNameComboBox(context_, node_, ROBOT_FRAME))); frame_layout->addRow("Robot base frame:", frames_["base"]); for (std::pair& frame : frames_) connect(frame.second, SIGNAL(activated(int)), this, SLOT(updateFrameName(int))); // Camera Pose initial guess area - QGroupBox* pose_group = new QGroupBox("Camera Pose Inital Guess", this); + QGroupBox* pose_group = new QGroupBox("Camera Pose Initial Guess", this); pose_group->setMinimumWidth(300); layout_right->addWidget(pose_group); QFormLayout* pose_layout = new QFormLayout(); @@ -234,19 +237,19 @@ ContextTabWidget::ContextTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* fov_pose_ = Eigen::Quaterniond(0.5, -0.5, 0.5, -0.5); fov_pose_.translate(Eigen::Vector3d(0.0149, 0.0325, 0.0125)); - camera_info_.reset(new sensor_msgs::CameraInfo()); + camera_info_.reset(new sensor_msgs::msg::CameraInfo()); - visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("world")); + visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools(node_, "world", "/moveit_visual_tools")); visual_tools_->enableFrameLocking(true); visual_tools_->setAlpha(1.0); visual_tools_->setLifetime(0.0); visual_tools_->trigger(); - calibration_display_->setStatus(rviz::StatusProperty::Warn, "Calibration context", + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Warn, "Calibration context", "Not all calibration frames have been selected."); } -void ContextTabWidget::loadWidget(const rviz::Config& config) +void ContextTabWidget::loadWidget(const rviz_common::Config& config) { int index; if (config.mapGetInt("sensor_mount_type", &index)) @@ -280,7 +283,7 @@ void ContextTabWidget::loadWidget(const rviz::Config& config) Q_EMIT frameNameChanged(names); } -void ContextTabWidget::saveWidget(rviz::Config& config) +void ContextTabWidget::saveWidget(rviz_common::Config& config) { config.mapSetValue("sensor_mount_type", sensor_mount_type_->currentIndex()); @@ -315,7 +318,7 @@ void ContextTabWidget::updateAllMarkers() from_frame = frames_["eef"]->currentText(); break; default: - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error sensor mount type."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error sensor mount type."); break; } @@ -347,7 +350,7 @@ void ContextTabWidget::updateAllMarkers() // Publish new FOV marker if (calibration_display_->fov_marker_enabled_property_->getBool()) { - shape_msgs::Mesh mesh = + shape_msgs::msg::Mesh mesh = getCameraFOVMesh(*camera_info_, calibration_display_->fov_marker_size_property_->getFloat()); visual_tools_->setBaseFrame(to_frame.toStdString()); visual_tools_->setAlpha(calibration_display_->fov_marker_alpha_property_->getFloat()); @@ -359,36 +362,37 @@ void ContextTabWidget::updateAllMarkers() visual_tools_->trigger(); } else - ROS_ERROR("Visual or TF tool is NULL."); + RCLCPP_ERROR(node_->get_logger(), "Visual or TF tool is NULL."); } void ContextTabWidget::updateFOVPose() { QString sensor_frame = frames_["sensor"]->currentText(); - geometry_msgs::TransformStamped tf_msg; + geometry_msgs::msg::TransformStamped tf_msg; if (!optical_frame_.empty() && !sensor_frame.isEmpty()) { try { // Get FOV pose W.R.T sensor frame - tf_msg = tf_buffer_.lookupTransform(sensor_frame.toStdString(), optical_frame_, ros::Time(0)); + tf_msg = tf_buffer_->lookupTransform(sensor_frame.toStdString(), optical_frame_, rclcpp::Time(0)); fov_pose_ = tf2::transformToEigen(tf_msg); - ROS_DEBUG_STREAM_NAMED(LOGNAME, "FOV pose from '" << sensor_frame.toStdString() << "' to '" << optical_frame_ - << "' is:" - << "\nTranslation:\n" - << fov_pose_.translation() << "\nRotation:\n" - << fov_pose_.rotation()); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "FOV pose from '" << sensor_frame.toStdString() << "' to '" + << optical_frame_ << "' is:" + << "\nTranslation:\n" + << fov_pose_.translation() << "\nRotation:\n" + << fov_pose_.rotation()); } catch (tf2::TransformException& e) { - ROS_WARN_STREAM("TF exception: " << e.what()); + RCLCPP_WARN_STREAM(node_->get_logger(), "TF exception: " << e.what()); } } } -shape_msgs::Mesh ContextTabWidget::getCameraFOVMesh(const sensor_msgs::CameraInfo& camera_info, double max_dist) +shape_msgs::msg::Mesh ContextTabWidget::getCameraFOVMesh(const sensor_msgs::msg::CameraInfo& camera_info, + double max_dist) { - shape_msgs::Mesh mesh; + shape_msgs::msg::Mesh mesh; image_geometry::PinholeCameraModel camera_model; camera_model.fromCameraInfo(camera_info); double delta_x = camera_model.getDeltaX(camera_info.width / 2, max_dist); @@ -400,13 +404,13 @@ shape_msgs::Mesh ContextTabWidget::getCameraFOVMesh(const sensor_msgs::CameraInf // Get corners mesh.vertices.clear(); // Add the first corner at origin of the optical frame - mesh.vertices.push_back(geometry_msgs::Point()); + mesh.vertices.push_back(geometry_msgs::msg::Point()); // Add the four corners at bottom for (const double& x_it : x_cords) for (const double& y_it : y_cords) { - geometry_msgs::Point vertex; + geometry_msgs::msg::Point vertex; // Check in case camera info is not valid if (std::isfinite(x_it) && std::isfinite(y_it) && std::isfinite(max_dist)) { @@ -426,24 +430,26 @@ shape_msgs::Mesh ContextTabWidget::getCameraFOVMesh(const sensor_msgs::CameraInf return mesh; } -visualization_msgs::Marker ContextTabWidget::getCameraFOVMarker(const Eigen::Isometry3d& pose, - const shape_msgs::Mesh& mesh, rvt::colors color, - double alpha, std::string frame_id) +visualization_msgs::msg::Marker ContextTabWidget::getCameraFOVMarker(const Eigen::Isometry3d& pose, + const shape_msgs::msg::Mesh& mesh, + rvt::Colors color, double alpha, + std::string frame_id) { return getCameraFOVMarker(rvt::RvizVisualTools::convertPose(pose), mesh, color, alpha, frame_id); } -visualization_msgs::Marker ContextTabWidget::getCameraFOVMarker(const geometry_msgs::Pose& pose, - const shape_msgs::Mesh& mesh, rvt::colors color, - double alpha, std::string frame_id) +visualization_msgs::msg::Marker ContextTabWidget::getCameraFOVMarker(const geometry_msgs::msg::Pose& pose, + const shape_msgs::msg::Mesh& mesh, + rvt::Colors color, double alpha, + std::string frame_id) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = frame_id; marker.ns = "camera_fov"; marker.id = 0; - marker.type = visualization_msgs::Marker::TRIANGLE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.lifetime = ros::Duration(0.0); + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration(0, 0); visual_tools_->setAlpha(alpha); marker.color = visual_tools_->getColor(color); marker.pose = pose; @@ -452,7 +458,7 @@ visualization_msgs::Marker ContextTabWidget::getCameraFOVMarker(const geometry_m marker.scale.z = 1.0; marker.points.clear(); - for (const shape_msgs::MeshTriangle& triangle : mesh.triangles) + for (const shape_msgs::msg::MeshTriangle& triangle : mesh.triangles) for (const uint32_t& index : triangle.vertex_indices) marker.points.push_back(mesh.vertices[index]); @@ -465,17 +471,17 @@ void ContextTabWidget::setCameraPose(double tx, double ty, double tz, double rx, camera_pose_ = visual_tools_->convertFromXYZRPY(tx, ty, tz, rx, ry, rz, rviz_visual_tools::XYZ); } -void ContextTabWidget::setCameraInfo(sensor_msgs::CameraInfo camera_info) +void ContextTabWidget::setCameraInfo(sensor_msgs::msg::CameraInfo camera_info) { camera_info_->header = camera_info.header; camera_info_->height = camera_info.height; camera_info_->width = camera_info.width; camera_info_->distortion_model = camera_info.distortion_model; - camera_info_->D = camera_info.D; - camera_info_->K = camera_info.K; - camera_info_->R = camera_info.R; - camera_info_->P = camera_info.P; - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Camera info changed: " << *camera_info_); + camera_info_->d = camera_info.d; + camera_info_->k = camera_info.k; + camera_info_->r = camera_info.r; + camera_info_->p = camera_info.p; + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Camera info changed: " << camera_info_); } void ContextTabWidget::setOpticalFrame(const std::string& frame_id) @@ -520,12 +526,12 @@ void ContextTabWidget::updateFrameName(int index) } if (any_empty) { - calibration_display_->setStatus(rviz::StatusProperty::Warn, "Calibration context", + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Warn, "Calibration context", "Not all calibration frames have been selected."); } else { - calibration_display_->setStatus(rviz::StatusProperty::Ok, "Calibration context", + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Ok, "Calibration context", "Calibration frames have been selected."); } diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 455b030..62011dc 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -36,11 +36,10 @@ #include #include +#include namespace moveit_rviz_plugin { -const std::string LOGNAME = "handeye_control_widget"; - ProgressBarWidget::ProgressBarWidget(QWidget* parent, int min, int max, int value) : QWidget(parent) { QHBoxLayout* row = new QHBoxLayout(this); @@ -95,11 +94,13 @@ int ProgressBarWidget::getValue() return bar_->value(); } -ControlTabWidget::ControlTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* parent) +ControlTabWidget::ControlTabWidget(rclcpp::Node::SharedPtr node, HandEyeCalibrationDisplay* pdisplay, QWidget* parent) : QWidget(parent) + , node_(node) , calibration_display_(pdisplay) - , tf_buffer_(new tf2_ros::Buffer()) - , tf_listener_(*tf_buffer_) + , tf_buffer_(new tf2_ros::Buffer(std::make_shared(RCL_ROS_TIME), + (tf2::Duration)(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), node_)) + , tf_listener_(*tf_buffer_, node_) , sensor_mount_type_(mhc::EYE_TO_HAND) , from_frame_tag_("base") , solver_plugins_loader_(nullptr) @@ -108,7 +109,6 @@ ControlTabWidget::ControlTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* , camera_robot_pose_(Eigen::Isometry3d::Identity()) , auto_started_(false) , planning_res_(ControlTabWidget::SUCCESS) -// spinner_(0, &callback_queue_) { QVBoxLayout* layout = new QVBoxLayout(); this->setLayout(layout); @@ -241,10 +241,11 @@ ControlTabWidget::ControlTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* connect(execution_watcher_, &QFutureWatcher::finished, this, &ControlTabWidget::executeFinished); // Set initial status - calibration_display_->setStatus(rviz::StatusProperty::Ok, "Calibration", "Collect 5 samples to start calibration."); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Ok, "Calibration", + "Collect 5 samples to start calibration."); } -void ControlTabWidget::loadWidget(const rviz::Config& config) +void ControlTabWidget::loadWidget(const rviz_common::Config& config) { QString group_name; config.mapGetString("group", &group_name); @@ -274,7 +275,7 @@ void ControlTabWidget::loadWidget(const rviz::Config& config) } } -void ControlTabWidget::saveWidget(rviz::Config& config) +void ControlTabWidget::saveWidget(rviz_common::Config& config) { config.mapSetValue("solver", calibration_solver_->currentText()); config.mapSetValue("group", group_name_->currentText()); @@ -286,12 +287,12 @@ bool ControlTabWidget::loadSolverPlugin(std::vector& plugins) { try { - solver_plugins_loader_.reset(new pluginlib::ClassLoader( - "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeSolverBase")); + solver_plugins_loader_ = std::make_unique>( + "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeSolverBase"); } catch (pluginlib::PluginlibException& ex) { - calibration_display_->setStatus(rviz::StatusProperty::Error, "Calibration", + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Calibration", "Couldn't create solver plugin loader."); QMessageBox::warning(this, tr("Exception while creating handeye solver plugin loader "), tr(ex.what())); return false; @@ -312,8 +313,10 @@ bool ControlTabWidget::createSolverInstance(const std::string& plugin_name) } catch (pluginlib::PluginlibException& ex) { - calibration_display_->setStatus(rviz::StatusProperty::Error, "Calibration", "Couldn't load solver plugin."); - ROS_ERROR_STREAM_NAMED(LOGNAME, "Exception while loading handeye solver plugin: " << plugin_name << ex.what()); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Calibration", + "Couldn't load solver plugin."); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Exception while loading handeye solver plugin: " << plugin_name << ex.what()); solver_ = nullptr; } @@ -351,14 +354,14 @@ bool ControlTabWidget::takeTransformSamples() // Store the pair of two tf transforms and calculate camera_robot pose try { - geometry_msgs::TransformStamped camera_to_object_tf; - geometry_msgs::TransformStamped base_to_eef_tf; + geometry_msgs::msg::TransformStamped camera_to_object_tf; + geometry_msgs::msg::TransformStamped base_to_eef_tf; // Get the transform of the object w.r.t the camera - camera_to_object_tf = tf_buffer_->lookupTransform(frame_names_["sensor"], frame_names_["object"], ros::Time(0)); + camera_to_object_tf = tf_buffer_->lookupTransform(frame_names_["sensor"], frame_names_["object"], rclcpp::Time(0)); // Get the transform of the end-effector w.r.t the robot base - base_to_eef_tf = tf_buffer_->lookupTransform(frame_names_["base"], frame_names_["eef"], ros::Time(0)); + base_to_eef_tf = tf_buffer_->lookupTransform(frame_names_["base"], frame_names_["eef"], rclcpp::Time(0)); // Renormalize quaternions, to avoid numerical issues tf2::Quaternion tf2_quat; @@ -377,7 +380,7 @@ bool ControlTabWidget::takeTransformSamples() } catch (tf2::TransformException& e) { - ROS_WARN("TF exception: %s", e.what()); + RCLCPP_WARN(node_->get_logger(), "TF exception: %s", e.what()); return false; } @@ -410,7 +413,7 @@ bool ControlTabWidget::solveCameraRobotPose() camera_robot_pose_, sensor_mount_type_); std::ostringstream reproj_err_text; reproj_err_text << "Reprojection error:\n" << reproj_err.first << " m, " << reproj_err.second << " rad"; - ROS_WARN_NAMED(LOGNAME, "%s", reproj_err_text.str().c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", reproj_err_text.str().c_str()); reprojection_error_label_->setText(QString(reproj_err_text.str().c_str())); // Publish camera pose tf @@ -419,12 +422,13 @@ bool ControlTabWidget::solveCameraRobotPose() if (!from_frame.empty() && !to_frame.empty()) { tf_tools_->clearAllTransforms(); - calibration_display_->setStatus(rviz::StatusProperty::Ok, "Calibration", "Calibration successful."); - ROS_INFO_STREAM_NAMED(LOGNAME, "Publish camera transformation" << std::endl - << camera_robot_pose_.matrix() << std::endl - << "from " << from_frame_tag_ << " frame '" - << from_frame << "'" - << "to sensor frame '" << to_frame << "'"); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Ok, "Calibration", + "Calibration successful."); + RCLCPP_INFO_STREAM(node_->get_logger(), "Publish camera transformation" + << std::endl + << camera_robot_pose_.matrix() << std::endl + << "from " << from_frame_tag_ << " frame '" << from_frame << "'" + << "to sensor frame '" << to_frame << "'"); return tf_tools_->publishTransform(camera_robot_pose_, from_frame, to_frame); } else @@ -435,7 +439,7 @@ bool ControlTabWidget::solveCameraRobotPose() warn_msg << "Found camera pose:" << std::endl << camera_robot_pose_.matrix() << std::endl << "but " << from_frame_tag_ << " or sensor frame is undefined."; - ROS_ERROR_STREAM_NAMED(LOGNAME, warn_msg.str()); + RCLCPP_ERROR_STREAM(node_->get_logger(), warn_msg.str()); } // GUI warning message with formatting { @@ -445,21 +449,22 @@ bool ControlTabWidget::solveCameraRobotPose() << "but " << from_frame_tag_ << " or sensor frame is undefined."; QMessageBox::warning(this, "Solver Failed", QString::fromStdString(warn_msg.str())); } - calibration_display_->setStatus(rviz::StatusProperty::Warn, "Calibration", + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Warn, "Calibration", "Calibration successful but frames are undefined."); return false; } } else { - calibration_display_->setStatus(rviz::StatusProperty::Error, "Calibration", "Solver failed."); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Calibration", "Solver failed."); QMessageBox::warning(this, tr("Solver Failed"), tr((std::string("Error: ") + error_message).c_str())); return false; } } else { - calibration_display_->setStatus(rviz::StatusProperty::Error, "Calibration", "No solver available."); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Calibration", + "No solver available."); QMessageBox::warning(this, tr("No Solver Available"), tr("No available handeye calibration solver instance.")); return false; } @@ -494,8 +499,8 @@ void ControlTabWidget::setTFTool(rviz_visual_tools::TFVisualToolsPtr& tf_pub) tf_tools_ = tf_pub; } -void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformStamped& camera_to_object_tf, - const geometry_msgs::TransformStamped& base_to_eef_tf, int id) +void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::msg::TransformStamped& camera_to_object_tf, + const geometry_msgs::msg::TransformStamped& base_to_eef_tf, int id) { std::string item_name = "Sample " + std::to_string(id); QStandardItem* parent = new QStandardItem(QString(item_name.c_str())); @@ -504,13 +509,19 @@ void ControlTabWidget::addPoseSampleToTreeView(const geometry_msgs::TransformSta std::ostringstream ss; QStandardItem* child_1 = new QStandardItem("TF base-to-eef"); - ss << base_to_eef_tf.transform; + ss << "((" << base_to_eef_tf.transform.translation.x << ", " << base_to_eef_tf.transform.translation.y << ", " + << base_to_eef_tf.transform.translation.z << ",), (" << base_to_eef_tf.transform.rotation.x << ", " + << base_to_eef_tf.transform.rotation.y << ", " << base_to_eef_tf.transform.rotation.z << ", " + << base_to_eef_tf.transform.rotation.w << "))"; child_1->appendRow(new QStandardItem(ss.str().c_str())); parent->appendRow(child_1); QStandardItem* child_2 = new QStandardItem("TF camera-to-target"); ss.str(""); - ss << camera_to_object_tf.transform; + ss << "((" << camera_to_object_tf.transform.translation.x << ", " << camera_to_object_tf.transform.translation.y + << ", " << camera_to_object_tf.transform.translation.z << ",), (" << camera_to_object_tf.transform.rotation.x + << ", " << camera_to_object_tf.transform.rotation.y << ", " << camera_to_object_tf.transform.rotation.z << ", " + << camera_to_object_tf.transform.rotation.w << "))"; child_2->appendRow(new QStandardItem(ss.str().c_str())); parent->appendRow(child_2); } @@ -529,7 +540,7 @@ void ControlTabWidget::UpdateSensorMountType(int index) from_frame_tag_ = "eef"; break; default: - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error sensor mount type."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error sensor mount type."); break; } } @@ -538,9 +549,9 @@ void ControlTabWidget::UpdateSensorMountType(int index) void ControlTabWidget::updateFrameNames(std::map names) { frame_names_ = names; - ROS_DEBUG("Frame names changed:"); + RCLCPP_DEBUG(node_->get_logger(), "Frame names changed:"); for (const std::pair& name : frame_names_) - ROS_DEBUG_STREAM(name.first << " : " << name.second); + RCLCPP_DEBUG_STREAM(node_->get_logger(), name.first << " : " << name.second); } void ControlTabWidget::takeSampleBtnClicked(bool clicked) @@ -555,12 +566,12 @@ void ControlTabWidget::takeSampleBtnClicked(bool clicked) // Save the joint values of current robot state if (planning_scene_monitor_) { - planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now(), 0.1); + planning_scene_monitor_->waitForCurrentRobotState(rclcpp::Clock(RCL_ROS_TIME).now(), 0.1); // Revisit this change const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); if (ps) { - const robot_state::RobotState& state = ps->getCurrentState(); + const moveit::core::RobotState& state = ps->getCurrentState(); const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group_name_->currentText().toStdString()); const std::vector& names = jmg->getActiveJointModelNames(); if (joint_names_.size() != names.size() || joint_names_ != names) @@ -606,15 +617,20 @@ void ControlTabWidget::saveCameraPoseBtnClicked(bool clicked) } // DontUseNativeDialog option set to avoid this issue: https://github.com/ros-planning/moveit/issues/2357 - QString file_name = - QFileDialog::getSaveFileName(this, tr("Save Camera Robot Pose"), "", tr("Target File (*.launch);;All Files (*)"), - nullptr, QFileDialog::DontUseNativeDialog); + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save Camera Robot Pose"), "", + tr("Launch scripts - ALL (*.launch* *.py *.xml *.yaml *.yml);;Launch scripts - " + "PYTHON (*.launch.py *.py);;Launch scripts - XML (*.launch *.launch.xml *.xml);;Launch " + "scripts - YAML (*.launch.yaml *.launch.yml *.yaml *.yml);;All Files (*)"), + nullptr, QFileDialog::DontUseNativeDialog); if (file_name.isEmpty()) return; - if (!file_name.endsWith(".launch")) - file_name += ".launch"; + if (!file_name.contains(".")) + file_name += ".launch.py"; + else if (file_name.endsWith(".launch")) + file_name += ".py"; QFile file(file_name); if (!file.open(QIODevice::WriteOnly)) @@ -623,24 +639,47 @@ void ControlTabWidget::saveCameraPoseBtnClicked(bool clicked) return; } - QTextStream out(&file); - Eigen::Vector3d t = camera_robot_pose_.translation(); Eigen::Quaterniond r_quat(camera_robot_pose_.rotation()); Eigen::Vector3d r_euler = camera_robot_pose_.rotation().eulerAngles(0, 1, 2); + + std::string mount_type; + switch (sensor_mount_type_) + { + case mhc::EYE_TO_HAND: + mount_type = "EYE-TO-HAND"; + break; + case mhc::EYE_IN_HAND: + mount_type = "EYE-IN-HAND"; + break; + default: + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error sensor mount type."); + break; + } + std::stringstream ss; - ss << "" << std::endl; - ss << " " - << std::endl; - ss << " " << std::endl; - ss << " " << std::endl; - ss << "" << std::endl; + if (file_name.endsWith(".py")) + { + ss = generateCalibrationPython(from_frame, to_frame, t, r_quat, r_euler, mount_type); + } + else if (file_name.endsWith(".xml")) + { + ss = generateCalibrationXml(from_frame, to_frame, t, r_quat, r_euler, mount_type); + } + else if (file_name.endsWith(".yaml") || file_name.endsWith(".yml")) + { + ss = generateCalibrationYaml(from_frame, to_frame, t, r_quat, r_euler, mount_type); + } + else + { + QMessageBox::warning( + this, tr("Unknown file type"), + tr("Unable to save file, unknown file type. Only `.py`, `.xml`, and `.yaml`/`.yml` are currently " + "supported for ROS 2 launch scripts.")); + return; + } + + QTextStream out(&file); out << ss.str().c_str(); } @@ -664,8 +703,10 @@ void ControlTabWidget::setGroupName(const std::string& group_name) try { moveit::planning_interface::MoveGroupInterface::Options opt(group_name); - opt.node_handle_ = ros::NodeHandle(calibration_display_->move_group_ns_property_->getStdString()); - move_group_.reset(new moveit::planning_interface::MoveGroupInterface(opt, tf_buffer_, ros::WallDuration(5, 0))); + // opt.node_handle_ = ros::NodeHandle(calibration_display_->move_group_ns_property_->getStdString()); <- Do I need + // to assign opt.robot_model and opt.robot_description ? + move_group_.reset( + new moveit::planning_interface::MoveGroupInterface(node_, opt, tf_buffer_, rclcpp::Duration(5, 0))); // Clear the joint values from any previous group joint_states_.clear(); @@ -673,7 +714,7 @@ void ControlTabWidget::setGroupName(const std::string& group_name) } catch (std::exception& ex) { - ROS_ERROR_NAMED(LOGNAME, "%s", ex.what()); + RCLCPP_ERROR(node_->get_logger(), "%s", ex.what()); } } @@ -681,17 +722,17 @@ void ControlTabWidget::fillPlanningGroupNameComboBox() { group_name_->clear(); // Fill in available planning group names - planning_scene_monitor_.reset( - new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf_buffer_, "planning_scene_monitor")); + planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(node_, "robot_description", tf_buffer_, + "planning_scene_monitor")); if (planning_scene_monitor_) { planning_scene_monitor_->startSceneMonitor(calibration_display_->planning_scene_topic_property_->getStdString()); std::string service_name = planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE; if (!calibration_display_->move_group_ns_property_->getStdString().empty()) - service_name = ros::names::append(calibration_display_->move_group_ns_property_->getStdString(), service_name); + service_name = rclcpp::names::append(calibration_display_->move_group_ns_property_->getStdString(), service_name); if (planning_scene_monitor_->requestPlanningSceneState(service_name)) { - const robot_model::RobotModelConstPtr& kmodel = planning_scene_monitor_->getRobotModel(); + const moveit::core::RobotModelConstPtr& kmodel = planning_scene_monitor_->getRobotModel(); for (const std::string& group_name : kmodel->getJointModelGroupNames()) { group_name_->addItem(group_name.c_str()); @@ -706,7 +747,7 @@ void ControlTabWidget::saveJointStateBtnClicked(bool clicked) { if (!checkJointStates()) { - QMessageBox::warning(this, tr("Error"), tr("No joint states or joint state dosen't match joint names.")); + QMessageBox::warning(this, tr("Error"), tr("No joint states or joint state doesn't match joint names.")); return; } @@ -802,7 +843,7 @@ void ControlTabWidget::saveSamplesBtnClicked(bool clicked) { if (effector_wrt_world_.size() != object_wrt_sensor_.size()) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Different number of poses"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Different number of poses"); return; } @@ -877,7 +918,7 @@ void ControlTabWidget::loadJointStateBtnClicked(bool clicked) // Begin parsing try { - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Load joint states from file: " << file_name.toStdString().c_str()); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Load joint states from file: " << file_name.toStdString().c_str()); YAML::Node doc = YAML::LoadFile(file_name.toStdString()); if (!doc.IsMap()) return; @@ -892,7 +933,7 @@ void ControlTabWidget::loadJointStateBtnClicked(bool clicked) } else { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Can't find 'joint_names' in the openned file."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Can't find 'joint_names' in the opened file."); return; } @@ -913,13 +954,13 @@ void ControlTabWidget::loadJointStateBtnClicked(bool clicked) } else { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Can't find 'joint_values' in the openned file."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Can't find 'joint_values' in the opened file."); return; } } catch (YAML::ParserException& e) // Catch errors { - ROS_ERROR_STREAM_NAMED(LOGNAME, e.what()); + RCLCPP_ERROR_STREAM(node_->get_logger(), e.what()); return; } @@ -928,7 +969,7 @@ void ControlTabWidget::loadJointStateBtnClicked(bool clicked) auto_progress_->setMax(joint_states_.size()); auto_progress_->setValue(0); } - ROS_INFO_STREAM_NAMED(LOGNAME, "Loaded and parsed: " << file_name.toStdString()); + RCLCPP_INFO_STREAM(node_->get_logger(), "Loaded and parsed: " << file_name.toStdString()); } void ControlTabWidget::autoPlanBtnClicked(bool clicked) @@ -973,12 +1014,12 @@ void ControlTabWidget::computePlan() } // Get current joint state as start state - robot_state::RobotStatePtr start_state = move_group_->getCurrentState(); - planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now(), 0.1); + moveit::core::RobotStatePtr start_state = move_group_->getCurrentState(); + planning_scene_monitor_->waitForCurrentRobotState(rclcpp::Clock(RCL_ROS_TIME).now(), 0.1); const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); if (ps) - start_state.reset(new robot_state::RobotState(ps->getCurrentState())); + start_state.reset(new moveit::core::RobotState(ps->getCurrentState())); // Plan motion to the recorded joint state target if (auto_progress_->getValue() < joint_states_.size()) @@ -993,9 +1034,9 @@ void ControlTabWidget::computePlan() ControlTabWidget::FAILURE_PLAN_FAILED; if (planning_res_ == ControlTabWidget::SUCCESS) - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Planning succeed."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Planning succeed."); else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning failed."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Planning failed."); } } @@ -1019,10 +1060,10 @@ void ControlTabWidget::computeExecution() if (planning_res_ == ControlTabWidget::SUCCESS) { - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Execution succeed."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Execution succeed."); } else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Execution failed."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Execution failed."); } void ControlTabWidget::planFinished() @@ -1055,7 +1096,7 @@ void ControlTabWidget::planFinished() case ControlTabWidget::SUCCESS: break; } - ROS_DEBUG_NAMED(LOGNAME, "Plan finished"); + RCLCPP_DEBUG(node_->get_logger(), "Plan finished"); } void ControlTabWidget::executeFinished() @@ -1070,7 +1111,7 @@ void ControlTabWidget::executeFinished() if (effector_wrt_world_.size() == object_wrt_sensor_.size() && effector_wrt_world_.size() > 4) solveCameraRobotPose(); } - ROS_DEBUG_NAMED(LOGNAME, "Execution finished"); + RCLCPP_DEBUG(node_->get_logger(), "Execution finished"); } void ControlTabWidget::autoSkipBtnClicked(bool clicked) @@ -1078,4 +1119,118 @@ void ControlTabWidget::autoSkipBtnClicked(bool clicked) auto_progress_->setValue(auto_progress_->getValue() + 1); } +std::stringstream ControlTabWidget::generateCalibrationPython(std::string& from_frame, std::string& to_frame, + Eigen::Vector3d& t, Eigen::Quaterniond& r_quat, + Eigen::Vector3d& r_euler, std::string& mount_type) +{ + std::stringstream ss; + ss << "\"\"\" Static transform publisher acquired via MoveIt 2 hand-eye calibration \"\"\"" << std::endl; + ss << "\"\"\" " << mount_type << ": " << from_frame << " -> " << to_frame << " \"\"\"" << std::endl; + ss << "from launch import LaunchDescription" << std::endl; + ss << "from launch_ros.actions import Node" << std::endl; + ss << std::endl; + ss << std::endl; + ss << "def generate_launch_description() -> LaunchDescription:" << std::endl; + ss << " nodes = [" << std::endl; + ss << " Node(" << std::endl; + ss << " package=\"tf2_ros\"," << std::endl; + ss << " executable=\"static_transform_publisher\"," << std::endl; + ss << " output=\"log\"," << std::endl; + ss << " arguments=[" << std::endl; + ss << " \"--frame-id\"," << std::endl; + ss << " \"" << from_frame << "\"," << std::endl; + ss << " \"--child-frame-id\"," << std::endl; + ss << " \"" << to_frame << "\"," << std::endl; + ss << " \"--x\"," << std::endl; + ss << " \"" << t[0] << "\"," << std::endl; + ss << " \"--y\"," << std::endl; + ss << " \"" << t[1] << "\"," << std::endl; + ss << " \"--z\"," << std::endl; + ss << " \"" << t[2] << "\"," << std::endl; + ss << " \"--qx\"," << std::endl; + ss << " \"" << r_quat.x() << "\"," << std::endl; + ss << " \"--qy\"," << std::endl; + ss << " \"" << r_quat.y() << "\"," << std::endl; + ss << " \"--qz\"," << std::endl; + ss << " \"" << r_quat.z() << "\"," << std::endl; + ss << " \"--qw\"," << std::endl; + ss << " \"" << r_quat.w() << "\"," << std::endl; + ss << " # \"--roll\"," << std::endl; + ss << " # \"" << r_euler[0] << "\"," << std::endl; + ss << " # \"--pitch\"," << std::endl; + ss << " # \"" << r_euler[1] << "\"," << std::endl; + ss << " # \"--yaw\"," << std::endl; + ss << " # \"" << r_euler[2] << "\"," << std::endl; + ss << " ]," << std::endl; + ss << " )," << std::endl; + ss << " ]" << std::endl; + ss << " return LaunchDescription(nodes)" << std::endl; + return ss; +} + +std::stringstream ControlTabWidget::generateCalibrationXml(std::string& from_frame, std::string& to_frame, + Eigen::Vector3d& t, Eigen::Quaterniond& r_quat, + Eigen::Vector3d& r_euler, std::string& mount_type) +{ + std::stringstream ss; + ss << "" << std::endl; + ss << "" << std::endl; + ss << std::endl; + ss << "" << std::endl; + ss << " " << std::endl; + ss << " " << std::endl; + ss << "" << std::endl; + return ss; +} + +std::stringstream ControlTabWidget::generateCalibrationYaml(std::string& from_frame, std::string& to_frame, + Eigen::Vector3d& t, Eigen::Quaterniond& r_quat, + Eigen::Vector3d& r_euler, std::string& mount_type) +{ + std::stringstream ss; + ss << "# Static transform publisher acquired via MoveIt 2 hand-eye calibration" << std::endl; + ss << "# " << mount_type << ": " << from_frame << " -> " << to_frame << std::endl; + ss << std::endl; + ss << "launch:" << std::endl; + ss << " - node:" << std::endl; + ss << " pkg: tf2_ros" << std::endl; + ss << " exec: static_transform_publisher" << std::endl; + ss << " output: log" << std::endl; + ss << " args:" << std::endl; + ss << " \"" << std::endl; + ss << " --frame-id " << from_frame << std::endl; + ss << " --child-frame-id " << to_frame << std::endl; + ss << " --x " << t[0] << std::endl; + ss << " --y " << t[1] << std::endl; + ss << " --z " << t[2] << std::endl; + ss << " --qx " << r_quat.x() << std::endl; + ss << " --qy " << r_quat.y() << std::endl; + ss << " --qz " << r_quat.z() << std::endl; + ss << " --qw " << r_quat.w() << std::endl; + ss << " \"" << std::endl; + ss << " # --roll " << r_euler[0] << std::endl; + ss << " # --pitch " << r_euler[1] << std::endl; + ss << " # --yaw " << r_euler[2] << std::endl; + return ss; +} + } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index e51d8c0..86addfa 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -38,8 +38,6 @@ namespace moveit_rviz_plugin { -const std::string LOGNAME = "handeye_target_widget"; - void RosTopicComboBox::addMsgsFilterType(QString msgs_type) { message_types_.insert(msgs_type); @@ -54,18 +52,17 @@ bool RosTopicComboBox::hasTopic(const QString& topic_name) bool RosTopicComboBox::getFilteredTopics() { // Get all topic names - ros::master::V_TopicInfo ros_topic_vec; - if (ros::master::getTopics(ros_topic_vec)) + std::map> topic_names_and_types = node_->get_topic_names_and_types(); + image_topics_.clear(); + // Filter out the topic names with specific topic type + for (const auto& topic_info : topic_names_and_types) { - image_topics_.clear(); - // Filter out the topic names with specific topic type - for (const ros::master::TopicInfo& topic_info : ros_topic_vec) - { - if (message_types_.contains(QString(topic_info.datatype.c_str()))) + std::for_each(topic_info.second.begin(), topic_info.second.end(), [&](std::string topic_type) { + if (message_types_.contains(QString(topic_type.c_str()))) { - image_topics_.insert(QString(topic_info.name.c_str())); + image_topics_.insert(QString(topic_info.first.c_str())); } - } + }); } clear(); @@ -84,11 +81,12 @@ void RosTopicComboBox::mousePressEvent(QMouseEvent* event) showPopup(); } -TargetTabWidget::TargetTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* parent) +TargetTabWidget::TargetTabWidget(rclcpp::Node::SharedPtr node, HandEyeCalibrationDisplay* pdisplay, QWidget* parent) : QWidget(parent) + , node_(node) , calibration_display_(pdisplay) - , nh_("~") - , it_(nh_) + , it_(node_) + , tf_pub_(std::make_shared(node_)) , target_plugins_loader_(nullptr) , target_(nullptr) , target_param_layout_(new QFormLayout()) @@ -115,19 +113,13 @@ TargetTabWidget::TargetTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* p QFormLayout* layout_left_bottom = new QFormLayout(); group_left_bottom->setLayout(layout_left_bottom); - ros_topics_.insert(std::make_pair("image_topic", new RosTopicComboBox(this))); - ros_topics_["image_topic"]->addMsgsFilterType("sensor_msgs/Image"); - layout_left_bottom->addRow("Image Topic", ros_topics_["image_topic"]); + ros_topics_.insert(std::make_pair("image_topic", new RosTopicComboBox(node_, this))); + ros_topics_["image_topic"]->addMsgsFilterType("sensor_msgs/msg/Image"); + layout_left_bottom->addRow("Camera Image Topic", ros_topics_["image_topic"]); connect(ros_topics_["image_topic"], SIGNAL(activated(const QString&)), this, SLOT(imageTopicComboboxChanged(const QString&))); - ros_topics_.insert(std::make_pair("camera_info_topic", new RosTopicComboBox(this))); - ros_topics_["camera_info_topic"]->addMsgsFilterType("sensor_msgs/CameraInfo"); - layout_left_bottom->addRow("CameraInfo Topic", ros_topics_["camera_info_topic"]); - connect(ros_topics_["camera_info_topic"], SIGNAL(activated(const QString&)), this, - SLOT(cameraInfoComboBoxChanged(const QString&))); - - // Target image dislay, create and save area + // Target image display, create and save area QGroupBox* group_right = new QGroupBox("Target", this); group_right->setMinimumWidth(330); layout->addWidget(group_right); @@ -146,21 +138,22 @@ TargetTabWidget::TargetTabWidget(HandEyeCalibrationDisplay* pdisplay, QWidget* p layout_right->addWidget(save_target_btn); connect(save_target_btn, SIGNAL(clicked(bool)), this, SLOT(saveTargetImageBtnClicked(bool))); - // Load availible target plugins + // Load available target plugins loadAvailableTargetPlugins(); // Initialize image publisher image_pub_ = it_.advertise("/handeye_calibration/target_detection", 1); // Register custom types - qRegisterMetaType(); + qRegisterMetaType(); qRegisterMetaType(); // Initialize status - calibration_display_->setStatusStd(rviz::StatusProperty::Warn, "Target detection", "Not subscribed to image topic."); + calibration_display_->setStatusStd(rviz_common::properties::StatusProperty::Warn, "Target detection", + "Not subscribed to image topic."); } -void TargetTabWidget::loadWidget(const rviz::Config& config) +void TargetTabWidget::loadWidget(const rviz_common::Config& config) { if (target_type_->count() > 0) { @@ -209,26 +202,21 @@ void TargetTabWidget::loadWidget(const rviz::Config& config) { if (!topic.first.compare("image_topic")) { - image_sub_.shutdown(); - image_sub_ = it_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::imageCallback, this); - } - - if (!topic.first.compare("camera_info_topic")) - { - camerainfo_sub_.shutdown(); - camerainfo_sub_ = nh_.subscribe(topic_name.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); + camera_sub_.shutdown(); + camera_sub_ = it_.subscribeCamera(topic_name.toStdString(), 1, &TargetTabWidget::cameraCallback, this); } } catch (const image_transport::TransportLoadException& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Subscribe to " << topic_name.toStdString() << " fail: " << e.what()); } } } } } -void TargetTabWidget::saveWidget(rviz::Config& config) +void TargetTabWidget::saveWidget(rviz_common::Config& config) { config.mapSetValue("target_type", target_type_->currentText()); @@ -375,14 +363,21 @@ bool TargetTabWidget::createTargetInstance() return true; } -void TargetTabWidget::imageCallback(const sensor_msgs::ImageConstPtr& msg) +void TargetTabWidget::cameraCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info) +{ + cameraInfoCallback(camera_info); + imageCallback(image); +} + +void TargetTabWidget::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { createTargetInstance(); // Depth image format `16UC1` cannot be converted to `MONO8` if (msg->encoding == "16UC1") { - calibration_display_->setStatus(rviz::StatusProperty::Error, "Target detection", + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Target detection", "Received 16-bit image, which cannot be processed."); return; } @@ -398,16 +393,17 @@ void TargetTabWidget::imageCallback(const sensor_msgs::ImageConstPtr& msg) } else { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Image msg has empty frame_id."); - calibration_display_->setStatus(rviz::StatusProperty::Error, "Target detection", + RCLCPP_ERROR_STREAM(node_->get_logger(), "Image msg has empty frame_id."); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Target detection", "Image message has empty frame ID."); return; } if (msg->data.empty()) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Image msg has empty data."); - calibration_display_->setStatus(rviz::StatusProperty::Error, "Target detection", "Image message is empty."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Image msg has empty data."); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Target detection", + "Image message is empty."); return; } @@ -416,54 +412,67 @@ void TargetTabWidget::imageCallback(const sensor_msgs::ImageConstPtr& msg) { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); - sensor_msgs::ImagePtr pub_msg; + sensor_msgs::msg::Image::SharedPtr pub_msg; if (target_ && target_->detectTargetPose(cv_ptr->image)) { - pub_msg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", cv_ptr->image).toImageMsg(); + pub_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "rgb8", cv_ptr->image).toImageMsg(); - geometry_msgs::TransformStamped tf2_msg = target_->getTransformStamped(optical_frame_); - tf_pub_.sendTransform(tf2_msg); + geometry_msgs::msg::TransformStamped tf2_msg = target_->getTransformStamped(optical_frame_); + tf_pub_->sendTransform(tf2_msg); if (!target_->areIntrinsicsReasonable()) { calibration_display_->setStatus( - rviz::StatusProperty::Warn, "Target detection", + rviz_common::properties::StatusProperty::Warn, "Target detection", "Target detector has not received reasonable intrinsics. Attempted detection anyway."); } else { - calibration_display_->setStatus(rviz::StatusProperty::Ok, "Target detection", "Target pose detected."); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Ok, "Target detection", + "Target pose detected."); } } else { - pub_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", cv_ptr->image).toImageMsg(); - calibration_display_->setStatus(rviz::StatusProperty::Error, "Target detection", "Target detection failed."); + pub_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", cv_ptr->image).toImageMsg(); + calibration_display_->setStatus(rviz_common::properties::StatusProperty::Error, "Target detection", + "Target detection failed."); } image_pub_.publish(pub_msg); } catch (cv_bridge::Exception& e) { std::string error_message = "cv_bridge exception: " + std::string(e.what()); - calibration_display_->setStatusStd(rviz::StatusProperty::Error, "Target detection", error_message); - ROS_ERROR_NAMED(LOGNAME, "%s", error_message.c_str()); + calibration_display_->setStatusStd(rviz_common::properties::StatusProperty::Error, "Target detection", + error_message); + RCLCPP_ERROR(node_->get_logger(), "%s", error_message.c_str()); } catch (cv::Exception& e) { std::string error_message = "cv exception: " + std::string(e.what()); - calibration_display_->setStatusStd(rviz::StatusProperty::Error, "Target detection", error_message); - ROS_ERROR_NAMED(LOGNAME, "%s", error_message.c_str()); + calibration_display_->setStatusStd(rviz_common::properties::StatusProperty::Error, "Target detection", + error_message); + RCLCPP_ERROR(node_->get_logger(), "%s", error_message.c_str()); } } -void TargetTabWidget::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) +void TargetTabWidget::cameraInfoCallback(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) { - if (target_ && msg->height > 0 && msg->width > 0 && !msg->K.empty() && !msg->D.empty() && - (!camera_info_ || msg->K != camera_info_->K || msg->P != camera_info_->P)) + if (!camera_info_ || msg->k != camera_info_->k || msg->p != camera_info_->p) { - ROS_DEBUG_NAMED(LOGNAME, "Received camera info."); - camera_info_ = msg; - target_->setCameraIntrinsicParams(camera_info_); - Q_EMIT cameraInfoChanged(*camera_info_); + if (target_ && msg->height > 0 && msg->width > 0 && !msg->k.empty() && !msg->d.empty()) + { + RCLCPP_DEBUG(node_->get_logger(), "Received camera info."); + camera_info_ = msg; + target_->setCameraIntrinsicParams(camera_info_); + Q_EMIT cameraInfoChanged(*camera_info_); + } + else + { + std::string error_message = "Invalid CameraInfo message was received."; + calibration_display_->setStatusStd(rviz_common::properties::StatusProperty::Error, "Target detection", + error_message); + RCLCPP_ERROR(node_->get_logger(), "%s", error_message.c_str()); + } } } @@ -528,48 +537,29 @@ void TargetTabWidget::saveTargetImageBtnClicked(bool clicked) } if (!cv::imwrite(cv::String(fileName.toStdString()), target_image_)) - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error OpenCV saving image."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error OpenCV saving image."); } void TargetTabWidget::imageTopicComboboxChanged(const QString& topic) { - image_sub_.shutdown(); + camera_sub_.shutdown(); - calibration_display_->setStatusStd(rviz::StatusProperty::Warn, "Target detection", "Not subscribed to image topic."); + calibration_display_->setStatusStd(rviz_common::properties::StatusProperty::Warn, "Target detection", + "Not subscribed to image topic."); if (!topic.isNull() and !topic.isEmpty()) { try { - image_sub_ = it_.subscribe(topic.toStdString(), 1, &TargetTabWidget::imageCallback, this); + camera_sub_ = it_.subscribeCamera(topic.toStdString(), 1, &TargetTabWidget::cameraCallback, this); } catch (image_transport::TransportLoadException& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Subscribe to image topic: " << topic.toStdString() << " failed. " << e.what()); - calibration_display_->setStatusStd(rviz::StatusProperty::Error, "Target detection", + RCLCPP_ERROR_STREAM(node_->get_logger(), + "Subscribe to image topic: " << topic.toStdString() << " failed. " << e.what()); + calibration_display_->setStatusStd(rviz_common::properties::StatusProperty::Error, "Target detection", "Failed to subscribe to image topic."); } } } -void TargetTabWidget::cameraInfoComboBoxChanged(const QString& topic) -{ - camerainfo_sub_.shutdown(); - calibration_display_->setStatusStd(rviz::StatusProperty::Warn, "Target detection", - "Not subscribed to camera info topic."); - if (!topic.isNull() and !topic.isEmpty()) - { - try - { - camerainfo_sub_ = nh_.subscribe(topic.toStdString(), 1, &TargetTabWidget::cameraInfoCallback, this); - } - catch (ros::Exception& e) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, - "Subscribe to camera info topic: " << topic.toStdString() << " failed. " << e.what()); - calibration_display_->setStatusStd(rviz::StatusProperty::Error, "Target detection", - "Failed to subscribe to camera info topic."); - } - } -} - } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp index 6343c96..4e7139e 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/plugin_init.cpp @@ -34,7 +34,7 @@ /* Author: Yu Yan */ -#include +#include #include -CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::HandEyeCalibrationDisplay, rviz::Display) +PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::HandEyeCalibrationDisplay, rviz_common::Display) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml b/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml index 37c4a4f..46fab24 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin_description.xml @@ -1,5 +1,5 @@ - - + + RViz display for the hand-eye calibration. diff --git a/moveit_calibration_gui/package.xml b/moveit_calibration_gui/package.xml index 7f8d55f..76165a0 100644 --- a/moveit_calibration_gui/package.xml +++ b/moveit_calibration_gui/package.xml @@ -1,46 +1,44 @@ - + + moveit_calibration_gui - 0.1.0 - MoveIt calibration gui - + 2.0.0 + MoveIt calibration GUI plugins Yu Yan - Yu Yan - BSD - http://moveit.ros.org + https://moveit.ros.org https://github.com/ros-planning/moveit_calibration/issues https://github.com/ros-planning/moveit_calibration - catkin - pkg-config + ament_cmake + ament_cmake_ros - class_loader - eigen + eigen3_cmake_module + moveit_common qtbase5-dev - image_geometry cv_bridge - geometric_shapes - moveit_ros_visualization + eigen + image_geometry + image_transport + libopencv-dev moveit_calibration_plugins - moveit_core - moveit_ros_perception - moveit_ros_planning + moveit_ros_visualization moveit_ros_planning_interface moveit_visual_tools - pluginlib - rosconsole - roscpp - rviz + pluginlib + rclcpp + rviz_common rviz_visual_tools tf2_eigen - rostest + libqt5-core + libqt5-gui + libqt5-widgets + ament_cmake - diff --git a/moveit_calibration_plugins/CHANGELOG.rst b/moveit_calibration_plugins/CHANGELOG.rst index 75b5d69..95454f3 100644 --- a/moveit_calibration_plugins/CHANGELOG.rst +++ b/moveit_calibration_plugins/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_calibration_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.1.0 (forthcoming) +2.0.0 (forthcoming) +----------- +* First public release for ROS 2 + +0.1.1 +----------- +* Calculate and report reprojection error + +0.1.0 ----------- * First public release for Noetic diff --git a/moveit_calibration_plugins/CMakeLists.txt b/moveit_calibration_plugins/CMakeLists.txt index f93ed3b..b400798 100644 --- a/moveit_calibration_plugins/CMakeLists.txt +++ b/moveit_calibration_plugins/CMakeLists.txt @@ -1,60 +1,47 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.10.2) project(moveit_calibration_plugins) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_EXTENSIONS OFF) +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -find_package(catkin REQUIRED COMPONENTS - roscpp - rosconsole - tf2 - tf2_eigen - tf2_geometry_msgs +# find dependencies +find_package(ament_cmake_ros REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(OpenCV REQUIRED imgcodecs aruco) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + Eigen3 + geometry_msgs + OpenCV pluginlib + rclcpp sensor_msgs + tf2 + tf2_eigen ) -find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED imgcodecs aruco) - -if(OpenCV_VERSION VERSION_EQUAL 3.2) - message(WARNING "Your version of OpenCV (3.2) is known to have buggy ArUco pose detection! Use a ChArUco target or upgrade OpenCV") -endif() +# Add project sub-libraries +add_subdirectory(handeye_calibration_solver) +add_subdirectory(handeye_calibration_target) -catkin_package( - INCLUDE_DIRS - handeye_calibration_target/include - handeye_calibration_solver/include - LIBRARIES - # No libraries are exported because directly linking to a plugin is "highly discouraged": - # http://wiki.ros.org/class_loader#Caution_of_Linking_Directly_Against_Plugin_Libraries - CATKIN_DEPENDS - roscpp - sensor_msgs - DEPENDS - EIGEN3 +# Export plugin descriptions to register with pluginlib +pluginlib_export_plugin_description_file( + moveit_calibration_plugins + handeye_calibration_target_plugin_description.xml +) +pluginlib_export_plugin_description_file( + moveit_calibration_plugins + handeye_calibration_solver_plugin_description.xml ) -include_directories( - handeye_calibration_target/include - handeye_calibration_solver/include - ${catkin_INCLUDE_DIRS} - ) -include_directories(SYSTEM - ${OpenCV_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ) - -add_subdirectory(handeye_calibration_target) -add_subdirectory(handeye_calibration_solver) - -install( - FILES - handeye_calibration_target_plugin_description.xml - handeye_calibration_solver_plugin_description.xml - DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/moveit_calibration_plugins/handeye_calibration_solver/CMakeLists.txt b/moveit_calibration_plugins/handeye_calibration_solver/CMakeLists.txt index cab8464..12f0716 100644 --- a/moveit_calibration_plugins/handeye_calibration_solver/CMakeLists.txt +++ b/moveit_calibration_plugins/handeye_calibration_solver/CMakeLists.txt @@ -1,46 +1,66 @@ -# Suppress the warning 'int _import_array()' defined but not used in 'usr/include/python2.7/numpy/__multiarray_api.h' -add_compile_options(-Wno-unused-function) - -set(HEADERS - include/moveit/handeye_calibration_solver/handeye_solver_base.h - include/moveit/handeye_calibration_solver/handeye_solver_default.h +set(MOVEIT_LIB_NAME moveit_handeye_calibration_solver) +set(SOURCE_FILES_CORE + src/handeye_solver_opencv.cpp ) - -# ---[ Using cmake scripts and modules -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules) - -find_package(PythonLibs 2.7 REQUIRED) -find_package(NumPy 1.7 REQUIRED) - -#catkin_lint: ignore_once missing_directory -include_directories(${CMAKE_CURRENT_BINARY_DIR} ${PYTHON_INCLUDE_DIRS}) - -# Plugin Source -set(SOURCE_FILES - src/handeye_solver_default.cpp +set(SOURCE_FILES_PLUGINS + src/plugin_init.cpp ) -set(MOVEIT_LIB_NAME moveit_handeye_calibration_solver) -add_library(${MOVEIT_LIB_NAME}_core ${SOURCE_FILES} ${HEADERS}) +# Core library +add_library(${MOVEIT_LIB_NAME}_core SHARED ${SOURCE_FILES_CORE}) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME}_core ${OpenCV_LIBS} ${EIGEN3_LIBS}) +target_include_directories(${MOVEIT_LIB_NAME}_core PUBLIC + $ + $) +ament_target_dependencies( + ${MOVEIT_LIB_NAME}_core + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) -add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) +# Plugin library +add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCE_FILES_PLUGINS}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC + $ + $) +ament_target_dependencies( + ${MOVEIT_LIB_NAME} + pluginlib +) + +include_directories( + SYSTEM + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) +install( + TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +ament_export_include_directories( + include +) +ament_export_libraries( + {MOVEIT_LIB_NAME}_core {MOVEIT_LIB_NAME} +) -if (CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) find_package(jsoncpp REQUIRED) - get_target_property(JSON_INC_PATH jsoncpp_lib INTERFACE_INCLUDE_DIRECTORIES) + get_target_property(JSON_INC_PATH jsoncpp_lib INTERFACE_INCLUDE_DIRECTORIES) include_directories(${JSON_INC_PATH}) - catkin_add_gtest(test_handeye_solver test/handeye_solver_test.cpp) - target_link_libraries(test_handeye_solver ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME} jsoncpp_lib) + ament_add_gtest(test_handeye_solver test/handeye_solver_test.cpp) + target_link_libraries(test_handeye_solver ${MOVEIT_LIB_NAME} jsoncpp_lib) + ament_lint_auto_find_test_dependencies() endif() diff --git a/moveit_calibration_plugins/handeye_calibration_solver/cmake/Modules/FindNumPy.cmake b/moveit_calibration_plugins/handeye_calibration_solver/cmake/Modules/FindNumPy.cmake deleted file mode 100644 index 77019eb..0000000 --- a/moveit_calibration_plugins/handeye_calibration_solver/cmake/Modules/FindNumPy.cmake +++ /dev/null @@ -1,94 +0,0 @@ -# COPYRIGHT - -# All contributions by the University of California: -# Copyright (c) 2014-2017 The Regents of the University of California (Regents) -# All rights reserved. - -# All other contributions: -# Copyright (c) 2014-2017, the respective contributors -# All rights reserved. - -# Caffe uses a shared copyright model: each contributor holds copyright over -# their contributions to Caffe. The project versioning records all such -# contribution and copyright details. If a contributor wants to further mark -# their specific copyright on a particular contribution, they should indicate -# their copyright solely in the commit message of the change when it is -# committed. - -# LICENSE - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: - -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR -# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# - Find the NumPy libraries -# This module finds if NumPy is installed, and sets the following variables -# indicating where it is. -# -# TODO: Update to provide the libraries and paths for linking npymath lib. -# -# NUMPY_FOUND - was NumPy found -# NUMPY_VERSION - the version of NumPy found as a string -# NUMPY_VERSION_MAJOR - the major version number of NumPy -# NUMPY_VERSION_MINOR - the minor version number of NumPy -# NUMPY_VERSION_PATCH - the patch version number of NumPy -# NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601 -# NUMPY_INCLUDE_DIR - path to the NumPy include files - -unset(NUMPY_VERSION) -unset(NUMPY_INCLUDE_DIR) - -if(PYTHONINTERP_FOUND) - execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import numpy as n; print(n.__version__); print(n.get_include());" - RESULT_VARIABLE __result - OUTPUT_VARIABLE __output - OUTPUT_STRIP_TRAILING_WHITESPACE) - - if(__result MATCHES 0) - string(REGEX REPLACE ";" "\\\\;" __values ${__output}) - string(REGEX REPLACE "\r?\n" ";" __values ${__values}) - list(GET __values 0 NUMPY_VERSION) - list(GET __values 1 NUMPY_INCLUDE_DIR) - - string(REGEX MATCH "^([0-9])+\\.([0-9])+\\.([0-9])+" __ver_check "${NUMPY_VERSION}") - if(NOT "${__ver_check}" STREQUAL "") - set(NUMPY_VERSION_MAJOR ${CMAKE_MATCH_1}) - set(NUMPY_VERSION_MINOR ${CMAKE_MATCH_2}) - set(NUMPY_VERSION_PATCH ${CMAKE_MATCH_3}) - math(EXPR NUMPY_VERSION_DECIMAL - "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}") - string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIR ${NUMPY_INCLUDE_DIR}) - else() - unset(NUMPY_VERSION) - unset(NUMPY_INCLUDE_DIR) - message(STATUS "Requested NumPy version and include path, but got instead:\n${__output}\n") - endif() - endif() -else() - message(STATUS "Could not find NumPy. To find it, a Python interpreter needs to be found first.") -endif() - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(NumPy REQUIRED_VARS NUMPY_INCLUDE_DIR NUMPY_VERSION - VERSION_VAR NUMPY_VERSION) - -if(NUMPY_FOUND) - message(STATUS "NumPy ver. ${NUMPY_VERSION} found (include: ${NUMPY_INCLUDE_DIR})") -endif() diff --git a/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_base.h b/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_base.h index fb49cb7..c28e65e 100644 --- a/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_base.h +++ b/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_base.h @@ -36,17 +36,21 @@ #pragma once -#include -#include +#include +#include namespace moveit_handeye_calibration { +namespace +{ +const rclcpp::Logger LOGGER_CALIBRATION_SOLVER = rclcpp::get_logger("moveit_handeye_calibration_solver"); +} + enum SensorMountType { EYE_TO_HAND = 0, EYE_IN_HAND = 1, }; - class HandEyeSolverBase { public: @@ -101,8 +105,8 @@ class HandEyeSolverBase auto ret = std::make_pair(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); if (effector_wrt_world.size() != object_wrt_sensor.size()) { - ROS_ERROR_NAMED("moveit_calibration_handeye_solver", - "Different number of optical and kinematic transforms when calculating reprojection error."); + RCLCPP_ERROR(LOGGER_CALIBRATION_SOLVER, + "Different number of optical and kinematic transforms when calculating reprojection error."); return ret; } diff --git a/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_default.h b/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_opencv.h similarity index 77% rename from moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_default.h rename to moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_opencv.h index 0cbde52..33bb77b 100644 --- a/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_default.h +++ b/moveit_calibration_plugins/handeye_calibration_solver/include/moveit/handeye_calibration_solver/handeye_solver_opencv.h @@ -37,20 +37,11 @@ #pragma once #include -#include +#include -// Disable clang warnings -#if defined(__clang__) -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wdeprecated-register" -#include -#pragma clang diagnostic pop -#elif defined(__GNUC__) || defined(__GNUG__) -#include -#endif - -#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION -#include +#include +#include +#include namespace moveit_handeye_calibration { @@ -72,16 +63,9 @@ class HandEyeSolverDefault : public HandEyeSolverBase virtual const Eigen::Isometry3d& getCameraRobotPose() const override; -private: - /** - * @brief Convert a Eigen::Isometry3d pose to a 4x4 C array - * @param pose A Eigen::Isometry3d pose. - * @param c_arr Pointer to a C array of 4 elements. - */ - bool toCArray(const Eigen::Isometry3d& pose, double (*c_arr)[TRANSFORM_MATRIX_DIMENSION]) const; - - std::vector solver_names_; // Solver algorithm names - Eigen::Isometry3d camera_robot_pose_; // Computed camera pose with respect to a robot + std::vector solver_names_; // Solver algorithm names + std::map solvers_; // Map of solvers + Eigen::Isometry3d camera_robot_pose_; // Computed camera pose with respect to a robot }; } // namespace moveit_handeye_calibration diff --git a/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_default.cpp b/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_default.cpp deleted file mode 100644 index f5ab7f5..0000000 --- a/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_default.cpp +++ /dev/null @@ -1,435 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, Intel Corporation. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Yu Yan */ - -#include - -#if PY_MAJOR_VERSION >= 3 -#define PyInt_AsLong PyLong_AsLong -#define PyString_FromString PyUnicode_FromString -#endif - -namespace moveit_handeye_calibration -{ -const std::string LOGNAME = "handeye_solver_default"; - -void HandEyeSolverDefault::initialize() -{ - solver_names_ = { "Daniilidis1999", "ParkBryan1994", "TsaiLenz1989" }; - camera_robot_pose_ = Eigen::Isometry3d::Identity(); -} - -const std::vector& HandEyeSolverDefault::getSolverNames() const -{ - return solver_names_; -} - -const Eigen::Isometry3d& HandEyeSolverDefault::getCameraRobotPose() const -{ - return camera_robot_pose_; -} - -bool HandEyeSolverDefault::solve(const std::vector& effector_wrt_world, - const std::vector& object_wrt_sensor, SensorMountType setup, - const std::string& solver_name, std::string* error_message) -{ - std::string local_error_message; - if (!error_message) - { - error_message = &local_error_message; - } - // Check the size of the two sets of pose sample equal - if (effector_wrt_world.size() != object_wrt_sensor.size()) - { - *error_message = "The sizes of the two input pose sample vectors are not equal: effector_wrt_world.size() = " + - std::to_string(effector_wrt_world.size()) + - " and object_wrt_sensor.size() == " + std::to_string(object_wrt_sensor.size()); - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - return false; - } - - auto it = std::find(solver_names_.begin(), solver_names_.end(), solver_name); - if (it == solver_names_.end()) - { - *error_message = "Unknown handeye solver name: " + solver_name; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - return false; - } - - char program_name[7] = "python"; -#if PY_MAJOR_VERSION >= 3 - Py_SetProgramName(Py_DecodeLocale(program_name, NULL)); -#else - Py_SetProgramName(program_name); -#endif - static bool numpy_loaded{ false }; - if (!numpy_loaded) // Py_Initialize() can only be called once when numpy is - // loaded, otherwise will segfault - { - Py_Initialize(); - atexit(Py_Finalize); - numpy_loaded = true; - } - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Python C API start"); - - // Load numpy c api - if (_import_array() < 0) - { - *error_message = "Error importing numpy"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - return false; - } - - PyObject *python_name, *python_module, *python_class, *python_instance, *python_func_add_sample, *python_func_solve; - PyObject *python_args, *python_value; - - // Import handeye.calibrator python module - python_name = PyString_FromString("handeye.calibrator"); - python_module = PyImport_Import(python_name); - Py_DECREF(python_name); - if (!python_module) - { - *error_message = "Failed to load python module: handeye.calibrator"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - PyErr_Print(); - return false; - } - - // Find handeye.calibrator.HandEyeCalibrator class - python_class = PyObject_GetAttrString(python_module, "HandEyeCalibrator"); - Py_DECREF(python_module); - if (!python_class || !PyCallable_Check(python_class)) - { - *error_message = "Can't find \"HandEyeCalibrator\" python class"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - PyErr_Print(); - return false; - } - - // Parse sensor mount type - python_value = PyString_FromString(""); - if (setup == EYE_TO_HAND) - python_value = PyString_FromString("Fixed"); - else if (setup == EYE_IN_HAND) - python_value = PyString_FromString("Moving"); - if (!python_value) - { - *error_message = "Can't create sensor mount type python value"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_class); - PyErr_Print(); - return false; - } - - // Create handeye.calibrator.HandEyeCalibrator instance - python_args = PyTuple_New(1); - PyTuple_SetItem(python_args, 0, python_value); - if (!python_args) - { - *error_message = "Can't build python arguments"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_class); - PyErr_Print(); - return false; - } - python_instance = PyEval_CallObject(python_class, python_args); - Py_DECREF(python_args); - Py_DECREF(python_class); - if (!python_instance) - { - *error_message = "Can't create \"HandEyeCalibrator\" python instance"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - PyErr_Print(); - return false; - } - - // Find handeye.calibrator.HandEyeCalibrator.add_sample method - python_func_add_sample = PyObject_GetAttrString(python_instance, "add_sample"); - if (!python_func_add_sample || !PyCallable_Check(python_func_add_sample)) - { - *error_message = "Can't find 'add_sample' method"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // Add sample poses to handeye.calibrator.HandEyeCalibrator instance - size_t number_of_poses = effector_wrt_world.size(); - PyArrayObject *numpy_arg_eef_wrt_base[number_of_poses], *numpy_arg_obj_wrt_sensor[number_of_poses]; - PyObject *python_array_eef_wrt_base[number_of_poses], *python_array_obj_wrt_sensor[number_of_poses]; - PyObject* python_args_sample[number_of_poses]; - npy_intp dims[2]{ TRANSFORM_MATRIX_DIMENSION, TRANSFORM_MATRIX_DIMENSION }; - const int number_of_dims{ 2 }; - // Using C array to store the pyarray data, which will be automatically freed - double c_arr_eef_wrt_world[number_of_poses][TRANSFORM_MATRIX_DIMENSION][TRANSFORM_MATRIX_DIMENSION]; - double c_arr_obj_wrt_sensor[number_of_poses][TRANSFORM_MATRIX_DIMENSION][TRANSFORM_MATRIX_DIMENSION]; - for (size_t i = 0; i < number_of_poses; ++i) - { - // Convert effector_wrt_world[i] from Eigen::isometry3d to C array - if (!toCArray(effector_wrt_world[i], c_arr_eef_wrt_world[i])) - { - *error_message = "Error converting Eigen::isometry3d to C array"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // From C array to PyArrayObject - python_array_eef_wrt_base[i] = - PyArray_SimpleNewFromData(number_of_dims, dims, NPY_DOUBLE, (void*)(c_arr_eef_wrt_world[i])); - if (!python_array_eef_wrt_base[i]) - { - *error_message = "Error creating PyArray object"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - numpy_arg_eef_wrt_base[i] = (PyArrayObject*)(python_array_eef_wrt_base[i]); - if (PyArray_NDIM(numpy_arg_eef_wrt_base[i]) == 2) // Check PyArrayObject dims are 4x4 - { - npy_intp* py_array_dims = PyArray_DIMS(numpy_arg_eef_wrt_base[i]); - if (py_array_dims[0] != 4 || py_array_dims[1] != 4) - { - *error_message = - "Error PyArrayObject dims: " + std::to_string(py_array_dims[0]) + "x" + std::to_string(py_array_dims[1]); - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(numpy_arg_eef_wrt_base[i]); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - return false; - } - } - - // Convert object_wrt_sensor[i] from Eigen::isometry3d to C array - if (!toCArray(object_wrt_sensor[i], c_arr_obj_wrt_sensor[i])) - { - *error_message = "Error converting Eigen::isometry3d to C array"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // From C array to PyArrayObject - python_array_obj_wrt_sensor[i] = - PyArray_SimpleNewFromData(number_of_dims, dims, NPY_DOUBLE, (void*)(c_arr_obj_wrt_sensor[i])); - if (!python_array_obj_wrt_sensor[i]) - { - *error_message = "Error creating PyArray object"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - numpy_arg_obj_wrt_sensor[i] = (PyArrayObject*)(python_array_obj_wrt_sensor[i]); - if (PyArray_NDIM(numpy_arg_obj_wrt_sensor[i]) == 2) // Check PyArrayObject dims are 4x4 - { - npy_intp* py_array_dims = PyArray_DIMS(numpy_arg_obj_wrt_sensor[i]); - if (py_array_dims[0] != 4 || py_array_dims[1] != 4) - { - *error_message = - "Error PyArrayObject dims: " + std::to_string(py_array_dims[0]) + "x" + std::to_string(py_array_dims[1]); - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(numpy_arg_obj_wrt_sensor[i]); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - return false; - } - } - - // Assign sample poses to 'HandEyeCalibrator' instance - python_args_sample[i] = Py_BuildValue("OO", numpy_arg_eef_wrt_base[i], numpy_arg_obj_wrt_sensor[i]); - if (!python_args_sample[i]) - { - *error_message = "Can't create argument tuple for 'add_sample' method"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - python_value = PyEval_CallObject(python_func_add_sample, python_args_sample[i]); - if (!python_value) - { - *error_message = "Error calling 'add_sample' method"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_func_add_sample); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - ROS_DEBUG_STREAM_NAMED(LOGNAME, "num_samples: " << PyInt_AsLong(python_value)); - Py_DECREF(python_value); - } - Py_DECREF(python_func_add_sample); - - // print the pair of transforms as python arguments - for (size_t i = 0; i < number_of_poses; i++) - { - std::stringstream ss; - ss << "\nnp_arg_eef_wrt_base"; - for (size_t m = 0; m < TRANSFORM_MATRIX_DIMENSION; m++) - { - ss << "\n"; - for (size_t n = 0; n < TRANSFORM_MATRIX_DIMENSION; n++) - ss << *(double*)PyArray_GETPTR2(numpy_arg_eef_wrt_base[i], m, n) << " "; - } - ss << "\nnp_arg_obj_wrt_sensor"; - for (size_t m = 0; m < TRANSFORM_MATRIX_DIMENSION; m++) - { - ss << "\n"; - for (size_t n = 0; n < TRANSFORM_MATRIX_DIMENSION; n++) - ss << *(double*)PyArray_GETPTR2(numpy_arg_obj_wrt_sensor[i], m, n) << " "; - } - ROS_DEBUG_STREAM_NAMED(LOGNAME, ss.str()); - } - - // Import handeye.solver python module - python_name = PyString_FromString("handeye.solver"); - python_module = PyImport_Import(python_name); - Py_DECREF(python_name); - if (!python_module) - { - *error_message = "Failed to load python module: handeye.solver"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // Find handeye.solver.solver_name class - python_class = PyObject_GetAttrString(python_module, solver_name.c_str()); - Py_DECREF(python_module); - if (!python_class || !PyCallable_Check(python_class)) - { - *error_message = "Can't find \"" + solver_name + "\" python class"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // Find handeye.calibrator.HandEyeCalibrator.solve method - python_func_solve = PyObject_GetAttrString(python_instance, "solve"); - if (!python_func_solve || !PyCallable_Check(python_func_solve)) - { - *error_message = "Can't find 'solve' method"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_class); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // Create argument list for 'solve' method - python_args = Py_BuildValue("{s:O}", "method", python_class); - Py_DECREF(python_class); - if (!python_args) - { - *error_message = "Can't create argument tuple for 'solve' method"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_instance); - PyErr_Print(); - return false; - } - - // Call 'solve' method to solve AX=XB problem - python_value = PyEval_CallObjectWithKeywords(python_func_solve, nullptr, python_args); - Py_DECREF(python_args); - Py_DECREF(python_func_solve); - for (size_t i = 0; i < number_of_poses; ++i) - Py_DECREF(python_args_sample[i]); - Py_DECREF(python_instance); - if (!python_value) - { - *error_message = "Error calling 'solve' method"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - PyErr_Print(); - return false; - } - PyArrayObject* np_ret = (PyArrayObject*)python_value; - if (!PyArray_Check(python_value) || PyArray_NDIM(np_ret) != 2 || PyArray_NBYTES(np_ret) != sizeof(double) * 16) - { - *error_message = "Did not return a valid array"; - ROS_ERROR_STREAM_NAMED(LOGNAME, *error_message); - Py_DECREF(python_value); - PyErr_Print(); - return false; - } - - std::stringstream ss; - ss << "\n Result camera-robot pose"; - for (size_t m = 0; m < TRANSFORM_MATRIX_DIMENSION; m++) - { - ss << "\n"; - for (size_t n = 0; n < TRANSFORM_MATRIX_DIMENSION; n++) - { - double item = *(double*)PyArray_GETPTR2(np_ret, m, n); - camera_robot_pose_(m, n) = item; - ss << item << " "; - } - } - ROS_DEBUG_STREAM_NAMED(LOGNAME, ss.str()); - - Py_DECREF(python_value); - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Python C API end"); - return true; -} - -bool HandEyeSolverDefault::toCArray(const Eigen::Isometry3d& pose, double (*c_arr)[TRANSFORM_MATRIX_DIMENSION]) const -{ - const Eigen::MatrixXd& mat = pose.matrix(); - - if (mat.rows() != TRANSFORM_MATRIX_DIMENSION || mat.cols() != TRANSFORM_MATRIX_DIMENSION) - { - ROS_ERROR_NAMED(LOGNAME, "Error matrix dims: %zux%zu, required %dx%d", mat.rows(), mat.cols(), - TRANSFORM_MATRIX_DIMENSION, TRANSFORM_MATRIX_DIMENSION); - return false; - } - - for (size_t i = 0; i < TRANSFORM_MATRIX_DIMENSION; ++i) - for (size_t j = 0; j < TRANSFORM_MATRIX_DIMENSION; ++j) - c_arr[i][j] = mat(i, j); - return true; -} - -} // namespace moveit_handeye_calibration diff --git a/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_opencv.cpp b/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_opencv.cpp new file mode 100644 index 0000000..5845ebd --- /dev/null +++ b/moveit_calibration_plugins/handeye_calibration_solver/src/handeye_solver_opencv.cpp @@ -0,0 +1,184 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, University of Luxembourg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Andrej Orsula */ + +#include +#include + +std::vector convertToCVMatrixRotation(const std::vector& transformations) +{ + std::vector cvMatrices; + + cv::Rect rotationRect(0, 0, 3, 3); + + for (auto& transformation : transformations) + { + cv::Mat cvTransformation(4, 4, CV_64F); + cv::eigen2cv(transformation.matrix(), cvTransformation); + cv::Mat cvRotation = cvTransformation(rotationRect); + cvMatrices.push_back(cvRotation); + } + + return cvMatrices; +} + +std::vector convertToCVMatrixTranslation(const std::vector& transformations) +{ + std::vector cvMatrices; + + cv::Rect translationRect(3, 0, 1, 3); + + for (auto& transformation : transformations) + { + cv::Mat cvTransformation(4, 4, CV_64F); + cv::eigen2cv(transformation.matrix(), cvTransformation); + cv::Mat cvTranslation = cvTransformation(translationRect); + cvMatrices.push_back(cvTranslation); + } + + return cvMatrices; +} + +Eigen::Isometry3d convertToIsometry(const cv::Mat& rotationMatrix, const cv::Mat& translationVector) +{ + cv::Rect rotationRect(0, 0, 3, 3); + cv::Rect translationRect(3, 0, 1, 3); + + cv::Mat T_cam2gripper = cv::Mat::zeros(4, 4, CV_64F); + rotationMatrix.copyTo(T_cam2gripper(rotationRect)); + translationVector.copyTo(T_cam2gripper(translationRect)); + + Eigen::Matrix4d transformationMatrix; + cv::cv2eigen(T_cam2gripper, transformationMatrix); + + Eigen::Isometry3d transformation = Eigen::Isometry3d(transformationMatrix); + + return transformation; +} + +namespace moveit_handeye_calibration +{ +void HandEyeSolverDefault::initialize() +{ + solver_names_ = { "Tsai1989", "Park1994", "Horaud1995", "Andreff1999", "Daniilidis1998" }; + solvers_["Tsai1989"] = cv::CALIB_HAND_EYE_TSAI; + solvers_["Park1994"] = cv::CALIB_HAND_EYE_PARK; + solvers_["Horaud1995"] = cv::CALIB_HAND_EYE_HORAUD; + solvers_["Andreff1999"] = cv::CALIB_HAND_EYE_ANDREFF; + solvers_["Daniilidis1998"] = cv::CALIB_HAND_EYE_DANIILIDIS; + camera_robot_pose_ = Eigen::Isometry3d::Identity(); +} + +const std::vector& HandEyeSolverDefault::getSolverNames() const +{ + return solver_names_; +} + +const Eigen::Isometry3d& HandEyeSolverDefault::getCameraRobotPose() const +{ + return camera_robot_pose_; +} + +bool HandEyeSolverDefault::solve(const std::vector& effector_wrt_world, + const std::vector& object_wrt_sensor, SensorMountType setup, + const std::string& solver_name, std::string* error_message) +{ + std::string local_error_message; + if (!error_message) + { + error_message = &local_error_message; + } + + // Check the size of the two sets of pose sample equal + if (effector_wrt_world.size() != object_wrt_sensor.size()) + { + *error_message = "The sizes of the two input pose sample vectors are not equal: effector_wrt_world.size() = " + + std::to_string(effector_wrt_world.size()) + + " and object_wrt_sensor.size() == " + std::to_string(object_wrt_sensor.size()); + RCLCPP_ERROR_STREAM(LOGGER_CALIBRATION_SOLVER, *error_message); + return false; + } + + // Determine method + cv::HandEyeCalibrationMethod solver_method; + if (std::find(solver_names_.begin(), solver_names_.end(), solver_name) == solver_names_.end()) + { + *error_message = "Unknown handeye solver name: " + solver_name; + RCLCPP_ERROR_STREAM(LOGGER_CALIBRATION_SOLVER, *error_message); + return false; + } + solver_method = solvers_[solver_name]; + + std::vector R_gripper2base, t_gripper2base, R_target2cam, t_target2cam; + + if (setup == EYE_IN_HAND) + { + auto T_gripper2base = effector_wrt_world; + R_gripper2base = convertToCVMatrixRotation(T_gripper2base); + t_gripper2base = convertToCVMatrixTranslation(T_gripper2base); + } + else if (setup == EYE_TO_HAND) + { + auto T_gripper2base = effector_wrt_world; + for (auto& T : T_gripper2base) + { + T = T.inverse(); + } + auto T_base2gripper = T_gripper2base; + R_gripper2base = convertToCVMatrixRotation(T_base2gripper); + t_gripper2base = convertToCVMatrixTranslation(T_base2gripper); + } + else + { + *error_message = "Invalid sensor mount configuration (must be eye-to-hand or eye-in-hand)"; + RCLCPP_ERROR_STREAM(LOGGER_CALIBRATION_SOLVER, *error_message); + return false; + } + + auto T_cam2target = object_wrt_sensor; + auto T_target2cam = T_cam2target; + R_target2cam = convertToCVMatrixRotation(T_target2cam); + t_target2cam = convertToCVMatrixTranslation(T_target2cam); + + cv::Mat R_cam2gripper, t_cam2gripper; + cv::calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper, + solver_method); + + camera_robot_pose_ = convertToIsometry(R_cam2gripper, t_cam2gripper); + + return true; +} + +} // namespace moveit_handeye_calibration diff --git a/moveit_calibration_plugins/handeye_calibration_solver/src/plugin_init.cpp b/moveit_calibration_plugins/handeye_calibration_solver/src/plugin_init.cpp index bfbb6f3..8688918 100644 --- a/moveit_calibration_plugins/handeye_calibration_solver/src/plugin_init.cpp +++ b/moveit_calibration_plugins/handeye_calibration_solver/src/plugin_init.cpp @@ -33,9 +33,7 @@ *********************************************************************/ /* Author: Yu Yan */ +#include +#include -#include -#include - -CLASS_LOADER_REGISTER_CLASS(moveit_handeye_calibration::HandEyeSolverDefault, - moveit_handeye_calibration::HandEyeSolverBase) +PLUGINLIB_EXPORT_CLASS(moveit_handeye_calibration::HandEyeSolverDefault, moveit_handeye_calibration::HandEyeSolverBase) diff --git a/moveit_calibration_plugins/handeye_calibration_solver/test/handeye_solver_test.cpp b/moveit_calibration_plugins/handeye_calibration_solver/test/handeye_solver_test.cpp index 7e13655..15f9c44 100644 --- a/moveit_calibration_plugins/handeye_calibration_solver/test/handeye_solver_test.cpp +++ b/moveit_calibration_plugins/handeye_calibration_solver/test/handeye_solver_test.cpp @@ -39,8 +39,10 @@ #include #include #include -#include +#include +#include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("handeye_solver_test"); class MoveItHandEyeSolverTester : public ::testing::Test { protected: @@ -51,17 +53,22 @@ class MoveItHandEyeSolverTester : public ::testing::Test { solver_plugins_loader_.reset(new pluginlib::ClassLoader( "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeSolverBase")); - solver_ = solver_plugins_loader_->createUniqueInstance("crigroup"); + // solver_plugins_loader_ = + // std::make_unique>("moveit_calibration_plugins", + // "moveit_handeye_calibration::HandEyeSolverBase"); + solver_ = solver_plugins_loader_->createUniqueInstance("OpenCV"); solver_->initialize(); } catch (const pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Exception while creating handeye target plugin: " << ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, "Exception while creating handeye target plugin: " << ex.what()); return; } Json::Reader reader; - std::string moveit_calibration_plugins_package_path = ros::package::getPath("moveit_calibration_plugins"); + // std::string moveit_calibration_plugins_package_path = ros::package::getPath("moveit_calibration_plugins"); + std::string moveit_calibration_plugins_package_path = + ament_index_cpp::get_package_share_directory("moveit_calibration_plugins"); moveit_calibration_plugins_package_path += "/handeye_calibration_solver/test/pose_samples.json"; std::ifstream ifs(moveit_calibration_plugins_package_path); @@ -72,10 +79,10 @@ class MoveItHandEyeSolverTester : public ::testing::Test solver_ok_ = true; } else - ROS_ERROR_STREAM("Can't parse json file: ./pose_samples.json"); + RCLCPP_ERROR_STREAM(LOGGER, "Can't parse json file: ./pose_samples.json"); } else - ROS_ERROR_STREAM("Can't load file: ./pose_samples.json"); + RCLCPP_ERROR_STREAM(LOGGER, "Can't load file: ./pose_samples.json"); } void TearDown() override @@ -153,6 +160,6 @@ TEST_F(MoveItHandEyeSolverTester, SolveAXEQXB) int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/moveit_calibration_plugins/handeye_calibration_solver_plugin_description.xml b/moveit_calibration_plugins/handeye_calibration_solver_plugin_description.xml index 3b5ef38..7ac8925 100644 --- a/moveit_calibration_plugins/handeye_calibration_solver_plugin_description.xml +++ b/moveit_calibration_plugins/handeye_calibration_solver_plugin_description.xml @@ -1,7 +1,7 @@ - - + + diff --git a/moveit_calibration_plugins/handeye_calibration_target/CMakeLists.txt b/moveit_calibration_plugins/handeye_calibration_target/CMakeLists.txt index 69af582..12069a8 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/CMakeLists.txt +++ b/moveit_calibration_plugins/handeye_calibration_target/CMakeLists.txt @@ -1,46 +1,78 @@ -set(HEADERS - include/moveit/handeye_calibration_target/handeye_target_base.h - include/moveit/handeye_calibration_target/handeye_target_aruco.h - include/moveit/handeye_calibration_target/handeye_target_charuco.h -) - -#catkin_lint: ignore_once missing_directory -include_directories(${CMAKE_CURRENT_BINARY_DIR}) - -# Plugin Source -set(SOURCE_FILES +set(MOVEIT_LIB_NAME moveit_handeye_calibration_target) +set(SOURCE_FILES_CORE src/handeye_target_aruco.cpp src/handeye_target_charuco.cpp ) +set(SOURCE_FILES_PLUGINS + src/plugin_init.cpp +) -set(MOVEIT_LIB_NAME moveit_handeye_calibration_target) -add_library(${MOVEIT_LIB_NAME}_core ${SOURCE_FILES} ${HEADERS}) +# Core library +add_library(${MOVEIT_LIB_NAME}_core SHARED ${SOURCE_FILES_CORE}) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME}_core ${OpenCV_LIBS} ${EIGEN3_LIBS}) +target_include_directories(${MOVEIT_LIB_NAME}_core PUBLIC + $ + $) +ament_target_dependencies( + ${MOVEIT_LIB_NAME}_core + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) -add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp) +# Plugin library +add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCE_FILES_PLUGINS}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC + $ + $) +ament_target_dependencies( + ${MOVEIT_LIB_NAME} + pluginlib +) -install(TARGETS ${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +include_directories( + SYSTEM + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) +install(DIRECTORY include/ DESTINATION include) install( - FILES - ./test/test_aruco_marker_detection.png - ./test/test_charuco_board_detection.jpg - DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION}/handeye_calibration_target/test/) + TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_include_directories( + include +) +ament_export_libraries( + {MOVEIT_LIB_NAME}_core {MOVEIT_LIB_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(jsoncpp REQUIRED) -if (CATKIN_ENABLE_TESTING) + get_target_property(JSON_INC_PATH jsoncpp_lib INTERFACE_INCLUDE_DIRECTORIES) + include_directories(${JSON_INC_PATH}) - catkin_add_gtest(test_handeye_target_aruco test/handeye_target_aruco_test.cpp) - target_link_libraries(test_handeye_target_aruco ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME} ${OpenCV_LIBS}) + install( + FILES + ./test/test_aruco_marker_detection.png + ./test/test_charuco_board_detection.jpg + DESTINATION + share/${PROJECT_NAME}/test + ) - catkin_add_gtest(test_handeye_target_charuco test/handeye_target_charuco_test.cpp) - target_link_libraries(test_handeye_target_charuco ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME} ${OpenCV_LIBS}) + ament_add_gtest(test_handeye_target_aruco test/handeye_target_aruco_test.cpp) + target_link_libraries(test_handeye_target_aruco ${MOVEIT_LIB_NAME} jsoncpp_lib) + ament_add_gtest(test_handeye_target_charuco test/handeye_target_charuco_test.cpp) + target_link_libraries(test_handeye_target_charuco ${MOVEIT_LIB_NAME} jsoncpp_lib) + ament_lint_auto_find_test_dependencies() endif() diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h index 833793e..b404579 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_aruco.h @@ -63,8 +63,14 @@ class HandEyeArucoTarget : public HandEyeTargetBase virtual bool setTargetDimension(double marker_measured_size, double marker_measured_separation); private: - // Predefined dictionary map - std::map marker_dictionaries_; + // Predefined ARUCO dictionaries in OpenCV for creating ARUCO marker board + const std::map ARUCO_DICTIONARY = { + { "DICT_4X4_250", cv::aruco::DICT_4X4_250 }, + { "DICT_5X5_250", cv::aruco::DICT_5X5_250 }, + { "DICT_6X6_250", cv::aruco::DICT_6X6_250 }, + { "DICT_7X7_250", cv::aruco::DICT_7X7_250 }, + { "DICT_ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL } + }; // Target intrinsic params int markers_x_; // Number of markers along X axis diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h index ffeaee5..2f78ea7 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h @@ -36,19 +36,29 @@ #pragma once -#include #include -#include +#include +// Eigen/Dense should be included before opencv stuff +// https://stackoverflow.com/questions/9876209/using-eigen-library-with-opencv-2-3-1 +#include +#include #include -#include + +#include +#include +#include #include #include -#include -#include -#include +#include namespace moveit_handeye_calibration { +namespace +{ +const rclcpp::Logger LOGGER_CALIBRATION_TARGET = rclcpp::get_logger("moveit_handeye_calibration_target"); +constexpr size_t LOG_THROTTLE_PERIOD = 2; +} // namespace + /** * @class HandEyeTargetBase * @brief Provides an interface for handeye calibration target detectors. @@ -77,7 +87,8 @@ class HandEyeTargetBase if (parameter_type_ == ParameterType::Int) value_.i = default_value; else - ROS_ERROR("Integer default value specified for non-integer parameter %s", name.c_str()); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, "Integer default value specified for non-integer parameter %s", + name.c_str()); } Parameter(std::string name, ParameterType parameter_type, float default_value = 0.) @@ -86,7 +97,8 @@ class HandEyeTargetBase if (parameter_type_ == ParameterType::Float) value_.f = default_value; else - ROS_ERROR("Float default value specified for non-float parameter %s", name.c_str()); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, "Float default value specified for non-float parameter %s", + name.c_str()); } Parameter(std::string name, ParameterType parameter_type, double default_value = 0.) @@ -95,7 +107,8 @@ class HandEyeTargetBase if (parameter_type_ == ParameterType::Float) value_.f = default_value; else - ROS_ERROR("Float default value specified for non-float parameter %s", name.c_str()); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, "Float default value specified for non-float parameter %s", + name.c_str()); } Parameter(std::string name, ParameterType parameter_type, std::vector enum_values, @@ -105,11 +118,11 @@ class HandEyeTargetBase if (default_option < enum_values_.size()) value_.e = default_option; else - ROS_ERROR("Invalid default option for enum parameter %s", name.c_str()); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, "Invalid default option for enum parameter %s", name.c_str()); } }; - const std::string LOGNAME = "handeye_target_base"; + rclcpp::Clock clock; const std::size_t CAMERA_MATRIX_VECTOR_DIMENSION = 9; // 3x3 camera intrinsic matrix const std::size_t CAMERA_MATRIX_WIDTH = 3; const std::size_t CAMERA_MATRIX_HEIGHT = 3; @@ -149,10 +162,10 @@ class HandEyeTargetBase * @param frame_id The name of the frame this transform is with respect to. * @return A `TransformStamped` message. */ - virtual geometry_msgs::TransformStamped getTransformStamped(const std::string& frame_id) const + virtual geometry_msgs::msg::TransformStamped getTransformStamped(const std::string& frame_id) const { - geometry_msgs::TransformStamped transform_stamped; - transform_stamped.header.stamp = ros::Time::now(); + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); // Not sure if this is the right approach transform_stamped.header.frame_id = frame_id; transform_stamped.child_frame_id = "handeye_target"; @@ -162,8 +175,8 @@ class HandEyeTargetBase return transform_stamped; } - // Convert cv::Vec3d rotation vector to geometry_msgs::Quaternion - geometry_msgs::Quaternion convertToQuaternionROSMsg(const cv::Vec3d& input_rvect) const + // Convert cv::Vec3d rotation vector to geometry_msgs::msg::Quaternion + geometry_msgs::msg::Quaternion convertToQuaternionROSMsg(const cv::Vec3d& input_rvect) const { cv::Mat cv_rotation_matrix; cv::Rodrigues(input_rvect, cv_rotation_matrix); @@ -173,12 +186,12 @@ class HandEyeTargetBase return tf2::toMsg(Eigen::Quaterniond(eigen_rotation_matrix)); } - // Convert cv::Vec3d translation vector to geometry_msgs::Vector3 - geometry_msgs::Vector3 convertToVectorROSMsg(const cv::Vec3d& input_tvect) const + // Convert cv::Vec3d translation vector to geometry_msgs::msg::Vector3 + geometry_msgs::msg::Vector3 convertToVectorROSMsg(const cv::Vec3d& input_tvect) const { Eigen::Vector3d eigen_tvect; cv::cv2eigen(input_tvect, eigen_tvect); - geometry_msgs::Vector3 msg_tvect; + geometry_msgs::msg::Vector3 msg_tvect; tf2::toMsg(eigen_tvect, msg_tvect); return msg_tvect; } @@ -210,25 +223,26 @@ class HandEyeTargetBase * @param msg Input camera info message. * @return True if the input camera info format is correct, false otherwise. */ - virtual bool setCameraIntrinsicParams(const sensor_msgs::CameraInfoConstPtr& msg) + virtual bool setCameraIntrinsicParams(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg) { if (!msg) { - ROS_ERROR_NAMED(LOGNAME, "CameraInfo msg is NULL."); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, "CameraInfo msg is NULL."); return false; } - if (msg->K.size() != CAMERA_MATRIX_VECTOR_DIMENSION) + if (msg->k.size() != CAMERA_MATRIX_VECTOR_DIMENSION) { - ROS_ERROR_NAMED(LOGNAME, "Invalid camera matrix dimension, current is %ld, required is %zu.", msg->K.size(), - CAMERA_MATRIX_VECTOR_DIMENSION); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, "Invalid camera matrix dimension, current is %ld, required is %zu.", + msg->k.size(), CAMERA_MATRIX_VECTOR_DIMENSION); return false; } - if (msg->D.size() != CAMERA_DISTORTION_VECTOR_DIMENSION) + if (msg->d.size() != CAMERA_DISTORTION_VECTOR_DIMENSION) { - ROS_ERROR_NAMED(LOGNAME, "Invalid distortion parameters dimension, current is %ld, required is %zu.", - msg->D.size(), CAMERA_DISTORTION_VECTOR_DIMENSION); + RCLCPP_ERROR(LOGGER_CALIBRATION_TARGET, + "Invalid distortion parameters dimension, current is %ld, required is %zu.", msg->d.size(), + CAMERA_DISTORTION_VECTOR_DIMENSION); return false; } @@ -239,17 +253,17 @@ class HandEyeTargetBase { for (size_t j = 0; j < CAMERA_MATRIX_HEIGHT; j++) { - camera_matrix_.at(i, j) = msg->K[i * CAMERA_MATRIX_WIDTH + j]; + camera_matrix_.at(i, j) = msg->k[i * CAMERA_MATRIX_WIDTH + j]; } } // Store camera distortion info for (size_t i = 0; i < CAMERA_DISTORTION_VECTOR_DIMENSION; i++) { - distortion_coeffs_.at(i, 0) = msg->D[i]; + distortion_coeffs_.at(i, 0) = msg->d[i]; } - ROS_DEBUG_STREAM_NAMED(LOGNAME, "Set camera intrinsic parameter to: " << *msg); + RCLCPP_DEBUG_STREAM(LOGGER_CALIBRATION_TARGET, "Set camera intrinsic parameter to: " << msg); return true; } diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h index 4806072..9e0049b 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_charuco.h @@ -63,8 +63,14 @@ class HandEyeCharucoTarget : public HandEyeTargetBase virtual bool setTargetDimension(double board_size_meters, double marker_size_meters); private: - // Predefined dictionary map - std::map marker_dictionaries_; + // Predefined ARUCO dictionaries in OpenCV for creating CHARUCO marker board + const std::map ARUCO_DICTIONARY = { + { "DICT_4X4_250", cv::aruco::DICT_4X4_250 }, + { "DICT_5X5_250", cv::aruco::DICT_5X5_250 }, + { "DICT_6X6_250", cv::aruco::DICT_6X6_250 }, + { "DICT_7X7_250", cv::aruco::DICT_7X7_250 }, + { "DICT_ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL } + }; // Target intrinsic params int squares_x_; // Number of squares along X axis diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp index c514bcd..9f54bb0 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp @@ -38,17 +38,6 @@ namespace moveit_handeye_calibration { -const std::string LOGNAME = "handeye_aruco_target"; - -// Predefined ARUCO dictionaries in OpenCV for creating ARUCO marker board -const std::map ARUCO_DICTIONARY = { - { "DICT_4X4_250", cv::aruco::DICT_4X4_250 }, - { "DICT_5X5_250", cv::aruco::DICT_5X5_250 }, - { "DICT_6X6_250", cv::aruco::DICT_6X6_250 }, - { "DICT_7X7_250", cv::aruco::DICT_7X7_250 }, - { "DICT_ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL } -}; - HandEyeArucoTarget::HandEyeArucoTarget() { parameters_.push_back(Parameter("markers, X", Parameter::ParameterType::Int, 3)); @@ -68,8 +57,6 @@ HandEyeArucoTarget::HandEyeArucoTarget() bool HandEyeArucoTarget::initialize() { - marker_dictionaries_ = ARUCO_DICTIONARY; - int markers_x; int markers_y; int marker_size; @@ -95,16 +82,16 @@ bool HandEyeArucoTarget::setTargetIntrinsicParams(int markers_x, int markers_y, int border_bits, const std::string& dictionary_id) { if (markers_x <= 0 || markers_y <= 0 || marker_size <= 0 || separation <= 0 || border_bits <= 0 || - marker_dictionaries_.find(dictionary_id) == marker_dictionaries_.end()) + ARUCO_DICTIONARY.find(dictionary_id) == ARUCO_DICTIONARY.end()) { - ROS_ERROR_STREAM_THROTTLE_NAMED(2., LOGNAME, - "Invalid target intrinsic params.\n" - << "markers_x_ " << std::to_string(markers_x) << "\n" - << "markers_y_ " << std::to_string(markers_y) << "\n" - << "marker_size " << std::to_string(marker_size) << "\n" - << "separation " << std::to_string(separation) << "\n" - << "border_bits " << std::to_string(border_bits) << "\n" - << "dictionary_id " << dictionary_id << "\n"); + RCLCPP_ERROR_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Invalid target intrinsic params.\n" + << "markers_x_ " << std::to_string(markers_x) << "\n" + << "markers_y_ " << std::to_string(markers_y) << "\n" + << "marker_size " << std::to_string(marker_size) << "\n" + << "separation " << std::to_string(separation) << "\n" + << "border_bits " << std::to_string(border_bits) << "\n" + << "dictionary_id " << dictionary_id << "\n"); return false; } @@ -115,7 +102,7 @@ bool HandEyeArucoTarget::setTargetIntrinsicParams(int markers_x, int markers_y, separation_ = separation; border_bits_ = border_bits; - const auto& it = marker_dictionaries_.find(dictionary_id); + const auto& it = ARUCO_DICTIONARY.find(dictionary_id); dictionary_id_ = it->second; return true; @@ -125,19 +112,20 @@ bool HandEyeArucoTarget::setTargetDimension(double marker_measured_size, double { if (marker_measured_size <= 0 || marker_measured_separation <= 0) { - ROS_ERROR_THROTTLE_NAMED(2., LOGNAME, "Invalid target measured dimensions: marker_size %f, marker_seperation %f", - marker_measured_size, marker_measured_separation); + RCLCPP_ERROR_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Invalid target measured dimensions: marker_size %f, marker_seperation %f", + marker_measured_size, marker_measured_separation); return false; } std::lock_guard aruco_lock(aruco_mutex_); marker_size_real_ = marker_measured_size; marker_separation_real_ = marker_measured_separation; - ROS_INFO_STREAM_THROTTLE_NAMED(2., LOGNAME, - "Set target real dimensions: \n" - << "marker_measured_size " << std::to_string(marker_measured_size) << "\n" - << "marker_measured_separation " << std::to_string(marker_measured_separation) - << "\n"); + RCLCPP_INFO_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Set target real dimensions: \n" + << "marker_measured_size " << std::to_string(marker_measured_size) << "\n" + << "marker_measured_separation " << std::to_string(marker_measured_separation) + << "\n"); return true; } @@ -159,7 +147,7 @@ bool HandEyeArucoTarget::createTargetImage(cv::Mat& image) const } catch (const cv::Exception& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Aruco target image creation exception: " << e.what()); + RCLCPP_ERROR_STREAM(LOGGER_CALIBRATION_TARGET, "Aruco target image creation exception: " << e.what()); return false; } @@ -189,7 +177,7 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::detectMarkers(image, dictionary, marker_corners, marker_ids, params_ptr); if (marker_ids.empty()) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(1., LOGNAME, "No aruco marker detected."); + RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, "No aruco marker detected."); return false; } @@ -205,7 +193,8 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image) // Draw the markers and frame axis if at least one marker is detected if (valid == 0) { - ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Cannot estimate aruco board pose."); + RCLCPP_WARN_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Cannot estimate aruco board pose."); return false; } @@ -213,7 +202,8 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image) std::log10(std::fabs(rotation_vect_[2])) > 10 || std::log10(std::fabs(translation_vect_[0])) > 10 || std::log10(std::fabs(translation_vect_[1])) > 10 || std::log10(std::fabs(translation_vect_[2])) > 10) { - ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Invalid target pose, please check CameraInfo msg."); + RCLCPP_WARN_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Invalid target pose, please check CameraInfo msg."); return false; } @@ -225,7 +215,8 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image) } catch (const cv::Exception& e) { - ROS_ERROR_STREAM_THROTTLE_NAMED(1., LOGNAME, "Aruco target detection exception: " << e.what()); + RCLCPP_ERROR_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Aruco target detection exception: " << e.what()); return false; } diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp index 5d44c64..ef864ce 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp @@ -38,17 +38,6 @@ namespace moveit_handeye_calibration { -const std::string LOGNAME = "handeye_charuco_target"; - -// Predefined ARUCO dictionaries in OpenCV for creating ARUCO marker board -const std::map ARUCO_DICTIONARY = { - { "DICT_4X4_250", cv::aruco::DICT_4X4_250 }, - { "DICT_5X5_250", cv::aruco::DICT_5X5_250 }, - { "DICT_6X6_250", cv::aruco::DICT_6X6_250 }, - { "DICT_7X7_250", cv::aruco::DICT_7X7_250 }, - { "DICT_ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL } -}; - HandEyeCharucoTarget::HandEyeCharucoTarget() { parameters_.push_back(Parameter("squares, X", Parameter::ParameterType::Int, 5)); @@ -69,8 +58,6 @@ HandEyeCharucoTarget::HandEyeCharucoTarget() bool HandEyeCharucoTarget::initialize() { - marker_dictionaries_ = ARUCO_DICTIONARY; - int squares_x; int squares_y; int marker_size_pixels; @@ -100,17 +87,17 @@ bool HandEyeCharucoTarget::setTargetIntrinsicParams(int squares_x, int squares_y { if (squares_x <= 0 || squares_y <= 0 || marker_size_pixels <= 0 || square_size_pixels <= 0 || margin_size_pixels < 0 || border_size_bits <= 0 || square_size_pixels <= marker_size_pixels || - 0 == marker_dictionaries_.count(dictionary_id)) + 0 == ARUCO_DICTIONARY.count(dictionary_id)) { - ROS_ERROR_STREAM_THROTTLE_NAMED(2., LOGNAME, - "Invalid target intrinsic params.\n" - << "squares_x " << std::to_string(squares_x) << "\n" - << "squares_y " << std::to_string(squares_y) << "\n" - << "marker_size_pixels " << std::to_string(marker_size_pixels) << "\n" - << "square_size_pixels " << std::to_string(square_size_pixels) << "\n" - << "border_size_bits " << std::to_string(border_size_bits) << "\n" - << "margin_size_pixels " << std::to_string(margin_size_pixels) << "\n" - << "dictionary_id " << dictionary_id << "\n"); + RCLCPP_ERROR_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Invalid target intrinsic params.\n" + << "squares_x " << std::to_string(squares_x) << "\n" + << "squares_y " << std::to_string(squares_y) << "\n" + << "marker_size_pixels " << std::to_string(marker_size_pixels) << "\n" + << "square_size_pixels " << std::to_string(square_size_pixels) << "\n" + << "border_size_bits " << std::to_string(border_size_bits) << "\n" + << "margin_size_pixels " << std::to_string(margin_size_pixels) << "\n" + << "dictionary_id " << dictionary_id << "\n"); return false; } @@ -122,7 +109,7 @@ bool HandEyeCharucoTarget::setTargetIntrinsicParams(int squares_x, int squares_y border_size_bits_ = border_size_bits; margin_size_pixels_ = margin_size_pixels; - const auto& it = marker_dictionaries_.find(dictionary_id); + const auto& it = ARUCO_DICTIONARY.find(dictionary_id); dictionary_id_ = it->second; return true; @@ -134,18 +121,18 @@ bool HandEyeCharucoTarget::setTargetDimension(double board_size_meters, double m if (board_size_meters <= 0 || marker_size_meters <= 0 || board_size_meters < marker_size_meters * std::max(squares_x_, squares_y_)) { - ROS_ERROR_THROTTLE_NAMED(2., LOGNAME, - "Invalid target measured dimensions. Longest board dimension: %f. Marker size: %f", - board_size_meters, marker_size_meters); + RCLCPP_ERROR_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Invalid target measured dimensions. Longest board dimension: %f. Marker size: %f", + board_size_meters, marker_size_meters); return false; } std::lock_guard charuco_lock(charuco_mutex_); - ROS_INFO_STREAM_THROTTLE_NAMED(2., LOGNAME, - "Set target real dimensions: \n" - << "board_size_meters " << std::to_string(board_size_meters) << "\n" - << "marker_size_meters " << std::to_string(marker_size_meters) << "\n" - << "\n"); + RCLCPP_INFO_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, LOG_THROTTLE_PERIOD, + "Set target real dimensions: \n" + << "board_size_meters " << std::to_string(board_size_meters) << "\n" + << "marker_size_meters " << std::to_string(marker_size_meters) << "\n" + << "\n"); board_size_meters_ = board_size_meters; marker_size_meters_ = marker_size_meters; return true; @@ -171,7 +158,7 @@ bool HandEyeCharucoTarget::createTargetImage(cv::Mat& image) const } catch (const cv::Exception& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "ChArUco target image creation exception: " << e.what()); + RCLCPP_ERROR_STREAM(LOGGER_CALIBRATION_TARGET, "ChArUco target image creation exception: " << e.what()); return false; } @@ -204,7 +191,8 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::detectMarkers(image, dictionary, marker_corners, marker_ids, params_ptr); if (marker_ids.empty()) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(1., LOGNAME, "No aruco marker detected. Dictionary ID: " << dictionary_id_); + RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, 1, + "No aruco marker detected. Dictionary ID: " << dictionary_id_); return false; } @@ -221,14 +209,15 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) // Draw the markers and frame axis if at least one marker is detected if (!valid) { - ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Cannot estimate aruco board pose."); + RCLCPP_WARN_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, 1, "Cannot estimate aruco board pose."); return false; } if (cv::norm(rotation_vect_) > 3.2 || std::log10(std::fabs(translation_vect_[0])) > 4 || std::log10(std::fabs(translation_vect_[1])) > 4 || std::log10(std::fabs(translation_vect_[2])) > 4) { - ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Invalid target pose, please check CameraInfo msg."); + RCLCPP_WARN_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, 1, + "Invalid target pose, please check CameraInfo msg."); return false; } @@ -240,7 +229,8 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) } catch (const cv::Exception& e) { - ROS_ERROR_STREAM_THROTTLE_NAMED(1., LOGNAME, "ChArUco target detection exception: " << e.what()); + RCLCPP_ERROR_STREAM_THROTTLE(LOGGER_CALIBRATION_TARGET, clock, 1, + "ChArUco target detection exception: " << e.what()); return false; } diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/plugin_init.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/plugin_init.cpp index 55fe61e..4661a91 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/plugin_init.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/plugin_init.cpp @@ -34,11 +34,9 @@ /* Author: Yu Yan */ -#include +#include #include #include -CLASS_LOADER_REGISTER_CLASS(moveit_handeye_calibration::HandEyeArucoTarget, - moveit_handeye_calibration::HandEyeTargetBase) -CLASS_LOADER_REGISTER_CLASS(moveit_handeye_calibration::HandEyeCharucoTarget, - moveit_handeye_calibration::HandEyeTargetBase) +PLUGINLIB_EXPORT_CLASS(moveit_handeye_calibration::HandEyeArucoTarget, moveit_handeye_calibration::HandEyeTargetBase) +PLUGINLIB_EXPORT_CLASS(moveit_handeye_calibration::HandEyeCharucoTarget, moveit_handeye_calibration::HandEyeTargetBase) diff --git a/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_aruco_test.cpp b/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_aruco_test.cpp index 67a715e..9d37721 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_aruco_test.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_aruco_test.cpp @@ -36,14 +36,17 @@ #include #include -#include +#include +#include #include #include -#include +#include #include -#include +#include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("handeye_target_aruco_test"); + class MoveItHandEyeTargetTester : public ::testing::Test { protected: @@ -51,7 +54,6 @@ class MoveItHandEyeTargetTester : public ::testing::Test { try { - ros::Time::init(); target_plugins_loader_.reset(new pluginlib::ClassLoader( "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeTargetBase")); target_ = target_plugins_loader_->createUniqueInstance("HandEyeTarget/Aruco"); @@ -68,18 +70,18 @@ class MoveItHandEyeTargetTester : public ::testing::Test } catch (const pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Exception while creating handeye target plugin: " << ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, "Exception while creating handeye target plugin: " << ex.what()); return; } - std::string image_path = ros::package::getPath("moveit_calibration_plugins") + + std::string image_path = ament_index_cpp::get_package_share_directory("moveit_calibration_plugins") + "/handeye_calibration_target/test/test_aruco_marker_detection.png"; image_ = cv::imread(image_path, cv::IMREAD_COLOR); resource_ok_ = false; if (!image_.data) - ROS_ERROR_STREAM("Could not open or find the image file: " << image_path); + RCLCPP_ERROR_STREAM(LOGGER, "Could not open or find the image file: " << image_path); else resource_ok_ = true; } @@ -108,17 +110,17 @@ TEST_F(MoveItHandEyeTargetTester, InitOK) TEST_F(MoveItHandEyeTargetTester, DetectArucoMarkerPose) { // Set camera intrinsic parameters - sensor_msgs::CameraInfoPtr camera_info(new sensor_msgs::CameraInfo()); + sensor_msgs::msg::CameraInfo::Ptr camera_info(new sensor_msgs::msg::CameraInfo()); camera_info->height = 480; camera_info->width = 640; camera_info->header.frame_id = "camera_color_optical_frame"; camera_info->distortion_model = "plumb_bob"; - camera_info->D = std::vector{ 0.0, 0.0, 0.0, 0.0, 0.0 }; - camera_info->K = boost::array{ + camera_info->d = std::vector{ 0.0, 0.0, 0.0, 0.0, 0.0 }; + camera_info->k = std::array{ 618.6002197265625, 0.0, 321.9837646484375, 0.0, 619.1103515625, 241.1459197998047, 0.0, 0.0, 1.0 }; - camera_info->R = boost::array{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; - camera_info->P = boost::array{ + camera_info->r = std::array{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; + camera_info->p = std::array{ 618.6002197265625, 0.0, 321.9837646484375, 0.0, 0.0, 619.1103515625, 241.1459197998047, 0.0, 0.0, 0.0, 1.0, 0.0 }; ASSERT_TRUE(target_->setCameraIntrinsicParams(camera_info)); @@ -133,8 +135,7 @@ TEST_F(MoveItHandEyeTargetTester, DetectArucoMarkerPose) ASSERT_TRUE(target_->detectTargetPose(gray_image)); // Get translation and rotation part - geometry_msgs::TransformStamped camera_transform; - ros::Time::init(); + geometry_msgs::msg::TransformStamped camera_transform; camera_transform = target_->getTransformStamped(camera_info->header.frame_id); Eigen::Affine3d ret = tf2::transformToEigen(camera_transform); std::cout << "Translation:\n" diff --git a/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_charuco_test.cpp b/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_charuco_test.cpp index 2cd85bb..a14a79d 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_charuco_test.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_charuco_test.cpp @@ -36,14 +36,17 @@ #include #include -#include +#include +#include #include #include -#include +#include #include -#include +#include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("handeye_target_charuco_test"); + class MoveItHandEyeTargetTester : public ::testing::Test { protected: @@ -51,7 +54,6 @@ class MoveItHandEyeTargetTester : public ::testing::Test { try { - ros::Time::init(); target_plugins_loader_.reset(new pluginlib::ClassLoader( "moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeTargetBase")); target_ = target_plugins_loader_->createUniqueInstance("HandEyeTarget/Charuco"); @@ -69,18 +71,18 @@ class MoveItHandEyeTargetTester : public ::testing::Test } catch (const pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Exception while creating handeye target plugin: " << ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, "Exception while creating handeye target plugin: " << ex.what()); return; } - std::string image_path = ros::package::getPath("moveit_calibration_plugins") + + std::string image_path = ament_index_cpp::get_package_share_directory("moveit_calibration_plugins") + "/handeye_calibration_target/test/test_charuco_board_detection.jpg"; image_ = cv::imread(image_path, cv::IMREAD_COLOR); resource_ok_ = false; if (!image_.data) - ROS_ERROR_STREAM("Could not open or find the image file: " << image_path); + RCLCPP_ERROR_STREAM(LOGGER, "Could not open or find the image file: " << image_path); else resource_ok_ = true; } @@ -109,17 +111,17 @@ TEST_F(MoveItHandEyeTargetTester, InitOK) TEST_F(MoveItHandEyeTargetTester, DetectCharucoMarkerPose) { // Set camera intrinsic parameters - sensor_msgs::CameraInfoPtr camera_info(new sensor_msgs::CameraInfo()); + sensor_msgs::msg::CameraInfo::Ptr camera_info(new sensor_msgs::msg::CameraInfo()); camera_info->height = 480; camera_info->width = 640; camera_info->header.frame_id = "camera_color_optical_frame"; camera_info->distortion_model = "plumb_bob"; - camera_info->D = std::vector{ 0.15405498, -0.24916842, 0.00350791, -0.00110041, 0.0 }; - camera_info->K = - boost::array{ 590.6972346, 0.0, 322.33104773, 0.0, 592.84676713, 247.40030325, 0.0, 0.0, 1.0 }; - camera_info->R = boost::array{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; - camera_info->P = boost::array{ 590.6972346, 0.0, 322.33104773, 0.0, 0.0, 592.84676713, - 247.40030325, 0.0, 0.0, 0.0, 1.0, 0.0 }; + camera_info->d = std::vector{ 0.15405498, -0.24916842, 0.00350791, -0.00110041, 0.0 }; + camera_info->k = + std::array{ 590.6972346, 0.0, 322.33104773, 0.0, 592.84676713, 247.40030325, 0.0, 0.0, 1.0 }; + camera_info->r = std::array{ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; + camera_info->p = std::array{ 590.6972346, 0.0, 322.33104773, 0.0, 0.0, 592.84676713, + 247.40030325, 0.0, 0.0, 0.0, 1.0, 0.0 }; ASSERT_TRUE(target_->setCameraIntrinsicParams(camera_info)); // Check target image creation @@ -132,8 +134,7 @@ TEST_F(MoveItHandEyeTargetTester, DetectCharucoMarkerPose) ASSERT_TRUE(target_->detectTargetPose(gray_image)); // Get translation and rotation part - geometry_msgs::TransformStamped camera_transform; - ros::Time::init(); + geometry_msgs::msg::TransformStamped camera_transform; camera_transform = target_->getTransformStamped(camera_info->header.frame_id); Eigen::Affine3d ret = tf2::transformToEigen(camera_transform); std::cout << "Translation:\n" diff --git a/moveit_calibration_plugins/handeye_calibration_target_plugin_description.xml b/moveit_calibration_plugins/handeye_calibration_target_plugin_description.xml index 63be5f8..58c3199 100644 --- a/moveit_calibration_plugins/handeye_calibration_target_plugin_description.xml +++ b/moveit_calibration_plugins/handeye_calibration_target_plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/moveit_calibration_plugins/package.xml b/moveit_calibration_plugins/package.xml index 67be8f8..d507ba1 100644 --- a/moveit_calibration_plugins/package.xml +++ b/moveit_calibration_plugins/package.xml @@ -1,41 +1,38 @@ - + + moveit_calibration_plugins - 0.1.0 + 2.0.0 Plugins of MoveIt calibration - Yu Yan - Yu Yan - BSD - http://moveit.ros.org + https://moveit.ros.org/ https://github.com/ros-planning/moveit_calibration/issues https://github.com/ros-planning/moveit_calibration - catkin + ament_cmake + ament_cmake_ros - roscpp - rosconsole - pluginlib + eigen3_cmake_module + moveit_common + + eigen + geometry_msgs + libopencv-dev + pluginlib + rclcpp sensor_msgs tf2 tf2_eigen - tf2_geometry_msgs - libjsoncpp-dev - libopencv-dev - - eigen - handeye - criutils - baldor - - rosunit + ament_lint_auto + ament_lint_common + libjsoncpp-dev - - + ament_cmake + + -