From 5bb6596f2fb75b447a0dcab27a3ad38f9f4714a2 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 16:58:15 +0300 Subject: [PATCH 1/8] Port github actions --- .github/workflows/ci.yaml | 26 +++++++++++++++----------- .github/workflows/format.yaml | 3 --- .pre-commit-config.yaml | 7 ------- .repos | 5 +++++ 4 files changed, 20 insertions(+), 21 deletions(-) create mode 100644 .repos diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c80d6f4ee..d80c4d33b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -14,32 +14,36 @@ jobs: fail-fast: false matrix: env: - - IMAGE: melodic-source + # TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers + - IMAGE: galactic-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - - IMAGE: master-source + - IMAGE: rolling-source CXXFLAGS: >- - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls + -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter - - IMAGE: noetic-source + - IMAGE: rolling-source CXX: clang++ CLANG_TIDY: true CXXFLAGS: >- - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls + -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy - - IMAGE: noetic-source + # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense + # see https://github.com/google/sanitizers/wiki/AddressSanitizer + - IMAGE: rolling-source NAME: asan DOCKER_RUN_OPTS: >- -e PRELOAD=libasan.so.5 - -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions" + -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0" TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" env: - CATKIN_LINT: true CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file - DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }} + DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }} UNDERLAY: /root/ws_moveit/install - DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" + # TODO: Port to ROS2 + # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" + UPSTREAM_WORKSPACE: .repos CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}" @@ -48,7 +52,7 @@ jobs: CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} - name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" + name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 05f532bef..39ea78667 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -17,9 +17,6 @@ jobs: - uses: actions/setup-python@v2 - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - - uses: rhaschke/install-catkin_lint-action@v1.0 - with: - distro: noetic - uses: pre-commit/action@v2.0.0 id: precommit - name: Upload pre-commit changes diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index decc953aa..df17afc78 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -42,10 +42,3 @@ repos: 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'] - - 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/.repos b/.repos new file mode 100644 index 000000000..54a045dec --- /dev/null +++ b/.repos @@ -0,0 +1,5 @@ +repositories: + rosparam_shortcuts: + type: git + url: https://github.com/PickNikRobotics/rosparam_shortcuts + version: ros2 From e095e501ecc606571295711a9bac5ec4478953cf Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 16:58:42 +0300 Subject: [PATCH 2/8] Port msgs to ROS2 --- msgs/CMakeLists.txt | 57 ++++++++++++++++++++++++--------------------- msgs/package.xml | 15 ++++++++---- 2 files changed, 42 insertions(+), 30 deletions(-) diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index b72f07873..aaabf3498 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -1,38 +1,43 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_msgs) -set(MSG_DEPS moveit_msgs visualization_msgs) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS - message_generation - ${MSG_DEPS} +# ROS messages, services and actions +set(msg_files + "msg/Property.msg" + "msg/Solution.msg" + "msg/SolutionInfo.msg" + "msg/StageDescription.msg" + "msg/StageStatistics.msg" + "msg/SubSolution.msg" + "msg/SubTrajectory.msg" + "msg/TaskDescription.msg" + "msg/TaskStatistics.msg" ) -# ROS messages, services and actions -add_message_files(DIRECTORY msg FILES - Property.msg - Solution.msg - SolutionInfo.msg - StageDescription.msg - StageStatistics.msg - SubSolution.msg - SubTrajectory.msg - TaskDescription.msg - TaskStatistics.msg +set(srv_files + "srv/GetSolution.srv" ) -add_service_files(DIRECTORY srv FILES - GetSolution.srv +set(action_files + "action/ExecuteTaskSolution.action" ) -add_action_files(DIRECTORY action FILES - ExecuteTaskSolution.action +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + ${action_files} + DEPENDENCIES + builtin_interfaces + moveit_msgs + visualization_msgs ) -generate_messages(DEPENDENCIES ${MSG_DEPS}) +ament_export_dependencies(rosidl_default_runtime) -catkin_package( - CATKIN_DEPENDS - message_runtime - ${MSG_DEPS} -) +ament_package() diff --git a/msgs/package.xml b/msgs/package.xml index f0bf4ec57..b9ff68c86 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -1,4 +1,4 @@ - + moveit_task_constructor_msgs 0.0.0 Messages for MoveIt Task Pipeline @@ -7,11 +7,18 @@ Robert Haschke Michael Goerner - catkin + ament_cmake - message_generation moveit_msgs visualization_msgs - message_runtime + rosidl_default_runtime + + rosidl_default_generators + + rosidl_interface_packages + + + ament_cmake + From c62e7938aa9f897a14808879d3d19d6ba935f4b6 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 16:59:13 +0300 Subject: [PATCH 3/8] Port rviz_marker_tools to ROS2 --- rviz_marker_tools/CMakeLists.txt | 59 ++++++------- .../rviz_marker_tools/marker_creation.h | 55 ++++++------ rviz_marker_tools/package.xml | 14 +++- rviz_marker_tools/src/marker_creation.cpp | 83 ++++++++++--------- 4 files changed, 116 insertions(+), 95 deletions(-) diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index d7afda227..19b67eaa7 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -1,26 +1,17 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(rviz_marker_tools) -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - visualization_msgs - roscpp - rviz -) - -catkin_package( - LIBRARIES - ${PROJECT_NAME} - INCLUDE_DIRS - include - CATKIN_DEPENDS - geometry_msgs - visualization_msgs - roscpp - rviz -) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz2 REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2_eigen REQUIRED) -set(CMAKE_CXX_STANDARD 14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}) @@ -28,20 +19,32 @@ set(HEADERS ${PROJECT_INCLUDE}/marker_creation.h ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED ${HEADERS} src/marker_creation.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} visualization_msgs geometry_msgs Eigen3) target_include_directories(${PROJECT_NAME} - PUBLIC include - PRIVATE ${catkin_INCLUDE_DIRS} + PUBLIC + $ + $ ) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(eigen3_cmake_module) +ament_export_dependencies(Eigen3) +ament_export_dependencies(geometry_msgs) +ament_export_dependencies(visualization_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rviz2) +ament_export_dependencies(tf2_eigen) +ament_package() diff --git a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h index f4e3e55de..5533690a9 100644 --- a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h +++ b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h @@ -1,7 +1,7 @@ #pragma once -#include -#include +#include +#include #include namespace urdf { @@ -28,60 +28,63 @@ enum Color WHITE = 13, YELLOW = 14, }; -std_msgs::ColorRGBA getColor(Color color, double alpha = 1.0); -std_msgs::ColorRGBA& setColor(std_msgs::ColorRGBA& color, Color color_id, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(Color color, double alpha = 1.0); +std_msgs::msg::ColorRGBA& setColor(std_msgs::msg::ColorRGBA& color, Color color_id, double alpha = 1.0); -std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction); -std_msgs::ColorRGBA& brighten(std_msgs::ColorRGBA& color, double fraction); -std_msgs::ColorRGBA& darken(std_msgs::ColorRGBA& color, double fraction); +std_msgs::msg::ColorRGBA& interpolate(std_msgs::msg::ColorRGBA& color, const std_msgs::msg::ColorRGBA& other, + double fraction); +std_msgs::msg::ColorRGBA& brighten(std_msgs::msg::ColorRGBA& color, double fraction); +std_msgs::msg::ColorRGBA& darken(std_msgs::msg::ColorRGBA& color, double fraction); -geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second); -geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second); +geometry_msgs::msg::Pose composePoses(const geometry_msgs::msg::Pose& first, const Eigen::Isometry3d& second); +geometry_msgs::msg::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::msg::Pose& second); /** All routines only touch the geometry part of the marker * pose, color, namespace, id, etc need to be set externally */ /// create planes with corners (-1,-1) - (+1,+1) -visualization_msgs::Marker& makeXYPlane(visualization_msgs::Marker& m); -visualization_msgs::Marker& makeXZPlane(visualization_msgs::Marker& m); -visualization_msgs::Marker& makeYZPlane(visualization_msgs::Marker& m); +visualization_msgs::msg::Marker& makeXYPlane(visualization_msgs::msg::Marker& m); +visualization_msgs::msg::Marker& makeXZPlane(visualization_msgs::msg::Marker& m); +visualization_msgs::msg::Marker& makeYZPlane(visualization_msgs::msg::Marker& m); /// create a cone of given angle along the x-axis -visualization_msgs::Marker& makeCone(visualization_msgs::Marker& m, double angle); +visualization_msgs::msg::Marker& makeCone(visualization_msgs::msg::Marker& m, double angle); -visualization_msgs::Marker& makeSphere(visualization_msgs::Marker& m, double radius = 1.0); +visualization_msgs::msg::Marker& makeSphere(visualization_msgs::msg::Marker& m, double radius = 1.0); /// create a cylinder along z-axis -visualization_msgs::Marker& makeCylinder(visualization_msgs::Marker& m, double diameter, double height); +visualization_msgs::msg::Marker& makeCylinder(visualization_msgs::msg::Marker& m, double diameter, double height); /// create a box with given dimensions along x, y, z axes -visualization_msgs::Marker& makeBox(visualization_msgs::Marker& m, double x, double y, double z); +visualization_msgs::msg::Marker& makeBox(visualization_msgs::msg::Marker& m, double x, double y, double z); /// create a mesh marker -visualization_msgs::Marker& makeMesh(visualization_msgs::Marker& m, const std::string& filename, double sx = 1.0, - double sy = 1.0, double sz = 1.0); -inline visualization_msgs::Marker& makeMesh(visualization_msgs::Marker& m, const std::string& filename, double scale) { +visualization_msgs::msg::Marker& makeMesh(visualization_msgs::msg::Marker& m, const std::string& filename, + double sx = 1.0, double sy = 1.0, double sz = 1.0); +inline visualization_msgs::msg::Marker& makeMesh(visualization_msgs::msg::Marker& m, const std::string& filename, + double scale) { return makeMesh(m, filename, scale, scale, scale); } /// create an arrow with a start and end point -visualization_msgs::Marker& makeArrow(visualization_msgs::Marker& m, const Eigen::Vector3d& start_point, - const Eigen::Vector3d& end_point, double diameter, double head_length = 0.0); +visualization_msgs::msg::Marker& makeArrow(visualization_msgs::msg::Marker& m, const Eigen::Vector3d& start_point, + const Eigen::Vector3d& end_point, double diameter, double head_length = 0.0); /// create an arrow along x-axis -visualization_msgs::Marker& makeArrow(visualization_msgs::Marker& m, double scale = 1.0, bool tip_at_origin = false); +visualization_msgs::msg::Marker& makeArrow(visualization_msgs::msg::Marker& m, double scale = 1.0, + bool tip_at_origin = false); /// create text marker -visualization_msgs::Marker& makeText(visualization_msgs::Marker& m, const std::string& text); +visualization_msgs::msg::Marker& makeText(visualization_msgs::msg::Marker& m, const std::string& text); /// create marker from urdf::Geom -visualization_msgs::Marker& makeFromGeometry(visualization_msgs::Marker& m, const urdf::Geometry& geom); +visualization_msgs::msg::Marker& makeFromGeometry(visualization_msgs::msg::Marker& m, const urdf::Geometry& geom); template -void appendFrame(T& container, const geometry_msgs::PoseStamped& pose, double scale = 1.0, +void appendFrame(T& container, const geometry_msgs::msg::PoseStamped& pose, double scale = 1.0, const std::string& ns = "frame", double diameter_fraction = 0.1) { - visualization_msgs::Marker m; + visualization_msgs::msg::Marker m; makeCylinder(m, scale * diameter_fraction, scale); m.ns = ns; m.header = pose.header; diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index 17b0dc408..c53924e02 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -6,10 +6,18 @@ BSD Robert Haschke - catkin + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + eigen visualization_msgs geometry_msgs - roscpp - rviz + rclcpp + rviz2 + tf2_eigen + + + ament_cmake + diff --git a/rviz_marker_tools/src/marker_creation.cpp b/rviz_marker_tools/src/marker_creation.cpp index 8d68b2ffd..7221899de 100644 --- a/rviz_marker_tools/src/marker_creation.cpp +++ b/rviz_marker_tools/src/marker_creation.cpp @@ -1,13 +1,19 @@ #include #include +#if __has_include() +#include +#else #include -#include +#endif +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("rviz_marker_tools"); namespace vm = visualization_msgs; namespace rviz_marker_tools { -std_msgs::ColorRGBA& setColor(std_msgs::ColorRGBA& color, Color color_id, double alpha) { +std_msgs::msg::ColorRGBA& setColor(std_msgs::msg::ColorRGBA& color, Color color_id, double alpha) { switch (color_id) { case RED: color.r = 0.8; @@ -108,7 +114,8 @@ double interpolate(double start, double end, double fraction) { return start * (1.0 - fraction) + end * fraction; } -std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction) { +std_msgs::msg::ColorRGBA& interpolate(std_msgs::msg::ColorRGBA& color, const std_msgs::msg::ColorRGBA& other, + double fraction) { if (fraction < 0.0) fraction = 0.0; if (fraction > 1.0) @@ -120,40 +127,40 @@ std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::Col return color; } -std_msgs::ColorRGBA& brighten(std_msgs::ColorRGBA& color, double fraction) { - static std_msgs::ColorRGBA white; +std_msgs::msg::ColorRGBA& brighten(std_msgs::msg::ColorRGBA& color, double fraction) { + static std_msgs::msg::ColorRGBA white; if (white.r == 0.0) setColor(white, WHITE); return interpolate(color, white, fraction); } -std_msgs::ColorRGBA& darken(std_msgs::ColorRGBA& color, double fraction) { - static std_msgs::ColorRGBA black; +std_msgs::msg::ColorRGBA& darken(std_msgs::msg::ColorRGBA& color, double fraction) { + static std_msgs::msg::ColorRGBA black; return interpolate(color, black, fraction); } -std_msgs::ColorRGBA getColor(Color color, double alpha) { - std_msgs::ColorRGBA result; +std_msgs::msg::ColorRGBA getColor(Color color, double alpha) { + std_msgs::msg::ColorRGBA result; setColor(result, color, alpha); return result; } -geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second) { +geometry_msgs::msg::Pose composePoses(const geometry_msgs::msg::Pose& first, const Eigen::Isometry3d& second) { Eigen::Isometry3d result_eigen; tf2::fromMsg(first, result_eigen); result_eigen = result_eigen * second; return tf2::toMsg(result_eigen); } -geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second) { +geometry_msgs::msg::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::msg::Pose& second) { Eigen::Isometry3d result_eigen; tf2::fromMsg(second, result_eigen); result_eigen = first * result_eigen; return tf2::toMsg(result_eigen); } -void prepareMarker(vm::Marker& m, int marker_type) { - m.action = vm::Marker::ADD; +void prepareMarker(vm::msg::Marker& m, int marker_type) { + m.action = vm::msg::Marker::ADD; m.type = marker_type; m.points.clear(); m.colors.clear(); @@ -169,8 +176,8 @@ void prepareMarker(vm::Marker& m, int marker_type) { m.pose.orientation.w = 1.0; } -vm::Marker& makeXYPlane(vm::Marker& m) { - geometry_msgs::Point p[4]; +vm::msg::Marker& makeXYPlane(vm::msg::Marker& m) { + geometry_msgs::msg::Point p[4]; p[0].x = 1.0; p[0].y = 1.0; @@ -188,7 +195,7 @@ vm::Marker& makeXYPlane(vm::Marker& m) { p[3].y = -1.0; p[3].z = 0.0; - prepareMarker(m, vm::Marker::TRIANGLE_LIST); + prepareMarker(m, vm::msg::Marker::TRIANGLE_LIST); m.points.push_back(p[0]); m.points.push_back(p[1]); m.points.push_back(p[2]); @@ -199,7 +206,7 @@ vm::Marker& makeXYPlane(vm::Marker& m) { return m; } -vm::Marker& makeXZPlane(vm::Marker& m) { +vm::msg::Marker& makeXZPlane(vm::msg::Marker& m) { makeXYPlane(m); // swap y and z components of points for (auto& p : m.points) @@ -207,7 +214,7 @@ vm::Marker& makeXZPlane(vm::Marker& m) { return m; } -vm::Marker& makeYZPlane(vm::Marker& m) { +vm::msg::Marker& makeYZPlane(vm::msg::Marker& m) { makeXZPlane(m); // (additionally) swap x and y components of points for (auto& p : m.points) @@ -216,9 +223,9 @@ vm::Marker& makeYZPlane(vm::Marker& m) { } /// create a cone of given angle along the x-axis -vm::Marker makeCone(double angle, vm::Marker& m) { - prepareMarker(m, vm::Marker::TRIANGLE_LIST); - geometry_msgs::Point p[3]; +vm::msg::Marker makeCone(double angle, vm::msg::Marker& m) { + prepareMarker(m, vm::msg::Marker::TRIANGLE_LIST); + geometry_msgs::msg::Point p[3]; p[0].x = p[0].y = p[0].z = 0.0; p[1].x = p[2].x = 1.0; @@ -241,67 +248,67 @@ vm::Marker makeCone(double angle, vm::Marker& m) { return m; } -vm::Marker& makeSphere(vm::Marker& m, double radius) { +vm::msg::Marker& makeSphere(vm::msg::Marker& m, double radius) { m.scale.x = m.scale.y = m.scale.z = radius; - prepareMarker(m, vm::Marker::SPHERE); + prepareMarker(m, vm::msg::Marker::SPHERE); return m; } -vm::Marker& makeBox(vm::Marker& m, double x, double y, double z) { +vm::msg::Marker& makeBox(vm::msg::Marker& m, double x, double y, double z) { m.scale.x = x; m.scale.y = y; m.scale.z = z; - prepareMarker(m, vm::Marker::CUBE); + prepareMarker(m, vm::msg::Marker::CUBE); return m; } -vm::Marker& makeCylinder(vm::Marker& m, double diameter, double height) { +vm::msg::Marker& makeCylinder(vm::msg::Marker& m, double diameter, double height) { m.scale.x = m.scale.y = diameter; m.scale.z = height; - prepareMarker(m, vm::Marker::CYLINDER); + prepareMarker(m, vm::msg::Marker::CYLINDER); return m; } -vm::Marker& makeMesh(vm::Marker& m, const std::string& filename, double sx, double sy, double sz) { +vm::msg::Marker& makeMesh(vm::msg::Marker& m, const std::string& filename, double sx, double sy, double sz) { m.scale.x = sx; m.scale.y = sy; m.scale.z = sz; - prepareMarker(m, vm::Marker::MESH_RESOURCE); + prepareMarker(m, vm::msg::Marker::MESH_RESOURCE); m.mesh_resource = filename; m.mesh_use_embedded_materials = 1u; return m; } -vm::Marker& makeArrow(vm::Marker& m, const Eigen::Vector3d& start_point, const Eigen::Vector3d& end_point, - double diameter, double head_length) { +vm::msg::Marker& makeArrow(vm::msg::Marker& m, const Eigen::Vector3d& start_point, const Eigen::Vector3d& end_point, + double diameter, double head_length) { // scale.y is set according to default proportions in rviz/default_plugin/markers/arrow_marker.cpp#L61 // for the default head_length=0, the head length will keep the default proportion defined in arrow_marker.cpp#L106 m.scale.x = diameter; m.scale.y = 2 * diameter; m.scale.z = head_length; - prepareMarker(m, vm::Marker::ARROW); + prepareMarker(m, vm::msg::Marker::ARROW); m.points.resize(2); m.points[0] = tf2::toMsg(start_point); m.points[1] = tf2::toMsg(end_point); return m; } -vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) { +vm::msg::Marker& makeArrow(vm::msg::Marker& m, double scale, bool tip_at_origin) { m.scale.y = m.scale.z = 0.1 * scale; m.scale.x = scale; - prepareMarker(m, vm::Marker::ARROW); + prepareMarker(m, vm::msg::Marker::ARROW); if (tip_at_origin) m.pose = composePoses(m.pose, Eigen::Translation3d(-scale, 0, 0) * Eigen::Isometry3d::Identity()); return m; } -vm::Marker& makeText(vm::Marker& m, const std::string& text) { - prepareMarker(m, vm::Marker::TEXT_VIEW_FACING); +vm::msg::Marker& makeText(vm::msg::Marker& m, const std::string& text) { + prepareMarker(m, vm::msg::Marker::TEXT_VIEW_FACING); m.text = text; return m; } -vm::Marker& makeFromGeometry(vm::Marker& m, const urdf::Geometry& geom) { +vm::msg::Marker& makeFromGeometry(vm::msg::Marker& m, const urdf::Geometry& geom) { switch (geom.type) { case urdf::Geometry::SPHERE: { const urdf::Sphere& sphere = static_cast(geom); @@ -324,7 +331,7 @@ vm::Marker& makeFromGeometry(vm::Marker& m, const urdf::Geometry& geom) { break; } default: - ROS_WARN("Unsupported geometry type: %d", geom.type); + RCLCPP_WARN(LOGGER, "Unsupported geometry type: %d", geom.type); break; } From f21c8ccd351364fe810fe38eb9f5529bb8ee3a7d Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 16:59:24 +0300 Subject: [PATCH 4/8] Port core to ROS2 --- core/CMakeLists.txt | 65 ++++----- .../moveit/task_constructor/introspection.h | 23 ++-- .../moveit/task_constructor/marker_tools.h | 4 +- .../moveit/task_constructor/moveit_compat.h | 13 +- .../moveit/task_constructor/properties.h | 9 +- .../task_constructor/solvers/cartesian_path.h | 4 +- .../solvers/joint_interpolation.h | 4 +- .../solvers/pipeline_planner.h | 21 ++- .../solvers/planner_interface.h | 6 +- .../include/moveit/task_constructor/stage_p.h | 5 +- .../task_constructor/stages/compute_ik.h | 6 +- .../moveit/task_constructor/stages/connect.h | 4 +- .../stages/fix_collision_objects.h | 4 +- .../stages/fixed_cartesian_poses.h | 4 +- .../stages/generate_grasp_pose.h | 4 +- .../task_constructor/stages/generate_pose.h | 4 +- .../stages/modify_planning_scene.h | 8 +- .../task_constructor/stages/move_relative.h | 14 +- .../moveit/task_constructor/stages/move_to.h | 16 +-- .../task_constructor/stages/passthrough.h | 2 +- .../moveit/task_constructor/stages/pick.h | 14 +- .../task_constructor/stages/simple_grasp.h | 4 +- .../include/moveit/task_constructor/storage.h | 18 +-- core/include/moveit/task_constructor/task.h | 10 +- ...ion_planning_stages_plugin_description.xml | 2 +- core/package.xml | 19 +-- core/src/CMakeLists.txt | 20 ++- core/src/container.cpp | 19 +-- core/src/cost_terms.cpp | 11 +- core/src/introspection.cpp | 128 ++++++++++++------ core/src/marker_tools.cpp | 10 +- core/src/merge.cpp | 8 +- core/src/properties.cpp | 10 +- core/src/solvers/cartesian_path.cpp | 18 +-- core/src/solvers/joint_interpolation.cpp | 8 +- core/src/solvers/pipeline_planner.cpp | 56 +++++--- core/src/stage.cpp | 33 ++--- core/src/stages/CMakeLists.txt | 23 +++- core/src/stages/compute_ik.cpp | 59 ++++---- core/src/stages/connect.cpp | 18 +-- core/src/stages/current_state.cpp | 50 ++++--- core/src/stages/fix_collision_objects.cpp | 24 ++-- core/src/stages/fixed_cartesian_poses.cpp | 10 +- core/src/stages/generate_grasp_pose.cpp | 13 +- core/src/stages/generate_place_pose.cpp | 16 ++- core/src/stages/generate_pose.cpp | 8 +- core/src/stages/modify_planning_scene.cpp | 20 +-- core/src/stages/move_relative.cpp | 33 +++-- core/src/stages/move_to.cpp | 40 +++--- core/src/stages/passthrough.cpp | 2 - core/src/stages/pick.cpp | 7 +- core/src/stages/plugins.cpp | 2 +- core/src/stages/simple_grasp.cpp | 12 +- core/src/storage.cpp | 19 +-- core/src/task.cpp | 65 ++++++--- core/src/utils.cpp | 8 +- core/test/CMakeLists.txt | 66 +++++---- core/test/models.cpp | 7 +- core/test/models.h | 3 +- core/test/move_to.launch.py | 110 +++++++++++++++ core/test/move_to.test | 7 - core/test/pick_pa10.cpp | 22 +-- core/test/pick_pr2.cpp | 19 ++- core/test/pick_ur5.cpp | 19 ++- core/test/stage_mockups.h | 1 - core/test/test_cost_terms.cpp | 2 +- core/test/test_fallback.cpp | 6 +- core/test/test_move_to.cpp | 37 ++--- core/test/test_stage.cpp | 28 ++-- 69 files changed, 822 insertions(+), 542 deletions(-) create mode 100644 core/test/move_to.launch.py delete mode 100644 core/test/move_to.test diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index be97790b2..a7bcdfdd1 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -1,34 +1,22 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_core) +find_package(ament_cmake REQUIRED) + find_package(Boost REQUIRED) -find_package(catkin REQUIRED COMPONENTS - tf2_eigen - geometry_msgs - moveit_core - moveit_ros_planning - moveit_ros_planning_interface - moveit_task_constructor_msgs - roscpp - visualization_msgs - rviz_marker_tools -) - -catkin_package( - LIBRARIES - ${PROJECT_NAME} - ${PROJECT_NAME}_stages - ${PROJECT_NAME}_stage_plugins - INCLUDE_DIRS - include - CATKIN_DEPENDS - geometry_msgs - moveit_core - moveit_task_constructor_msgs - visualization_msgs -) - -set(CMAKE_CXX_STANDARD 14) +find_package(geometry_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_task_constructor_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_marker_tools REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(visualization_msgs REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() add_compile_options(-fvisibility-inlines-hidden) @@ -37,8 +25,21 @@ set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) add_subdirectory(src) add_subdirectory(test) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} +install(DIRECTORY include/ DESTINATION include PATTERN "*_p.h" EXCLUDE) -install(FILES - motion_planning_stages_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +pluginlib_export_plugin_description_file(${PROJECT_NAME} motion_planning_stages_plugin_description.xml) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME} ${PROJECT_NAME}_stages) +ament_export_dependencies(Boost) +ament_export_dependencies(geometry_msgs) +ament_export_dependencies(moveit_core) +ament_export_dependencies(moveit_ros_planning) +ament_export_dependencies(moveit_ros_planning_interface) +ament_export_dependencies(moveit_task_constructor_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rviz_marker_tools) +ament_export_dependencies(tf2_eigen) +ament_export_dependencies(visualization_msgs) +ament_package() diff --git a/core/include/moveit/task_constructor/introspection.h b/core/include/moveit/task_constructor/introspection.h index d57c05cec..c42e0209a 100644 --- a/core/include/moveit/task_constructor/introspection.h +++ b/core/include/moveit/task_constructor/introspection.h @@ -39,10 +39,10 @@ #pragma once #include -#include -#include -#include -#include +#include +#include +#include +#include #define DESCRIPTION_TOPIC "description" #define STATISTICS_TOPIC "statistics" @@ -72,13 +72,14 @@ class Introspection ~Introspection(); /// fill task description message for publishing the task configuration - moveit_task_constructor_msgs::TaskDescription& - fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg); + moveit_task_constructor_msgs::msg::TaskDescription& + fillTaskDescription(moveit_task_constructor_msgs::msg::TaskDescription& msg); /// publish detailed task description void publishTaskDescription(); /// fill task state message for publishing the current task state - moveit_task_constructor_msgs::TaskStatistics& fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg); + moveit_task_constructor_msgs::msg::TaskStatistics& + fillTaskStatistics(moveit_task_constructor_msgs::msg::TaskStatistics& msg); /// publish the current state of task void publishTaskState(); @@ -95,8 +96,8 @@ class Introspection void publishAllSolutions(bool wait = true); /// get solution - bool getSolution(moveit_task_constructor_msgs::GetSolution::Request& req, - moveit_task_constructor_msgs::GetSolution::Response& res); + bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req, + const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res); /// retrieve id of given stage uint32_t stageId(const moveit::task_constructor::Stage* const s) const; @@ -105,8 +106,8 @@ class Introspection uint32_t solutionId(const moveit::task_constructor::SolutionBase& s); private: - void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s); - void fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s); + void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::msg::StageStatistics& s); + void fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s); /// retrieve or set id of given stage uint32_t stageId(const moveit::task_constructor::Stage* const s); /// retrieve solution with given id diff --git a/core/include/moveit/task_constructor/marker_tools.h b/core/include/moveit/task_constructor/marker_tools.h index 5c47c0672..2ac47927f 100644 --- a/core/include/moveit/task_constructor/marker_tools.h +++ b/core/include/moveit/task_constructor/marker_tools.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include @@ -19,7 +19,7 @@ namespace moveit { namespace task_constructor { /** signature of callback function, passing the generated marker and the name of the robot link / scene object */ -using MarkerCallback = std::function; +using MarkerCallback = std::function; /** generate marker msgs to visualize the planning scene, calling the given callback for each of them * object_names: set of links to include (or all if empty) */ diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 91e7d12bc..1f41353be 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -44,16 +44,7 @@ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ major * 1'000'000 + minor * 1'000 + patch) -// use collision env instead of collision robot/world -#define MOVEIT_HAS_COLLISION_ENV MOVEIT_VERSION_GE(1, 1, 0) - -// cartesian interpolator got separated from RobotState at some point -#define MOVEIT_HAS_CARTESIAN_INTERPOLATOR MOVEIT_VERSION_GE(1, 1, 0) - -// isEmpty got split off into its own header -#define MOVEIT_HAS_MESSAGE_CHECKS MOVEIT_VERSION_GE(1, 1, 0) - // use object shape poses relative to a single object pose -#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) +#define MOVEIT_HAS_OBJECT_POSE 1 -#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) +#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 0 diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index c93870866..e217ad60f 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -45,7 +45,8 @@ #include #include #include -#include +#include +#include namespace moveit { namespace task_constructor { @@ -210,12 +211,12 @@ class PropertySerializer : protected PropertySerializerBase PropertySerializer() { insert(typeid(T), typeName(), &serialize, &deserialize); } template - static typename std::enable_if::value, std::string>::type typeName() { - return ros::message_traits::DataType::value(); + static typename std::enable_if::value, std::string>::type typeName() { + return rosidl_generator_traits::data_type(); } template - static typename std::enable_if::value, std::string>::type typeName() { + static typename std::enable_if::value, std::string>::type typeName() { return typeid(T).name(); } diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 72e63fe3a..c5f7cfaf9 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -63,12 +63,12 @@ class CartesianPath : public PlannerInterface bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 6f27c240e..9fa94113c 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -60,12 +60,12 @@ class JointInterpolationPlanner : public PlannerInterface bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 75af734b2..412d0efa5 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include namespace planning_pipeline { @@ -58,20 +59,25 @@ class PipelinePlanner : public PlannerInterface struct Specification { moveit::core::RobotModelConstPtr model; - std::string ns{ "move_group" }; + std::string ns{ "ompl" }; std::string pipeline{ "ompl" }; std::string adapter_param{ "request_adapters" }; }; - static planning_pipeline::PlanningPipelinePtr create(const moveit::core::RobotModelConstPtr& model) { + static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, + const moveit::core::RobotModelConstPtr& model) { Specification spec; spec.model = model; - return create(spec); + return create(node, spec); } - static planning_pipeline::PlanningPipelinePtr create(const Specification& spec); + static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec); - PipelinePlanner(const std::string& pipeline = "ompl"); + /** + * + * @param node used to load the parameters for the planning pipeline + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl"); PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); @@ -81,16 +87,17 @@ class PipelinePlanner : public PlannerInterface bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; protected: std::string pipeline_name_; planning_pipeline::PlanningPipelinePtr planner_; + rclcpp::Node::SharedPtr node_; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 65a7d3ac0..f67649328 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include #include @@ -82,13 +82,13 @@ class PlannerInterface virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; /// plan trajectory from current robot state to Cartesian target virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 87ed7b273..0dbd9f693 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -43,7 +43,7 @@ #include #include -#include +#include #include #include @@ -150,7 +150,7 @@ class StagePrivate void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { - ROS_DEBUG_STREAM_NAMED("Stage", "Computing stage '" << name() << "'"); + RCLCPP_DEBUG_STREAM(LOGGER, "Computing stage '" << name() << "'"); auto compute_start_time = std::chrono::steady_clock::now(); try { compute(); @@ -200,6 +200,7 @@ class StagePrivate InterfaceWeakPtr next_starts_; // interface to be used for sendForward() Introspection* introspection_; // task's introspection instance + inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage"); }; PIMPL_FUNCTIONS(Stage) std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index f2d3e5be2..3bb84a134 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -38,7 +38,7 @@ #include #include -#include +#include #include namespace moveit { @@ -82,7 +82,7 @@ class ComputeIK : public WrapperBase void setGroup(const std::string& group) { setProperty("group", group); } /// setters for IK frame - void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } + void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); template void setIKFrame(const T& p, const std::string& link) { @@ -97,7 +97,7 @@ class ComputeIK : public WrapperBase * This property should almost always be set in the InterfaceState sent by the child. * If possible, avoid setting it manually. */ - void setTargetPose(const geometry_msgs::PoseStamped& pose) { setProperty("target_pose", pose); } + void setTargetPose(const geometry_msgs::msg::PoseStamped& pose) { setProperty("target_pose", pose); } void setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame = ""); template void setTargetPose(const T& p, const std::string& frame = "") { diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index bb64dbb74..c8cf883d5 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -41,7 +41,7 @@ #include #include -#include +#include namespace moveit { namespace core { @@ -76,7 +76,7 @@ class Connect : public Connecting using GroupPlannerVector = std::vector >; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); - void setPathConstraints(moveit_msgs::Constraints path_constraints) { + void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h index f7a956f3e..823d85322 100644 --- a/core/include/moveit/task_constructor/stages/fix_collision_objects.h +++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include namespace moveit { @@ -55,7 +55,7 @@ class FixCollisionObjects : public PropagatingEitherWay void computeForward(const InterfaceState& from) override; void computeBackward(const InterfaceState& to) override; - void setDirection(const geometry_msgs::Vector3& dir) { setProperty("direction", dir); } + void setDirection(const geometry_msgs::msg::Vector3& dir) { setProperty("direction", dir); } void setMaxPenetration(double penetration) { setProperty("max_penetration", penetration); } private: diff --git a/core/include/moveit/task_constructor/stages/fixed_cartesian_poses.h b/core/include/moveit/task_constructor/stages/fixed_cartesian_poses.h index 985f497ad..98dbebc49 100644 --- a/core/include/moveit/task_constructor/stages/fixed_cartesian_poses.h +++ b/core/include/moveit/task_constructor/stages/fixed_cartesian_poses.h @@ -40,7 +40,7 @@ #include #include -#include +#include namespace moveit { namespace task_constructor { @@ -55,7 +55,7 @@ class FixedCartesianPoses : public MonitoringGenerator bool canCompute() const override; void compute() override; - void addPose(const geometry_msgs::PoseStamped& pose); + void addPose(const geometry_msgs::msg::PoseStamped& pose); protected: void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index f5ac57da9..98cc750f3 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -57,9 +57,9 @@ class GenerateGraspPose : public GeneratePose void setAngleDelta(double delta) { setProperty("angle_delta", delta); } void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); } - void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); } + void setPreGraspPose(const moveit_msgs::msg::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); } void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); } - void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); } + void setGraspPose(const moveit_msgs::msg::RobotState& grasp) { properties().set("grasp", grasp); } protected: void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/stages/generate_pose.h b/core/include/moveit/task_constructor/stages/generate_pose.h index 77a4eb949..51c051fd7 100644 --- a/core/include/moveit/task_constructor/stages/generate_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_pose.h @@ -40,7 +40,7 @@ #include #include -#include +#include namespace moveit { namespace task_constructor { @@ -55,7 +55,7 @@ class GeneratePose : public MonitoringGenerator bool canCompute() const override; void compute() override; - void setPose(const geometry_msgs::PoseStamped& pose) { setProperty("pose", pose); } + void setPose(const geometry_msgs::msg::PoseStamped& pose) { setProperty("pose", pose); } protected: void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 80ec9d4a3..1fc3b3cc8 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include namespace moveit { @@ -78,7 +78,7 @@ class ModifyPlanningScene : public PropagatingEitherWay /// attach or detach a list of objects to the given link void attachObjects(const Names& objects, const std::string& attach_link, bool attach); /// Add an object to the planning scene - void addObject(const moveit_msgs::CollisionObject& collision_object); + void addObject(const moveit_msgs::msg::CollisionObject& collision_object); /// Remove an object from the planning scene void removeObject(const std::string& object_name); @@ -135,7 +135,7 @@ class ModifyPlanningScene : public PropagatingEitherWay // list of objects to attach (true) / detach (false) to a given link std::map> attach_objects_; // list of objects to add / remove to the planning scene - std::vector collision_objects_; + std::vector collision_objects_; // list of objects to mutually struct CollisionMatrixPairs @@ -150,7 +150,7 @@ class ModifyPlanningScene : public PropagatingEitherWay protected: // apply stored modifications to scene InterfaceState apply(const InterfaceState& from, bool invert); - void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object); + void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::msg::CollisionObject& object); void attachObjects(planning_scene::PlanningScene& scene, const std::pair>& pair, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index 21855c865..9e20ec0e7 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -40,9 +40,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace moveit { namespace core { @@ -64,7 +64,7 @@ class MoveRelative : public PropagatingEitherWay void setGroup(const std::string& group) { setProperty("group", group); } /// setters for IK frame - void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } + void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); template void setIKFrame(const T& p, const std::string& link) { @@ -82,14 +82,14 @@ class MoveRelative : public PropagatingEitherWay setProperty("max_distance", max_distance); } - void setPathConstraints(moveit_msgs::Constraints path_constraints) { + void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } /// perform twist motion on specified link - void setDirection(const geometry_msgs::TwistStamped& twist) { setProperty("direction", twist); } + void setDirection(const geometry_msgs::msg::TwistStamped& twist) { setProperty("direction", twist); } /// translate link along given direction - void setDirection(const geometry_msgs::Vector3Stamped& direction) { setProperty("direction", direction); } + void setDirection(const geometry_msgs::msg::Vector3Stamped& direction) { setProperty("direction", direction); } /// move specified joint variables by given amount void setDirection(const std::map& joint_deltas) { setProperty("direction", joint_deltas); } diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 5968017b8..107bac3b6 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -40,9 +40,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace moveit { namespace core { @@ -63,7 +63,7 @@ class MoveTo : public PropagatingEitherWay void setGroup(const std::string& group) { setProperty("group", group); } /// setters for IK frame - void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } + void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); template void setIKFrame(const T& p, const std::string& link) { @@ -74,21 +74,21 @@ class MoveTo : public PropagatingEitherWay void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } /// move link to given pose - void setGoal(const geometry_msgs::PoseStamped& pose) { setProperty("goal", pose); } + void setGoal(const geometry_msgs::msg::PoseStamped& pose) { setProperty("goal", pose); } /// move link to given point, keeping current orientation - void setGoal(const geometry_msgs::PointStamped& point) { setProperty("goal", point); } + void setGoal(const geometry_msgs::msg::PointStamped& point) { setProperty("goal", point); } /// move joint model group to given named pose void setGoal(const std::string& named_joint_pose) { setProperty("goal", named_joint_pose); } /// move joints specified in msg to their target values - void setGoal(const moveit_msgs::RobotState& robot_state) { setProperty("goal", robot_state); } + void setGoal(const moveit_msgs::msg::RobotState& robot_state) { setProperty("goal", robot_state); } /// move joints by name to their mapped target value void setGoal(const std::map& joints); - void setPathConstraints(moveit_msgs::Constraints path_constraints) { + void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } diff --git a/core/include/moveit/task_constructor/stages/passthrough.h b/core/include/moveit/task_constructor/stages/passthrough.h index 61f1c40dc..a43139486 100644 --- a/core/include/moveit/task_constructor/stages/passthrough.h +++ b/core/include/moveit/task_constructor/stages/passthrough.h @@ -37,7 +37,7 @@ #include #include -#include +#include #include namespace moveit { diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index dca07f669..8ee67e4ee 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -38,7 +38,7 @@ #include #include -#include +#include namespace moveit { namespace task_constructor { @@ -82,9 +82,9 @@ class PickPlaceBase : public SerialContainer solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } - void setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + void setApproachRetract(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance); - void setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + void setLiftPlace(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance); void setLiftPlace(const std::map& joints); }; @@ -95,11 +95,11 @@ class Pick : public PickPlaceBase Pick(Stage::pointer&& grasp_stage = Stage::pointer(), const std::string& name = "pick") : PickPlaceBase(std::move(grasp_stage), name, true) {} - void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { + void setApproachMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) { setApproachRetract(motion, min_distance, max_distance); } - void setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { + void setLiftMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) { setLiftPlace(motion, min_distance, max_distance); } void setLiftMotion(const std::map& joints) { setLiftPlace(joints); } @@ -112,11 +112,11 @@ class Place : public PickPlaceBase Place(Stage::pointer&& ungrasp_stage = Stage::pointer(), const std::string& name = "place") : PickPlaceBase(std::move(ungrasp_stage), name, false) {} - void setRetractMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { + void setRetractMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) { setApproachRetract(motion, min_distance, max_distance); } - void setPlaceMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { + void setPlaceMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) { setLiftPlace(motion, min_distance, max_distance); } void setPlaceMotion(const std::map& joints) { setLiftPlace(joints); } diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 30e7a52be..6fd9786b4 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -38,7 +38,7 @@ #include #include -#include +#include #include namespace moveit { @@ -78,7 +78,7 @@ class SimpleGraspBase : public SerialContainer void setObject(const std::string& object) { setProperty("object", object); } /// set properties of IK solver - void setIKFrame(const geometry_msgs::PoseStamped& transform) { setProperty("ik_frame", transform); } + void setIKFrame(const geometry_msgs::msg::PoseStamped& transform) { setProperty("ik_frame", transform); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); template void setIKFrame(const T& t, const std::string& link) { diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2d3397bbe..dc278dd93 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -42,14 +42,15 @@ #include #include #include -#include -#include +#include +#include #include #include #include #include #include +#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -271,9 +272,9 @@ class SolutionBase const auto& markers() const { return markers_; } /// append this solution to Solution msg - virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution, + virtual void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution, Introspection* introspection = nullptr) const = 0; - void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; + void fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection = nullptr) const; /// required to dispatch to type-specific CostTerm methods via vtable virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0; @@ -293,7 +294,7 @@ class SolutionBase // comment for this solution, e.g. explanation of failure std::string comment_; // markers for this solution, e.g. target frame or collision indicators - std::deque markers_; + std::deque markers_; // begin and end InterfaceState of this solution/trajectory const InterfaceState* start_ = nullptr; @@ -313,7 +314,8 @@ class SubTrajectory : public SolutionBase robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } - void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; + void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, + Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -346,7 +348,7 @@ class SolutionSequence : public SolutionBase void push_back(const SolutionBase& solution); /// append all subsolutions to solution - void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -377,7 +379,7 @@ class WrappedSolution : public SolutionBase : SolutionBase(creator, cost), wrapped_(wrapped) {} explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) : WrappedSolution(creator, wrapped, wrapped->cost()) {} - void fillMessage(moveit_task_constructor_msgs::Solution& solution, + void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution, Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 6bac28d73..04ca2ceb5 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -42,11 +42,13 @@ #include "container.h" #include -#include +#include #include -#include +#include + +#include namespace moveit { namespace core { @@ -86,7 +88,7 @@ class Task : protected WrapperBase /// setting the robot model also resets the task void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); /// load robot model from given parameter - void loadRobotModel(const std::string& robot_description = "robot_description"); + void loadRobotModel(const rclcpp::Node::SharedPtr& node, const std::string& robot_description = "robot_description"); void add(Stage::pointer&& stage); void insert(Stage::pointer&& stage, int before = -1) override; @@ -121,7 +123,7 @@ class Task : protected WrapperBase /// interrupt current planning (or execution) void preempt(); /// execute solution, return the result - moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s); + moveit_msgs::msg::MoveItErrorCodes execute(const SolutionBase& s); /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream& os = std::cout) const; diff --git a/core/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 389c11ff0..c5b0d1017 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/core/package.xml b/core/package.xml index 8fbadef25..f3517c12c 100644 --- a/core/package.xml +++ b/core/package.xml @@ -7,13 +7,11 @@ Michael Goerner Robert Haschke - catkin - - roscpp - roscpp + ament_cmake tf2_eigen geometry_msgs + rclcpp moveit_core moveit_ros_planning moveit_ros_planning_interface @@ -21,11 +19,16 @@ visualization_msgs rviz_marker_tools - rosunit - rostest - moveit_resources_fanuc_moveit_config + ament_cmake_gmock + ament_cmake_gtest + + + + + + - + ament_cmake diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index d37ea27c0..68333667a 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED ${PROJECT_INCLUDE}/container.h ${PROJECT_INCLUDE}/container_p.h ${PROJECT_INCLUDE}/cost_queue.h @@ -35,16 +35,24 @@ add_library(${PROJECT_NAME} solvers/pipeline_planner.cpp solvers/joint_interpolation.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} + moveit_core + geometry_msgs + moveit_ros_planning + moveit_ros_planning_interface + moveit_task_constructor_msgs + rclcpp + rviz_marker_tools + visualization_msgs +) target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} ) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) add_subdirectory(stages) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/core/src/container.cpp b/core/src/container.cpp index 422dc92c7..af7daf0e9 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -107,7 +107,7 @@ void ContainerBasePrivate::compute() { } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure"); switch (child.pimpl()->interfaceFlags()) { case GENERATE: // just ignore: the pair of (new) states isn't known to us anyway @@ -115,11 +115,11 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); setStatus(from, InterfaceState::Status::FAILED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); setStatus(to, InterfaceState::Status::FAILED); break; @@ -127,11 +127,11 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState if (const Connecting* conn = dynamic_cast(&child)) { auto cimpl = conn->pimpl(); if (!cimpl->hasPendingOpposites(from)) { - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); setStatus(from, InterfaceState::Status::FAILED); } if (!cimpl->hasPendingOpposites(to)) { - ROS_DEBUG_STREAM_NAMED("Pruning", "prune forward branch"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune forward branch"); setStatus(to, InterfaceState::Status::FAILED); } } @@ -446,8 +446,9 @@ inline void updateStatePrios(const SolutionSequence::container_type& partial_sol } void SerialContainer::onNewSolution(const SolutionBase& current) { - ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '" - << current.creator()->name() << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), "'" << this->name() + << "' received solution of child stage '" + << current.creator()->name() << "'"); // failures should never trigger this callback assert(!current.isFailure()); @@ -907,7 +908,7 @@ void Merger::onNewSolution(const SolutionBase& s) { void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { const SubTrajectory* trajectory = dynamic_cast(&s); if (!trajectory || !trajectory->trajectory()) { - ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported"); + RCLCPP_ERROR(rclcpp::get_logger("Merger"), "Only simple, valid trajectories are supported"); return; } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 130a9bb3f..363bbb096 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -113,7 +113,7 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) if (traj == nullptr || traj->getWayPointCount() == 0) return 0.0; - std::vector joint_models; + std::vector joint_models; joint_models.reserve(joints.size()); const auto& first_waypoint = traj->getWayPoint(0); for (auto& joint : joints) { @@ -196,18 +196,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { collision_detection::DistanceResult result; if (with_world) -#if MOVEIT_HAS_COLLISION_ENV state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); -#else - state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(), - robot); -#endif else -#if MOVEIT_HAS_COLLISION_ENV state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); -#else - state->scene()->getCollisionRobot()->distanceSelf(request, result, robot); -#endif if (result.minimum_distance.distance <= 0) { return result.minimum_distance; diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 588b9e99d..8692a5747 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -39,11 +39,11 @@ #include #include #include -#include +#include -#include -#include -#include +#include +#include +#include #include #include @@ -64,34 +64,73 @@ std::string getTaskId(const TaskPrivate* task) { } } // namespace +/** + * A shared executor between all tasks' introspection, this class will keep track of the number of the added nodes and + * will only create a spinning thread when we have nodes in the executor, once all the nodes are removed from the + * executor it will stop the spinning thread, later on if a new node is added a spinning thread will be started again + */ +class IntrospectionExecutor +{ +public: + void add_node(const rclcpp::Node::SharedPtr& node) { + std::call_once(once_flag_, [this] { executor_ = rclcpp::executors::SingleThreadedExecutor::make_unique(); }); + std::lock_guard lock(mutex_); + executor_->add_node(node); + if (nodes_count_++ == 0) + executor_thread_ = std::thread([this] { executor_->spin(); }); + } + + void remove_node(const rclcpp::Node::SharedPtr& node) { + std::lock_guard lock(mutex_); + executor_->remove_node(node); + if (--nodes_count_ == 0) { + executor_->cancel(); + if (executor_thread_.joinable()) + executor_thread_.join(); + } + } + +private: + rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_; + std::thread executor_thread_; + size_t nodes_count_ = 0; + std::mutex mutex_; + std::once_flag once_flag_; +}; + class IntrospectionPrivate { public: - IntrospectionPrivate(const TaskPrivate* task, Introspection* self) - : nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace - , task_(task) - , task_id_(getTaskId(task)) { - task_description_publisher_ = - nh_.advertise(DESCRIPTION_TOPIC, 2, true); + IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : task_(task), task_id_(getTaskId(task)) { + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-r", "__node:=introspection_" + task_id_ }); + node_ = rclcpp::Node::make_shared("_", task_->ns(), options); + executor_.add_node(node_); + task_description_publisher_ = node_->create_publisher( + DESCRIPTION_TOPIC, rclcpp::QoS(2).transient_local()); // send reset message as early as possible to give subscribers time to see it indicateReset(); - task_statistics_publisher_ = - nh_.advertise(STATISTICS_TOPIC, 1, true); - solution_publisher_ = nh_.advertise(SOLUTION_TOPIC, 1, true); - - get_solution_service_ = - nh_.advertiseService(std::string(GET_SOLUTION_SERVICE "_") + task_id_, &Introspection::getSolution, self); + task_statistics_publisher_ = node_->create_publisher( + STATISTICS_TOPIC, rclcpp::QoS(1).transient_local()); + solution_publisher_ = node_->create_publisher( + SOLUTION_TOPIC, rclcpp::QoS(1).transient_local()); + get_solution_service_ = node_->create_service( + std::string(GET_SOLUTION_SERVICE "_") + task_id_, + std::bind(&Introspection::getSolution, self, std::placeholders::_1, std::placeholders::_2)); resetMaps(); } - ~IntrospectionPrivate() { indicateReset(); } + ~IntrospectionPrivate() { + indicateReset(); + executor_.remove_node(node_); + } void indicateReset() { // send empty task description message to indicate reset - ::moveit_task_constructor_msgs::TaskDescription msg; + ::moveit_task_constructor_msgs::msg::TaskDescription msg; msg.task_id = task_id_; - task_description_publisher_.publish(msg); + task_description_publisher_->publish(msg); } void resetMaps() { @@ -102,21 +141,22 @@ class IntrospectionPrivate id_solution_bimap_.clear(); } - ros::NodeHandle nh_; /// associated task const TaskPrivate* task_; const std::string task_id_; /// publish task detailed description and current state - ros::Publisher task_description_publisher_; - ros::Publisher task_statistics_publisher_; + rclcpp::Publisher::SharedPtr task_description_publisher_; + rclcpp::Publisher::SharedPtr task_statistics_publisher_; /// publish new solutions - ros::Publisher solution_publisher_; + rclcpp::Publisher::SharedPtr solution_publisher_; /// services to provide an individual Solution - ros::ServiceServer get_solution_service_; + rclcpp::Service::SharedPtr get_solution_service_; + rclcpp::Node::SharedPtr node_; + inline static IntrospectionExecutor executor_; /// mapping from stages to their id - std::map stage_to_id_map_; + std::map stage_to_id_map_; boost::bimap id_solution_bimap_; }; @@ -127,13 +167,13 @@ Introspection::~Introspection() { } void Introspection::publishTaskDescription() { - ::moveit_task_constructor_msgs::TaskDescription msg; - impl->task_description_publisher_.publish(fillTaskDescription(msg)); + ::moveit_task_constructor_msgs::msg::TaskDescription msg; + impl->task_description_publisher_->publish(fillTaskDescription(msg)); } void Introspection::publishTaskState() { - ::moveit_task_constructor_msgs::TaskStatistics msg; - impl->task_statistics_publisher_.publish(fillTaskStatistics(msg)); + ::moveit_task_constructor_msgs::msg::TaskStatistics msg; + impl->task_statistics_publisher_->publish(fillTaskStatistics(msg)); } void Introspection::reset() { @@ -145,7 +185,7 @@ void Introspection::registerSolution(const SolutionBase& s) { solutionId(s); } -void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) { +void Introspection::fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s) { s.fillMessage(msg, this); s.start()->scene()->getPlanningSceneMsg(msg.start_scene); @@ -153,9 +193,9 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co } void Introspection::publishSolution(const SolutionBase& s) { - moveit_task_constructor_msgs::Solution msg; + moveit_task_constructor_msgs::msg::Solution msg; fillSolution(msg, s); - impl->solution_publisher_.publish(msg); + impl->solution_publisher_->publish(msg); } void Introspection::publishAllSolutions(bool wait) { @@ -178,13 +218,13 @@ const SolutionBase* Introspection::solutionFromId(uint id) const { return it->second; } -bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request& req, - moveit_task_constructor_msgs::GetSolution::Response& res) { - const SolutionBase* solution = solutionFromId(req.solution_id); +bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req, + const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res) { + const SolutionBase* solution = solutionFromId(req->solution_id); if (!solution) return false; - fillSolution(res.solution, *solution); + fillSolution(res->solution, *solution); return true; } @@ -203,7 +243,7 @@ uint32_t Introspection::solutionId(const SolutionBase& s) { return result.first->first; } -void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s) { +void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::msg::StageStatistics& s) { // successful solutions for (const auto& solution : stage.solutions()) s.solved.push_back(solutionId(*solution)); @@ -216,18 +256,18 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc s.num_failed = stage.numFailures(); } -moveit_task_constructor_msgs::TaskDescription& -Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg) { +moveit_task_constructor_msgs::msg::TaskDescription& +Introspection::fillTaskDescription(moveit_task_constructor_msgs::msg::TaskDescription& msg) { ContainerBase::StageCallback stage_processor = [this, &msg](const Stage& stage, unsigned int /*depth*/) -> bool { // this method is called for each child stage of a given parent - moveit_task_constructor_msgs::StageDescription desc; + moveit_task_constructor_msgs::msg::StageDescription desc; desc.id = stageId(&stage); desc.name = stage.name(); desc.flags = stage.pimpl()->interfaceFlags(); // fill stage properties for (const auto& pair : stage.properties()) { - moveit_task_constructor_msgs::Property p; + moveit_task_constructor_msgs::msg::Property p; p.name = pair.first; p.description = pair.second.description(); p.type = pair.second.typeName(); @@ -251,11 +291,11 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription return msg; } -moveit_task_constructor_msgs::TaskStatistics& -Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg) { +moveit_task_constructor_msgs::msg::TaskStatistics& +Introspection::fillTaskStatistics(moveit_task_constructor_msgs::msg::TaskStatistics& msg) { ContainerBase::StageCallback stage_processor = [this, &msg](const Stage& stage, unsigned int /*depth*/) -> bool { // this method is called for each child stage of a given parent - moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg + moveit_task_constructor_msgs::msg::StageStatistics stat; // create new Stage msg stat.id = stageId(&stage); fillStageStatistics(stage, stat); diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp index 6dbb5e5d0..865122f57 100644 --- a/core/src/marker_tools.cpp +++ b/core/src/marker_tools.cpp @@ -1,7 +1,6 @@ #include #include #include -#include namespace vm = visualization_msgs; @@ -17,7 +16,7 @@ void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scen const std::vector* names = object_names.empty() ? &scene->getCollisionObjectMsg() : &link_names; for (const auto &name : *names) { - visualization_msgs::MarkerArray markers; + visualization_msgs::msg::MarkerArray markers; robot_state.getRobotMarkers(markers, {name}, false); for (auto &marker : markers.markers) callback(marker, name); @@ -25,8 +24,9 @@ void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scen */ } -visualization_msgs::Marker& createGeometryMarker(visualization_msgs::Marker& marker, const urdf::Geometry& geom, - const urdf::Pose& pose, const urdf::Color& color) { +visualization_msgs::msg::Marker& createGeometryMarker(visualization_msgs::msg::Marker& marker, + const urdf::Geometry& geom, const urdf::Pose& pose, + const urdf::Color& color) { rviz_marker_tools::makeFromGeometry(marker, geom); marker.pose.position.x = pose.position.x; marker.pose.position.y = pose.position.y; @@ -110,7 +110,7 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa if (!model) return; - visualization_msgs::Marker m; + visualization_msgs::msg::Marker m; m.header.frame_id = robot_state.getRobotModel()->getModelFrame(); // code adapted from rviz::RobotLink::createVisual() / createCollision() diff --git a/core/src/merge.cpp b/core/src/merge.cpp index a07115095..f43a00aac 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -106,7 +106,7 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -141,7 +141,7 @@ merge(const std::vector& sub_trajecto std::vector values; values.reserve(max_num_vars); - auto merged_state = std::make_shared(base_state); + auto merged_state = std::make_shared(base_state); while (true) { bool finished = true; size_t index = merged_traj->getWayPointCount(); @@ -151,7 +151,7 @@ merge(const std::vector& sub_trajecto continue; // no more waypoints in this sub solution finished = false; // there was a waypoint, continue while loop - const robot_state::RobotState& sub_state = sub->getWayPoint(index); + const moveit::core::RobotState& sub_state = sub->getWayPoint(index); sub_state.copyJointGroupPositions(sub->getGroup(), values); merged_state->setJointGroupPositions(sub->getGroup(), values); } @@ -162,7 +162,7 @@ merge(const std::vector& sub_trajecto // add waypoint without timing merged_traj->addSuffixWayPoint(merged_state, 0.0); // create new RobotState for next waypoint - merged_state = std::make_shared(*merged_state); + merged_state = std::make_shared(*merged_state); } // add timing diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 257d1455a..12b2bca8c 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -39,12 +39,12 @@ #include #include #include -#include +#include namespace moveit { namespace task_constructor { -const std::string LOGNAME = "Properties"; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("Properties"); class PropertyTypeRegistry { @@ -73,7 +73,7 @@ class PropertyTypeRegistry const Entry& entry(const std::type_index& type_index) const { auto it = types_.find(type_index); if (it == types_.end()) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name()); + RCLCPP_ERROR_STREAM(LOGGER, "Unregistered type: " << type_index.name()); return dummy_; } return it->second; @@ -289,8 +289,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } - ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": " - << Property::serialize(value)); + RCLCPP_DEBUG_STREAM(LOGGER, pair.first << ": " << p.initialized_from_ << " -> " << source << ": " + << Property::serialize(value)); p.setCurrentValue(value); p.initialized_from_ = source; } diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index e15306ab8..059e83487 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -41,14 +41,14 @@ #include #include -#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR #include -#endif namespace moveit { namespace task_constructor { namespace solvers { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("CartesianPath"); + CartesianPath::CartesianPath() { auto& p = properties(); p.declare("step_size", 0.01, "step size between consecutive waypoints"); @@ -61,10 +61,10 @@ void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); if (!link) { - ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName()); + RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName()); return false; } @@ -75,7 +75,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); planning_scene::PlanningScenePtr sandbox_scene = from->diff(); @@ -86,21 +86,15 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons const double* joint_positions) { state->setJointGroupPositions(jmg, joint_positions); state->update(); - return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) && + return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()) && kcs.decide(*state).satisfied; }; std::vector trajectory; -#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid); -#else - double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath( - jmg, trajectory, &link, target, true, props.get("step_size"), props.get("jump_threshold"), - is_valid); -#endif assert(!trajectory.empty()); // there should be at least the start state result = std::make_shared(sandbox_scene->getRobotModel(), jmg); diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 35b3935f2..0729b6f34 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -46,6 +46,8 @@ namespace moveit { namespace task_constructor { namespace solvers { +static const auto LOGGER = rclcpp::get_logger("JointInterpolationPlanner"); + JointInterpolationPlanner::JointInterpolationPlanner() { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); @@ -57,7 +59,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& /*path_constraints*/) { + const moveit_msgs::msg::Constraints& /*path_constraints*/) { const auto& props = properties(); // Get maximum joint distance @@ -101,7 +103,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { const auto start_time = std::chrono::steady_clock::now(); auto to{ from->diff() }; @@ -119,7 +121,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints - ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); + RCLCPP_WARN(LOGGER, "IK failed for pose target"); return false; } to->getCurrentStateNonConst().update(); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index d2d162957..4575a6f00 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -40,9 +40,14 @@ #include #include #include -#include +#include #include + +#if __has_include() +#include +#else #include +#endif namespace moveit { namespace task_constructor { @@ -52,10 +57,10 @@ struct PlannerCache { using PlannerID = std::tuple; using PlannerMap = std::map >; - using ModelList = std::list, PlannerMap> >; + using ModelList = std::list, PlannerMap> >; ModelList cache_; - PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) { + PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) { // find model in cache_ and remove expired entries while doing so ModelList::iterator model_it = cache_.begin(); while (model_it != cache_.end()) { @@ -74,17 +79,22 @@ struct PlannerCache } }; -planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePlanner::Specification& spec) { +planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node, + const PipelinePlanner::Specification& spec) { static PlannerCache cache; static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin"; - std::string pipeline_ns = spec.ns + "/planning_pipelines/" + spec.pipeline; + std::string pipeline_ns = spec.ns; + const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME; // fallback to old structure for pipeline parameters in MoveIt - if (!ros::NodeHandle(pipeline_ns).hasParam(PLUGIN_PARAMETER_NAME)) { - ROS_WARN("Failed to find '%s/%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME, - "Attempting to load pipeline from old parameter structure. Please update your MoveIt config."); - pipeline_ns = spec.ns; + if (!node->has_parameter(parameter_name)) { + node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING); + } + if (std::string parameter; !node->get_parameter(parameter_name, parameter)) { + RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME, + "Attempting to load pipeline from old parameter structure. Please update your MoveIt config."); + pipeline_ns = "move_group"; } PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param); @@ -93,7 +103,7 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePla planning_pipeline::PlanningPipelinePtr planner = entry.lock(); if (!planner) { // create new entry - planner = std::make_shared(spec.model, ros::NodeHandle(pipeline_ns), + planner = std::make_shared(spec.model, node, pipeline_ns, PLUGIN_PARAMETER_NAME, spec.adapter_param); // store in cache entry = planner; @@ -101,13 +111,14 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePla return planner; } -PipelinePlanner::PipelinePlanner(const std::string& pipeline_name) : pipeline_name_{ pipeline_name } { +PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) + : pipeline_name_{ pipeline_name }, node_(node) { auto& p = properties(); p.declare("planner", "", "planner id"); p.declare("num_planning_attempts", 1u, "number of planning attempts"); - p.declare("workspace_parameters", moveit_msgs::WorkspaceParameters(), - "allowed workspace of mobile base?"); + p.declare("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), + "allowed workspace of mobile base?"); p.declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); p.declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); @@ -120,7 +131,8 @@ PipelinePlanner::PipelinePlanner(const std::string& pipeline_name) : pipeline_na planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); } -PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) : PipelinePlanner() { +PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) + : PipelinePlanner(rclcpp::Node::SharedPtr()) { planner_ = planning_pipeline; } @@ -129,7 +141,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { Specification spec; spec.model = robot_model; spec.pipeline = pipeline_name_; - planner_ = create(spec); + planner_ = create(node_, spec); } else if (robot_model != planner_->getRobotModel()) { throw std::runtime_error( "The robot model of the planning pipeline isn't the same as the task's robot model -- " @@ -139,7 +151,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { planner_->publishReceivedRequests(properties().get("publish_planning_requests")); } -void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMap& p, +void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const PropertyMap& p, const moveit::core::JointModelGroup* jmg, double timeout) { req.group_name = jmg->getName(); req.planner_id = p.get("planner"); @@ -149,15 +161,15 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa req.num_planning_attempts = p.get("num_planning_attempts"); req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); - req.workspace_parameters = p.get("workspace_parameters"); + req.workspace_parameters = p.get("workspace_parameters"); } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); - moveit_msgs::MotionPlanRequest req; + moveit_msgs::msg::MotionPlanRequest req; initMotionPlanRequest(req, props, jmg, timeout); req.goal_constraints.resize(1); @@ -174,12 +186,12 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); - moveit_msgs::MotionPlanRequest req; + moveit_msgs::msg::MotionPlanRequest req; initMotionPlanRequest(req, props, jmg, timeout); - geometry_msgs::PoseStamped target; + geometry_msgs::msg::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); target.pose = tf2::toMsg(target_eigen); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2eaf6a436..946125bac 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -42,7 +42,7 @@ #include -#include +#include #include @@ -87,7 +87,7 @@ void InitStageException::append(InitStageException& other) { } const char* InitStageException::what() const noexcept { - static const char* msg = "Error initializing stage(s). ROS_ERROR_STREAM(e) for details."; + static const char* msg = "Error initializing stage(s). RCLCPP_ERROR_STREAM(e) for details."; return msg; } @@ -325,7 +325,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { impl->properties_.reset(); if (impl->parent()) { try { - ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Properties"), "init '" << name() << "'"); impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } catch (const Property::error& e) { std::ostringstream oss; @@ -795,6 +795,7 @@ std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { os << reset; return os; } +static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting"); Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {} @@ -809,7 +810,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta const planning_scene::PlanningSceneConstPtr& to = to_state.scene(); if (from->getWorld()->size() != to->getWorld()->size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects"); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of collision objects"); return false; } @@ -819,19 +820,19 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta const collision_detection::World::ObjectPtr& from_object = from_object_pair.second; const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name); if (!to_object) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object_name); return false; } #if MOVEIT_HAS_OBJECT_POSE if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object_name); return false; // transforms do not match } #endif if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object_name); return false; // shapes not matching } @@ -839,7 +840,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it) if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different shape pose: " << from_object_name); return false; // transforms do not match } } @@ -850,7 +851,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta from->getCurrentState().getAttachedBodies(from_attached); to->getCurrentState().getAttachedBodies(to_attached); if (from_attached.size() != to_attached.size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of objects"); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of objects"); return false; } @@ -860,18 +861,18 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return object->getName() == from_object->getName(); }); if (it == to_attached.cend()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName()); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object->getName()); return false; } const moveit::core::AttachedBody* to_object = *it; if (from_object->getAttachedLink() != to_object->getAttachedLink()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName() - << " attached to " << from_object->getAttachedLinkName() << " / " - << to_object->getAttachedLinkName()); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different attach links: " << from_object->getName() << " attached to " + << from_object->getAttachedLinkName() << " / " + << to_object->getAttachedLinkName()); return false; // links not matching } if (from_object->getShapes().size() != to_object->getShapes().size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName()); + RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object->getName()); return false; // shapes not matching } @@ -886,8 +887,8 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta #endif for (; from_it != from_end; ++from_it, ++to_it) if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { - ROS_DEBUG_STREAM_NAMED("Connecting", - name() << ": different pose of attached object shape: " << from_object->getName()); + RCLCPP_DEBUG_STREAM(LOGGER, name() + << ": different pose of attached object shape: " << from_object->getName()); return false; // transforms do not match } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 2becaa638..050e94014 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(${PROJECT_NAME}_stages +add_library(${PROJECT_NAME}_stages SHARED ${PROJECT_INCLUDE}/stages/modify_planning_scene.h ${PROJECT_INCLUDE}/stages/fix_collision_objects.h @@ -39,13 +39,24 @@ add_library(${PROJECT_NAME}_stages simple_grasp.cpp pick.cpp ) -target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME}) +ament_target_dependencies(${PROJECT_NAME}_stages + moveit_core + geometry_msgs + moveit_ros_planning + moveit_ros_planning_interface + moveit_task_constructor_msgs + rclcpp + tf2_eigen + visualization_msgs +) -add_library(${PROJECT_NAME}_stage_plugins +add_library(${PROJECT_NAME}_stage_plugins SHARED plugins.cpp ) -target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages) install(TARGETS ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 0e5a6b55c..b16990009 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -44,16 +44,22 @@ #include #include +#if __has_include() +#include +#else #include +#endif #include #include #include -#include +#include namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("ComputeIK"); + ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) { auto& p = properties(); p.declare("eef", "name of end-effector group"); @@ -65,19 +71,19 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB "minimum distance between seperate IK solutions for the same target"); // ik_frame and target_pose are read from the interface - p.declare("ik_frame", "frame to be moved towards goal pose"); - p.declare("target_pose", "goal pose for ik frame"); + p.declare("ik_frame", "frame to be moved towards goal pose"); + p.declare("target_pose", "goal pose for ik frame"); } void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = link; pose_msg.pose = tf2::toMsg(pose); setIKFrame(pose_msg); } void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) { - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = frame; pose_msg.pose = tf2::toMsg(pose); setTargetPose(pose_msg); @@ -91,11 +97,11 @@ namespace { // ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, - robot_state::RobotState& robot_state, Eigen::Isometry3d pose, - const robot_model::LinkModel* link, + moveit::core::RobotState& robot_state, Eigen::Isometry3d pose, + const moveit::core::LinkModel* link, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well - const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); + const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link); if (parent != link) // transform pose into pose suitable to place parent pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent); @@ -109,10 +115,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce while (parent) { pending_links.push_back(&parent->getName()); link = parent; - const robot_model::JointModel* joint = link->getParentJointModel(); + const moveit::core::JointModel* joint = link->getParentJointModel(); parent = joint->getParentLinkModel(); - if (joint->getType() != robot_model::JointModel::FIXED) { + if (joint->getType() != moveit::core::JointModel::FIXED) { for (const std::string* name : pending_links) acm.setDefaultEntry(*name, true); pending_links.clear(); @@ -240,21 +246,21 @@ void ComputeIK::compute() { std::string msg; if (!validateEEF(props, robot_model, eef_jmg, &msg)) { - ROS_WARN_STREAM_NAMED("ComputeIK", msg); + RCLCPP_WARN_STREAM(LOGGER, msg); return; } if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) { - ROS_WARN_STREAM_NAMED("ComputeIK", msg); + RCLCPP_WARN_STREAM(LOGGER, msg); return; } if (!eef_jmg && !jmg) { - ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined"); + RCLCPP_WARN_STREAM(LOGGER, "Neither eef nor group are well defined"); return; } properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout()); // extract target_pose - geometry_msgs::PoseStamped target_pose_msg = props.get("target_pose"); + geometry_msgs::msg::PoseStamped target_pose_msg = props.get("target_pose"); if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame target_pose_msg.header.frame_id = scene->getPlanningFrame(); @@ -262,8 +268,7 @@ void ComputeIK::compute() { tf2::fromMsg(target_pose_msg.pose, target_pose); if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) { if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) { - ROS_WARN_STREAM_NAMED("ComputeIK", - "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id); + RCLCPP_WARN_STREAM(LOGGER, "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id); return; } // transform target_pose w.r.t. planning frame @@ -271,25 +276,25 @@ void ComputeIK::compute() { } // determine IK link from ik_frame - const robot_model::LinkModel* link = nullptr; - geometry_msgs::PoseStamped ik_pose_msg; + const moveit::core::LinkModel* link = nullptr; + geometry_msgs::msg::PoseStamped ik_pose_msg; const boost::any& value = props.get("ik_frame"); if (value.empty()) { // property undefined // determine IK link from eef/group if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) : jmg->getOnlyOneEndEffectorTip())) { - ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link"); + RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link"); return; } ik_pose_msg.header.frame_id = link->getName(); ik_pose_msg.pose.orientation.w = 1.0; } else { - ik_pose_msg = boost::any_cast(value); + ik_pose_msg = boost::any_cast(value); Eigen::Isometry3d ik_pose; tf2::fromMsg(ik_pose_msg.pose, ik_pose); if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { - ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); + RCLCPP_WARN_STREAM(LOGGER, "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); return; } ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; @@ -302,17 +307,17 @@ void ComputeIK::compute() { // validate placed link for collisions collision_detection::CollisionResult collisions; - robot_state::RobotState sandbox_state{ scene->getCurrentState() }; + moveit::core::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); // markers used for failures - std::deque failure_markers; + std::deque failure_markers; // frames at target pose and ik frame rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); // visualize placed end-effector - auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) { + auto appender = [&failure_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) { marker.ns = "ik target"; marker.color.a *= 0.5; failure_markers.push_back(marker); @@ -338,7 +343,7 @@ void ComputeIK::compute() { std::vector compare_pose; const std::string& compare_pose_name = props.get("default_pose"); if (!compare_pose_name.empty()) { - robot_state::RobotState compare_state(robot_model); + moveit::core::RobotState compare_state(robot_model); compare_state.setToDefaultValues(jmg, compare_pose_name); compare_state.copyJointGroupPositions(jmg, compare_pose); } else @@ -348,7 +353,7 @@ void ComputeIK::compute() { IKSolutions ik_solutions; auto is_valid = [scene, ignore_collisions, min_solution_distance, - &ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, + &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { if (jmg->distance(joint_positions, sol.data()) < min_solution_distance) @@ -396,7 +401,7 @@ void ComputeIK::compute() { solution.markAsFailure(); // set scene's robot state - robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); + moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); solution_state.setJointGroupPositions(jmg, ik_solutions[i].data()); solution_state.update(); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 9aadd4b89..0fac2feb2 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -46,14 +46,16 @@ namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connect"); + Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); setCostTerm(std::make_unique()); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); - p.declare("path_constraints", moveit_msgs::Constraints(), - "constraints to maintain during trajectory"); + p.declare("path_constraints", moveit_msgs::msg::Constraints(), + "constraints to maintain during trajectory"); } void Connect::reset() { @@ -88,7 +90,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) { try { merged_jmg_.reset(task_constructor::merge(groups)); } catch (const std::runtime_error& e) { - ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging."); + RCLCPP_INFO_STREAM(LOGGER, this->name() << ": " << e.what() << ". Disabling merging."); } } @@ -119,8 +121,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& Eigen::Map positions_from(from.getJointPositions(jm), num); Eigen::Map positions_to(to.getJointPositions(jm), num); if (!(positions_from - positions_to).isZero(1e-4)) { - ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() - << "] != [" << positions_to.transpose() << "]"); + RCLCPP_INFO_STREAM(LOGGER, "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() + << "] != [" << positions_to.transpose() << "]"); return false; } } @@ -131,7 +133,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& props = properties(); double timeout = this->timeout(); MergeMode mode = props.get("merge_mode"); - const auto& path_constraints = props.get("path_constraints"); + const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); std::vector sub_trajectories; @@ -147,7 +149,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { planning_scene::PlanningScenePtr end = start->diff(); const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first); final_goal_state.copyJointGroupPositions(jmg, positions); - robot_state::RobotState& goal_state = end->getCurrentStateNonConst(); + moveit::core::RobotState& goal_state = end->getCurrentStateNonConst(); goal_state.setJointGroupPositions(jmg, positions); goal_state.update(); intermediate_scenes.push_back(end); @@ -225,7 +227,7 @@ SubTrajectoryPtr Connect::merge(const std::vectorisPathValid(*trajectory, - properties().get("path_constraints"))) + properties().get("path_constraints"))) return SubTrajectoryPtr(); return std::make_shared(trajectory); diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index 822dc5c34..ca48fb1b6 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -37,16 +37,18 @@ #include #include -#include -#include +#include +#include #include #include -#include +#include namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("CurrentState"); + CurrentState::CurrentState(const std::string& name) : Generator(name) { auto& p = properties(); Property& timeout = p.property("timeout"); @@ -67,31 +69,37 @@ bool CurrentState::canCompute() const { void CurrentState::compute() { scene_ = std::make_shared(robot_model_); - ros::NodeHandle h; - ros::ServiceClient client = h.serviceClient("get_planning_scene"); + // Add random ID to prevent warnings about multiple publishers within the same node + rclcpp::NodeOptions options; + options.arguments( + { "--ros-args", "-r", "__node:=current_state_" + std::to_string(reinterpret_cast(this)) }); + auto node = rclcpp::Node::make_shared("_", options); + auto client = node->create_client("get_planning_scene"); - ros::Duration timeout(this->timeout()); - if (client.waitForExistence(timeout)) { - moveit_msgs::GetPlanningScene::Request req; - moveit_msgs::GetPlanningScene::Response res; + auto timeout = std::chrono::duration(this->timeout()); + if (client->wait_for_service(timeout)) { + auto req = std::make_shared(); - req.components.components = - moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS | moveit_msgs::PlanningSceneComponents::ROBOT_STATE | - moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS | - moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | - moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP | - moveit_msgs::PlanningSceneComponents::TRANSFORMS | - moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX | - moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING | - moveit_msgs::PlanningSceneComponents::OBJECT_COLORS; + req->components.components = moveit_msgs::msg::PlanningSceneComponents::SCENE_SETTINGS | + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE | + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS | + moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES | + moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | + moveit_msgs::msg::PlanningSceneComponents::OCTOMAP | + moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS | + moveit_msgs::msg::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX | + moveit_msgs::msg::PlanningSceneComponents::LINK_PADDING_AND_SCALING | + moveit_msgs::msg::PlanningSceneComponents::OBJECT_COLORS; - if (client.call(req, res)) { - scene_->setPlanningSceneMsg(res.scene); + auto res_future = client->async_send_request(req); + if (rclcpp::spin_until_future_complete(node, res_future) == rclcpp::FutureReturnCode::SUCCESS) { + auto res = res_future.get(); + scene_->setPlanningSceneMsg(res->scene); spawn(InterfaceState(scene_), 0.0); return; } } - ROS_WARN("failed to acquire current PlanningScene"); + RCLCPP_WARN(LOGGER, "failed to acquire current PlanningScene"); } } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index cf382ea24..bfbff1fb7 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -44,9 +44,13 @@ #include #include -#include +#if __has_include() +#include +#else #include -#include +#endif +#include +#include namespace vm = visualization_msgs; namespace cd = collision_detection; @@ -55,13 +59,15 @@ namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixCollisionObjects"); + FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { // TODO: possibly weight solutions based on the required displacement? setCostTerm(std::make_unique(0.0)); auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); - p.declare("direction", "direction vector to use for corrections"); + p.declare("direction", "direction vector to use for corrections"); } void FixCollisionObjects::computeForward(const InterfaceState& from) { @@ -79,8 +85,7 @@ bool computeCorrection(const std::vector& contacts, Eigen::Vector3d correction.setZero(); for (const cd::Contact& c : contacts) { if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) { - ROS_WARN_STREAM_NAMED("FixCollisionObjects", - "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2); + RCLCPP_WARN_STREAM(LOGGER, "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2); return false; } if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT) @@ -110,20 +115,15 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene& req.verbose = false; req.distance = false; - vm::Marker m; + vm::msg::Marker m; m.header.frame_id = scene.getPlanningFrame(); m.ns = "collisions"; bool failure = false; while (!failure) { res.clear(); -#if MOVEIT_HAS_COLLISION_ENV scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(), scene.getAllowedCollisionMatrix()); -#else - scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), - scene.getCurrentState(), scene.getAllowedCollisionMatrix()); -#endif if (!res.collision) return result; @@ -147,7 +147,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene& // fix collision by shifting object along correction direction if (!dir.empty()) // if explicitly given, use this correction direction - tf2::fromMsg(boost::any_cast(dir), correction); + tf2::fromMsg(boost::any_cast(dir), correction); const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2; scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction))); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index 068f2759c..97ff83617 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -46,7 +46,9 @@ namespace moveit { namespace task_constructor { namespace stages { -using PosesList = std::vector; +static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixedCartesianPoses"); + +using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { setCostTerm(std::make_unique(0.0)); @@ -55,7 +57,7 @@ FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGe p.declare("poses", PosesList(), "target poses to spawn"); } -void FixedCartesianPoses::addPose(const geometry_msgs::PoseStamped& pose) { +void FixedCartesianPoses::addPose(const geometry_msgs::msg::PoseStamped& pose) { moveit::task_constructor::Property& poses = properties().property("poses"); if (!poses.defined()) poses.setValue(PosesList({ pose })); @@ -82,11 +84,11 @@ void FixedCartesianPoses::compute() { return; planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); - for (geometry_msgs::PoseStamped pose : properties().get("poses")) { + for (geometry_msgs::msg::PoseStamped pose : properties().get("poses")) { if (pose.header.frame_id.empty()) pose.header.frame_id = scene->getPlanningFrame(); else if (!scene->knowsFrameTransform(pose.header.frame_id)) { - ROS_WARN_NAMED("FixedCartesianPoses", "Unknown frame: '%s'", pose.header.frame_id.c_str()); + RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", pose.header.frame_id.c_str()); continue; } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 1d53ca4d2..cb6688227 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -43,12 +43,18 @@ #include #include +#if __has_include() +#include +#else #include +#endif namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("GenerateGraspPose"); + GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(name) { auto& p = properties(); p.declare("eef", "name of end-effector"); @@ -73,7 +79,8 @@ static void applyPreGrasp(moveit::core::RobotState& state, const moveit::core::J try { // try RobotState - const moveit_msgs::RobotState& robot_state_msg = boost::any_cast(diff_property.value()); + const moveit_msgs::msg::RobotState& robot_state_msg = + boost::any_cast(diff_property.value()); if (!robot_state_msg.is_diff) throw moveit::Exception{ "RobotState message must be a diff" }; const auto& accepted = jmg->getJointModelNames(); @@ -150,7 +157,7 @@ void GenerateGraspPose::compute() { const std::string& eef = props.get("eef"); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); - robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); + moveit::core::RobotState& robot_state = scene->getCurrentStateNonConst(); try { applyPreGrasp(robot_state, jmg, props.property("pregrasp")); } catch (const moveit::Exception& e) { @@ -158,7 +165,7 @@ void GenerateGraspPose::compute() { return; } - geometry_msgs::PoseStamped target_pose_msg; + geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = props.get("object"); double current_angle = 0.0; diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 3aae11323..56dd2bdea 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -45,16 +45,22 @@ #include #include +#if __has_include() +#include +#else #include +#endif namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePlacePose"); + GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) { auto& p = properties(); p.declare("object"); - p.declare("ik_frame"); + p.declare("ik_frame"); } void GeneratePlacePose::onNewSolution(const SolutionBase& s) { @@ -75,7 +81,7 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) { solution.setComment(msg); spawn(std::move(state), std::move(solution)); } else - ROS_WARN_STREAM_NAMED("GeneratePlacePose", msg); + RCLCPP_WARN_STREAM(LOGGER, msg); return; } @@ -95,13 +101,13 @@ void GeneratePlacePose::compute() { // current object_pose w.r.t. planning frame const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; - const geometry_msgs::PoseStamped& pose_msg = props.get("pose"); + const geometry_msgs::msg::PoseStamped& pose_msg = props.get("pose"); Eigen::Isometry3d target_pose; tf2::fromMsg(pose_msg.pose, target_pose); // target pose w.r.t. planning frame scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose); - const geometry_msgs::PoseStamped& ik_frame_msg = props.get("ik_frame"); + const geometry_msgs::msg::PoseStamped& ik_frame_msg = props.get("ik_frame"); Eigen::Isometry3d ik_frame; tf2::fromMsg(ik_frame_msg.pose, ik_frame); ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame; @@ -121,7 +127,7 @@ void GeneratePlacePose::compute() { .pretranslate(pos); // target ik_frame's pose w.r.t. planning frame - geometry_msgs::PoseStamped target_pose_msg; + geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = scene->getPlanningFrame(); target_pose_msg.pose = tf2::toMsg(object * object_to_ik); diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 8315bc955..bea2fee21 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -45,11 +45,13 @@ namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePose"); + GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { setCostTerm(std::make_unique(0.0)); auto& p = properties(); - p.declare("pose", "target pose to pass on in spawned states"); + p.declare("pose", "target pose to pass on in spawned states"); } void GeneratePose::reset() { @@ -71,11 +73,11 @@ void GeneratePose::compute() { return; planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); - geometry_msgs::PoseStamped target_pose = properties().get("pose"); + geometry_msgs::msg::PoseStamped target_pose = properties().get("pose"); if (target_pose.header.frame_id.empty()) target_pose.header.frame_id = scene->getPlanningFrame(); else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) { - ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str()); + RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", target_pose.header.frame_id.c_str()); return; } diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index c14605a63..fc9701649 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -46,6 +46,8 @@ namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("ModifyPlanningScene"); + ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { setCostTerm(std::make_unique(0.0)); } @@ -56,19 +58,19 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& o.insert(o.end(), objects.begin(), objects.end()); } -void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collision_object) { - if (collision_object.operation != moveit_msgs::CollisionObject::ADD) { - ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": addObject is called with object's operation not set " - "to ADD -- ignoring the object"); +void ModifyPlanningScene::addObject(const moveit_msgs::msg::CollisionObject& collision_object) { + if (collision_object.operation != moveit_msgs::msg::CollisionObject::ADD) { + RCLCPP_ERROR_STREAM(LOGGER, name() << ": addObject is called with object's operation not set " + "to ADD -- ignoring the object"); return; } collision_objects_.push_back(collision_object); } void ModifyPlanningScene::removeObject(const std::string& object_name) { - moveit_msgs::CollisionObject obj; + moveit_msgs::msg::CollisionObject obj; obj.id = object_name; - obj.operation = moveit_msgs::CollisionObject::REMOVE; + obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; collision_objects_.push_back(obj); } @@ -93,13 +95,13 @@ void ModifyPlanningScene::computeBackward(const InterfaceState& to) { void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, const std::pair >& pair, bool invert) { - moveit_msgs::AttachedCollisionObject obj; + moveit_msgs::msg::AttachedCollisionObject obj; obj.link_name = pair.first; bool attach = pair.second.second; if (invert) attach = !attach; obj.object.operation = - attach ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE; + attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE; for (const std::string& name : pair.second.first) { obj.object.id = name; scene.processAttachedCollisionObjectMsg(obj); @@ -141,7 +143,7 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver } void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene, - const moveit_msgs::CollisionObject& object) { + const moveit_msgs::msg::CollisionObject& object) { scene.processCollisionObjectMsg(object); } } // namespace stages diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index dbf5ab7fe..23b7a9dc3 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -41,12 +41,18 @@ #include #include +#if __has_include() +#include +#else #include +#endif namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveRelative"); + MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { setCostTerm(std::make_unique()); @@ -54,21 +60,21 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); - p.declare("ik_frame", "frame to be moved in Cartesian direction"); + p.declare("ik_frame", "frame to be moved in Cartesian direction"); p.declare("direction", "motion specification"); // register actual types - PropertySerializer(); - PropertySerializer(); + PropertySerializer(); + PropertySerializer(); p.declare("min_distance", -1.0, "minimum distance to move"); p.declare("max_distance", 0.0, "maximum distance to move"); - p.declare("path_constraints", moveit_msgs::Constraints(), - "constraints to maintain during trajectory"); + p.declare("path_constraints", moveit_msgs::msg::Constraints(), + "constraints to maintain during trajectory"); } void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = link; pose_msg.pose = tf2::toMsg(pose); setIKFrame(pose_msg); @@ -104,7 +110,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } -static void visualizePlan(std::deque& markers, Interface::Direction dir, bool success, +static void visualizePlan(std::deque& markers, Interface::Direction dir, bool success, const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose, const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { double linear_norm = linear.norm(); @@ -118,7 +124,7 @@ static void visualizePlan(std::deque& markers, Inter Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); - visualization_msgs::Marker m; + visualization_msgs::msg::Marker m; m.ns = ns; m.header.frame_id = frame_id; if (dir == Interface::FORWARD) { @@ -164,7 +170,7 @@ static void visualizePlan(std::deque& markers, Inter bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, Interface::Direction dir) { scene = state.scene()->diff(); - const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); assert(robot_model); const auto& props = properties(); @@ -183,7 +189,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning double max_distance = props.get("max_distance"); double min_distance = props.get("min_distance"); - const auto& path_constraints = props.get("path_constraints"); + const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; @@ -209,7 +215,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success try { // try to extract Twist - const geometry_msgs::TwistStamped& target = boost::any_cast(direction); + const geometry_msgs::msg::TwistStamped& target = boost::any_cast(direction); const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id); tf2::fromMsg(target.twist.linear, linear); tf2::fromMsg(target.twist.angular, angular); @@ -252,7 +258,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } try { // try to extract Vector - const geometry_msgs::Vector3Stamped& target = boost::any_cast(direction); + const geometry_msgs::msg::Vector3Stamped& target = + boost::any_cast(direction); const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id); tf2::fromMsg(target.vector, linear); @@ -282,7 +289,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); - robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); + moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 3270a8315..6d4027c94 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -38,7 +38,6 @@ #include #include -#include #include #include @@ -46,11 +45,19 @@ #include #include +#if __has_include() +#include +#else +#include +#endif +#include namespace moveit { namespace task_constructor { namespace stages { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveTo"); + MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { setCostTerm(std::make_unique()); @@ -58,27 +65,27 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); - p.declare("ik_frame", "frame to be moved towards goal pose"); + p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("goal", "goal specification"); // register actual types PropertySerializer(); - PropertySerializer(); - PropertySerializer(); - PropertySerializer(); + PropertySerializer(); + PropertySerializer(); + PropertySerializer(); - p.declare("path_constraints", moveit_msgs::Constraints(), - "constraints to maintain during trajectory"); + p.declare("path_constraints", moveit_msgs::msg::Constraints(), + "constraints to maintain during trajectory"); } void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = link; pose_msg.pose = tf2::toMsg(pose); setIKFrame(pose_msg); } void MoveTo::setGoal(const std::map& joints) { - moveit_msgs::RobotState robot_state; + moveit_msgs::msg::RobotState robot_state; robot_state.joint_state.name.reserve(joints.size()); robot_state.joint_state.position.reserve(joints.size()); @@ -109,7 +116,7 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint try { // try RobotState - const moveit_msgs::RobotState& msg = boost::any_cast(goal); + const moveit_msgs::msg::RobotState& msg = boost::any_cast(goal); if (!msg.is_diff) throw InitStageException(*this, "Expecting a diff state"); @@ -147,8 +154,9 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target) { try { - const geometry_msgs::PoseStamped& msg = boost::any_cast(goal); + const geometry_msgs::msg::PoseStamped& msg = boost::any_cast(goal); tf2::fromMsg(msg.pose, target); + // transform target into global frame target = scene->getFrameTransform(msg.header.frame_id) * target; } catch (const boost::bad_any_cast&) { @@ -160,7 +168,7 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningS bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) { try { - const geometry_msgs::PointStamped& target = boost::any_cast(goal); + const geometry_msgs::msg::PointStamped& target = boost::any_cast(goal); Eigen::Vector3d target_point; tf2::fromMsg(target.point, target_point); // transform target into global frame @@ -178,7 +186,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_po bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, Interface::Direction dir) { scene = state.scene()->diff(); - const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); assert(robot_model); const auto& props = properties(); @@ -195,7 +203,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP return false; } - const auto& path_constraints = props.get("path_constraints"); + const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; @@ -206,7 +214,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP // Where to go? Eigen::Isometry3d target; // What frame+offset in the robot should go there? - geometry_msgs::PoseStamped ik_pose_msg; + geometry_msgs::msg::PoseStamped ik_pose_msg; const moveit::core::LinkModel* link; Eigen::Isometry3d ik_pose_world; @@ -219,7 +227,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP } auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) { - geometry_msgs::PoseStamped msg; + geometry_msgs::msg::PoseStamped msg; msg.header.frame_id = scene->getPlanningFrame(); msg.pose = tf2::toMsg(pose); rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name); diff --git a/core/src/stages/passthrough.cpp b/core/src/stages/passthrough.cpp index 786221e1f..beefb21f8 100644 --- a/core/src/stages/passthrough.cpp +++ b/core/src/stages/passthrough.cpp @@ -44,11 +44,9 @@ #include #include -#include #include #include #include -#include namespace moveit { namespace task_constructor { diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index d289f2d4f..63c897169 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -26,7 +26,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order auto init_ik_frame = [](const PropertyMap& other) -> boost::any { - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; const boost::any& frame = other.get("eef_frame"); if (frame.empty()) return boost::any(); @@ -79,7 +79,7 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) { SerialContainer::init(robot_model); } -void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, +void PickPlaceBase::setApproachRetract(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) { auto& p = approach_stage_->properties(); p.set("direction", motion); @@ -87,7 +87,8 @@ void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion p.set("max_distance", max_distance); } -void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { +void PickPlaceBase::setLiftPlace(const geometry_msgs::msg::TwistStamped& motion, double min_distance, + double max_distance) { auto& p = lift_stage_->properties(); p.set("direction", motion); p.set("min_distance", min_distance); diff --git a/core/src/stages/plugins.cpp b/core/src/stages/plugins.cpp index 9b9864f8f..2c977174c 100644 --- a/core/src/stages/plugins.cpp +++ b/core/src/stages/plugins.cpp @@ -1,6 +1,6 @@ #include -#include +#include /// register plugins to use with ClassLoader diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index a45987ca5..303718ca0 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -44,7 +44,11 @@ #include #include +#if __has_include() +#include +#else #include +#endif namespace moveit { namespace task_constructor { @@ -126,9 +130,9 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) { const std::string& eef = p.get("eef"); - moveit_msgs::AttachedCollisionObject obj; - obj.object.operation = - forward ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE; + moveit_msgs::msg::AttachedCollisionObject obj; + obj.object.operation = forward ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : + (int8_t)moveit_msgs::msg::CollisionObject::REMOVE; obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); @@ -143,7 +147,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) } void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = link; pose_msg.pose = tf2::toMsg(pose); setIKFrame(pose_msg); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index a98ffd3cc..f08eb2591 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -46,6 +46,8 @@ namespace moveit { namespace task_constructor { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("InterfaceState"); + planning_scene::PlanningSceneConstPtr ensureUpdated(const planning_scene::PlanningScenePtr& scene) { // ensure scene's state is updated if (scene->getCurrentState().dirty()) @@ -58,7 +60,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps) : Int InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps) : scene_(ps), priority_(Priority(0, 0.0)) { if (scene_->getCurrentState().dirty()) - ROS_ERROR_NAMED("InterfaceState", "Dirty PlanningScene! Please only forward clean ones into InterfaceState."); + RCLCPP_ERROR(LOGGER, "Dirty PlanningScene! Please only forward clean ones into InterfaceState."); } InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps, const Priority& p) @@ -171,7 +173,7 @@ void SolutionBase::markAsFailure(const std::string& msg) { setComment(msg + "\n" + comment()); } -void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const { +void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const { info.id = introspection ? introspection->solutionId(*this) : 0; info.cost = this->cost(); info.comment = this->comment(); @@ -183,9 +185,9 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In std::copy(markers.begin(), markers.end(), info.markers.begin()); } -void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { +void SubTrajectory::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const { msg.sub_trajectory.emplace_back(); - moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back(); + moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back(); SolutionBase::fillInfo(t.info, introspection); if (trajectory()) @@ -202,8 +204,9 @@ void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } -void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { - moveit_task_constructor_msgs::SubSolution sub_msg; +void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, + Introspection* introspection) const { + moveit_task_constructor_msgs::msg::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); // Usually subsolutions originate from another stage than this solution. @@ -241,12 +244,12 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co return f(*this, comment); } -void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, +void WrappedSolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& solution, Introspection* introspection) const { wrapped_->fillMessage(solution, introspection); // prepend this solutions info as a SubSolution msg - moveit_task_constructor_msgs::SubSolution sub_msg; + moveit_task_constructor_msgs::msg::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0); solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg)); diff --git a/core/src/task.cpp b/core/src/task.cpp index 9f1db2790..9d1694fce 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -38,16 +38,18 @@ #include #include #include -#include +#include -#include -#include +#include +#include #include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor.task"); + namespace { std::string rosNormalizeName(const std::string& name) { std::string n; @@ -111,7 +113,7 @@ Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& c // monitor state on commandline // addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout))); // enable introspection by default, but only if ros::init() was called - if (ros::isInitialized() && introspection) + if (rclcpp::ok() && introspection) enableIntrospection(true); } @@ -137,7 +139,7 @@ Task::~Task() { void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) { if (!robot_model) { - ROS_ERROR_STREAM(name() << ": received invalid robot model"); + RCLCPP_ERROR_STREAM(LOGGER, name() << ": received invalid robot model"); return; } auto impl = pimpl(); @@ -146,9 +148,9 @@ void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) { impl->robot_model_ = robot_model; } -void Task::loadRobotModel(const std::string& robot_description) { +void Task::loadRobotModel(const rclcpp::Node::SharedPtr& node, const std::string& robot_description) { auto impl = pimpl(); - impl->robot_model_loader_ = std::make_shared(robot_description); + impl->robot_model_loader_ = std::make_shared(node, robot_description); setRobotModel(impl->robot_model_loader_->getModel()); if (!impl->robot_model_) throw Exception("Task failed to construct RobotModel"); @@ -212,7 +214,7 @@ void Task::reset() { void Task::init() { auto impl = pimpl(); if (!impl->robot_model_) - loadRobotModel(); + throw std::runtime_error("You need to call loadRobotModel or setRobotModel before initializing the task"); // initialize push connections of wrapped child StagePrivate* child = wrapped()->pimpl(); @@ -269,17 +271,48 @@ void Task::preempt() { pimpl()->preempt_requested_ = true; } -moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) { - actionlib::SimpleActionClient ac("execute_task_solution"); - ac.waitForServer(); - - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; +moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) { + // Add random ID to prevent warnings about multiple publishers within the same node + rclcpp::NodeOptions options; + options.arguments( + { "--ros-args", "-r", + "__node:=moveit_task_constructor_executor_" + std::to_string(reinterpret_cast(this)) }); + auto node = rclcpp::Node::make_shared("_", options); + auto ac = rclcpp_action::create_client( + node, "execute_task_solution"); + ac->wait_for_action_server(); + + moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal; s.fillMessage(goal.solution, pimpl()->introspection_.get()); s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene); - ac.sendGoal(goal); - ac.waitForResult(); - return ac.getResult()->error_code; + moveit_msgs::msg::MoveItErrorCodes error_code; + error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + auto goal_handle_future = ac->async_send_goal(goal); + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Send goal call failed"); + return error_code; + } + + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return error_code; + } + + auto result_future = ac->async_get_result(goal_handle); + if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Get result call failed"); + return error_code; + } + + auto result = result_future.get(); + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_ERROR(node->get_logger(), "Goal was aborted or canceled"); + return error_code; + } + + return result.result->error_code; } void Task::publishAllSolutions(bool wait) { diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 0a4e7e0ef..979e228df 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -35,7 +35,11 @@ /* Authors: Michael Goerner, Robert Haschke */ +#if __has_include() +#include +#else #include +#endif #include #include @@ -85,7 +89,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin } tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); } else { - auto ik_pose_msg = boost::any_cast(property.value()); + auto ik_pose_msg = boost::any_cast(property.value()); if (ik_pose_msg.header.frame_id.empty()) { if (!(robot_link = get_tip())) { solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); @@ -99,7 +103,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; } else { std::stringstream ss; - ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; + ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; solution.markAsFailure(ss.str()); return false; } diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 808e7b0dd..99eb6097b 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -3,27 +3,36 @@ ############# ## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) +if (BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + + add_library(gtest_utils SHARED gtest_value_printers.cpp models.cpp stage_mockups.cpp) - add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp) target_link_libraries(gtest_utils ${PROJECT_NAME}) macro(mtc_add_test TYPE) # Split ARGN into .test file and other sources set(TEST_FILE ${ARGN}) set(SOURCES ${ARGN}) - list(FILTER TEST_FILE INCLUDE REGEX "\.test$") - list(FILTER SOURCES EXCLUDE REGEX "\.test$") + list(FILTER TEST_FILE INCLUDE REGEX "\.launch.py$") + list(FILTER SOURCES EXCLUDE REGEX "\.launch.py$") # Determine TARGET name from first source file list(GET SOURCES 0 TEST_NAME) string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME}) string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) # Configure build target - if(TEST_FILE) # Add rostest if .test file was specified - _add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES}) + if(TEST_FILE) # Add launch test if .launch.py file was specified + ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + add_launch_test(${TEST_FILE} + ARGS "test_binary:=$") else() - _catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + if("${TYPE}" STREQUAL "gtest") + ament_add_gtest(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + elseif("${TYPE}" STREQUAL "gmock") + ament_add_gmock(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + endif() endif() target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) endmacro() @@ -45,29 +54,30 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_interface_state.cpp) - mtc_add_gtest(test_move_to.cpp move_to.test) + mtc_add_gtest(test_move_to.cpp move_to.launch.py) # building these integration tests works without moveit config packages - add_executable(pick_ur5 pick_ur5.cpp) - target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest) + ament_add_gtest_executable(pick_ur5 pick_ur5.cpp) + target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages) - add_executable(pick_pr2 pick_pr2.cpp) - target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages gtest) + ament_add_gtest_executable(pick_pr2 pick_pr2.cpp) + target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages) - add_executable(pick_pa10 pick_pa10.cpp) - target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages gtest) + ament_add_gtest_executable(pick_pa10 pick_pa10.cpp) + target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages) - # running these integrations test naturally requires the moveit configs - find_package(tams_ur5_setup_moveit_config QUIET) - if(tams_ur5_setup_moveit_config_FOUND) - add_rostest(pick_ur5.test) - endif() - find_package(pr2_moveit_config_FOUND QUIET) - if(pr2_moveit_config_FOUND) - add_rostest(pick_pr2.test) - endif() - find_package(pa10_shadow_moveit_config QUIET) - if(pa10_shadow_moveit_config_FOUND) - add_rostest(pick_pa10.test) - endif() + # TODO(JafarAbdi): Port integration tests + # # running these integrations test naturally requires the moveit configs + # find_package(tams_ur5_setup_moveit_config QUIET) + # if(tams_ur5_setup_moveit_config_FOUND) + # add_rostest(pick_ur5.test) + # endif() + # find_package(pr2_moveit_config_FOUND QUIET) + # if(pr2_moveit_config_FOUND) + # add_rostest(pick_pr2.test) + # endif() + # find_package(pa10_shadow_moveit_config QUIET) + # if(pa10_shadow_moveit_config_FOUND) + # add_rostest(pick_pa10.test) + # endif() endif() diff --git a/core/test/models.cpp b/core/test/models.cpp index 377df9539..3abe5ab12 100644 --- a/core/test/models.cpp +++ b/core/test/models.cpp @@ -6,7 +6,8 @@ using namespace moveit::core; RobotModelPtr getModel() { // suppress RobotModel errors and warnings - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal); + if (rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_FATAL) != RCUTILS_RET_OK) + throw std::runtime_error("Failed to set logger level to RCUTILS_LOG_SEVERITY_ERROR"); // create dummy robot model moveit::core::RobotModelBuilder builder("robot", "base"); @@ -17,7 +18,7 @@ RobotModelPtr getModel() { return builder.build(); } -moveit::core::RobotModelPtr loadModel() { - static robot_model_loader::RobotModelLoader loader; +moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node) { + robot_model_loader::RobotModelLoader loader(node); return loader.getModel(); } diff --git a/core/test/models.h b/core/test/models.h index 11cfcdbad..760235222 100644 --- a/core/test/models.h +++ b/core/test/models.h @@ -1,6 +1,7 @@ #pragma once #include +#include namespace moveit { namespace core { @@ -12,4 +13,4 @@ MOVEIT_CLASS_FORWARD(RobotModel); moveit::core::RobotModelPtr getModel(); // load a model from robot_description -moveit::core::RobotModelPtr loadModel(); +moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node); diff --git a/core/test/move_to.launch.py b/core/test/move_to.launch.py new file mode 100644 index 000000000..103a20cff --- /dev/null +++ b/core/test/move_to.launch.py @@ -0,0 +1,110 @@ +import unittest +import os +import yaml + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +import launch_testing +from launch_testing.asserts import assertExitCodes +from launch_testing.util import KeepAliveProc +from launch_testing.actions import ReadyToTest +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +import pytest + +import xacro + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +@pytest.mark.launch_test +def generate_test_description(): + # planning_context + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} + + robot_description_planning = { + "robot_description_planning": load_yaml( + "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" + ) + } + + test_exec = Node( + executable=[ + LaunchConfiguration("test_binary"), + ], + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + robot_description_planning, + ], + ) + + return ( + LaunchDescription( + [ + DeclareLaunchArgument( + name="test_binary", + description="Test executable", + ), + test_exec, + KeepAliveProc(), + ReadyToTest(), + ] + ), + {"test_exec": test_exec}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, test_exec): + proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TaskModelTestAfterShutdown(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/core/test/move_to.test b/core/test/move_to.test deleted file mode 100644 index 105e936a7..000000000 --- a/core/test/move_to.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp index 0751471a4..367372400 100644 --- a/core/test/pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -13,14 +13,14 @@ #include #include -#include +#include #include #include using namespace moveit::task_constructor; void spawnObject(const planning_scene::PlanningScenePtr& scene) { - moveit_msgs::CollisionObject o; + moveit_msgs::msg::CollisionObject o; o.id = "object"; o.header.frame_id = "world"; o.primitive_poses.resize(1); @@ -29,7 +29,7 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) { o.primitive_poses[0].position.z = 0.10; o.primitive_poses[0].orientation.w = 1.0; o.primitives.resize(1); - o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + o.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; o.primitives[0].dimensions.resize(2); o.primitives[0].dimensions[0] = 0.23; o.primitives[0].dimensions[1] = 0.03; @@ -39,13 +39,15 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) { TEST(PA10, pick) { Task t; t.stages()->setName("pick"); - t.loadRobotModel(); + + auto node = rclcpp::Node::make_shared("pa10"); + t.loadRobotModel(node); // define global properties used by most stages t.setProperty("group", std::string("left_arm")); t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("gripper", std::string("left_hand")); - auto pipeline = std::make_shared(); + auto pipeline = std::make_shared(node); pipeline->setPlannerId("RRTConnectkConfigDefault"); auto cartesian = std::make_shared(); @@ -87,7 +89,7 @@ TEST(PA10, pick) { move->setIKFrame("lh_tool_frame"); move->setMinMaxDistance(0.05, 0.1); - geometry_msgs::Vector3Stamped direction; + geometry_msgs::msg::Vector3Stamped direction; direction.header.frame_id = "lh_tool_frame"; direction.vector.z = 1; move->setDirection(direction); @@ -154,7 +156,7 @@ TEST(PA10, pick) { move->properties().set("marker_ns", std::string("lift")); move->setIKFrame("lh_tool_frame"); - geometry_msgs::Vector3Stamped direction; + geometry_msgs::msg::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.z = 1; move->setDirection(direction); @@ -168,7 +170,7 @@ TEST(PA10, pick) { move->properties().set("marker_ns", std::string("lift")); move->setIKFrame("lh_tool_frame"); - geometry_msgs::TwistStamped twist; + geometry_msgs::msg::TwistStamped twist; twist.header.frame_id = "object"; twist.twist.linear.y = 1; twist.twist.angular.y = 2; @@ -189,9 +191,7 @@ TEST(PA10, pick) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "pa10"); - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/core/test/pick_pr2.cpp b/core/test/pick_pr2.cpp index ac1706320..2554097b5 100644 --- a/core/test/pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include @@ -16,7 +16,7 @@ using namespace moveit::task_constructor; void spawnObject() { moveit::planning_interface::PlanningSceneInterface psi; - moveit_msgs::CollisionObject o; + moveit_msgs::msg::CollisionObject o; o.id = "object"; o.header.frame_id = "base_link"; o.primitive_poses.resize(1); @@ -25,7 +25,7 @@ void spawnObject() { o.primitive_poses[0].position.z = 0.84; o.primitive_poses[0].orientation.w = 1.0; o.primitives.resize(1); - o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + o.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; o.primitives[0].dimensions.resize(2); o.primitives[0].dimensions[0] = 0.23; o.primitives[0].dimensions[1] = 0.03; @@ -38,8 +38,9 @@ TEST(PR2, pick) { Stage* initial_stage = new stages::CurrentState("current state"); t.add(std::unique_ptr(initial_stage)); + auto node = rclcpp::Node::make_shared("pr2"); // planner used for connect - auto pipeline = std::make_shared(); + auto pipeline = std::make_shared(node); pipeline->setPlannerId("RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } }; @@ -61,12 +62,12 @@ TEST(PR2, pick) { auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("left_gripper")); pick->setProperty("object", std::string("object")); - geometry_msgs::TwistStamped approach; + geometry_msgs::msg::TwistStamped approach; approach.header.frame_id = "l_gripper_tool_frame"; approach.twist.linear.x = 1.0; pick->setApproachMotion(approach, 0.03, 0.1); - geometry_msgs::TwistStamped lift; + geometry_msgs::msg::TwistStamped lift; lift.header.frame_id = "base_link"; lift.twist.linear.z = 1.0; pick->setLiftMotion(lift, 0.03, 0.05); @@ -87,12 +88,10 @@ TEST(PR2, pick) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "pr2"); - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); // wait some time for move_group to come up - ros::WallDuration(5.0).sleep(); + rclcpp::sleep_for(std::chrono::seconds(5)); return RUN_ALL_TESTS(); } diff --git a/core/test/pick_ur5.cpp b/core/test/pick_ur5.cpp index 6c2808ec8..87d17e87e 100644 --- a/core/test/pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include @@ -16,7 +16,7 @@ using namespace moveit::task_constructor; void spawnObject() { moveit::planning_interface::PlanningSceneInterface psi; - moveit_msgs::CollisionObject o; + moveit_msgs::msg::CollisionObject o; o.id = "object"; o.header.frame_id = "table_top"; o.primitive_poses.resize(1); @@ -25,7 +25,7 @@ void spawnObject() { o.primitive_poses[0].position.z = 0.12; o.primitive_poses[0].orientation.w = 1.0; o.primitives.resize(1); - o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + o.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; o.primitives[0].dimensions.resize(2); o.primitives[0].dimensions[0] = 0.23; o.primitives[0].dimensions[1] = 0.03; @@ -40,8 +40,9 @@ TEST(UR5, pick) { initial_stage = initial.get(); t.add(std::move(initial)); + auto node = rclcpp::Node::make_shared("ur5"); // planner used for connect - auto pipeline = std::make_shared(); + auto pipeline = std::make_shared(node); pipeline->setPlannerId("RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } }; @@ -63,12 +64,12 @@ TEST(UR5, pick) { auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("gripper")); pick->setProperty("object", std::string("object")); - geometry_msgs::TwistStamped approach; + geometry_msgs::msg::TwistStamped approach; approach.header.frame_id = "s_model_tool0"; approach.twist.linear.x = 1.0; pick->setApproachMotion(approach, 0.03, 0.1); - geometry_msgs::TwistStamped lift; + geometry_msgs::msg::TwistStamped lift; lift.header.frame_id = "world"; lift.twist.linear.z = 1.0; pick->setLiftMotion(lift, 0.03, 0.05); @@ -89,12 +90,10 @@ TEST(UR5, pick) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "ur5"); - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); // wait some time for move_group to come up - ros::WallDuration(5.0).sleep(); + rclcpp::sleep_for(std::chrono::seconds(5)); return RUN_ALL_TESTS(); } diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index f7bec9a09..787711033 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -8,7 +8,6 @@ #include "models.h" #include -#include namespace moveit { namespace task_constructor { diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index a610ca043..1aa3bce80 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -7,7 +7,7 @@ #include -#include +#include #include diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d37d1e13e..d3f7e9068 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -9,6 +9,7 @@ #include "gtest_value_printers.h" #include +#include #include #include #include @@ -176,8 +177,9 @@ TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { int main(int argc, char** argv) { for (int i = 1; i < argc; ++i) { if (strcmp(argv[i], "--debug") == 0) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); + if (rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_FATAL) != + RCUTILS_RET_OK) + throw std::runtime_error("Failed to set logger level to RCUTILS_LOG_SEVERITY_ERROR"); break; } } diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 1fbc512ba..78fa8ee33 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -8,12 +8,16 @@ #include +#if __has_include() +#include +#else #include +#endif -#include -#include +#include +#include -#include +#include #include using namespace moveit::task_constructor; @@ -28,9 +32,10 @@ struct PandaMoveTo : public testing::Test Task t; stages::MoveTo* move_to; PlanningScenePtr scene; + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_to"); PandaMoveTo() { - t.setRobotModel(loadModel()); + t.loadRobotModel(node); scene = std::make_shared(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); @@ -63,7 +68,7 @@ TEST_F(PandaMoveTo, mapTarget) { TEST_F(PandaMoveTo, stateTarget) { move_to->setGoal([] { - moveit_msgs::RobotState state; + moveit_msgs::msg::RobotState state; state.is_diff = true; state.joint_state.name = { "panda_joint1", "panda_joint2" }; state.joint_state.position = { TAU / 8, TAU / 8 }; @@ -72,19 +77,19 @@ TEST_F(PandaMoveTo, stateTarget) { EXPECT_ONE_SOLUTION; } -geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { +geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); auto frame_eigen{ state.getFrameTransform(frame) }; - geometry_msgs::PoseStamped p; + geometry_msgs::msg::PoseStamped p; p.header.frame_id = state.getRobotModel()->getModelFrame(); p.pose = tf2::toMsg(frame_eigen); return p; } TEST_F(PandaMoveTo, pointTarget) { - geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") }; + geometry_msgs::msg::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") }; - geometry_msgs::PointStamped point; + geometry_msgs::msg::PointStamped point; point.header = pose.header; point.point = pose.pose.position; move_to->setGoal(point); @@ -104,24 +109,24 @@ TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { EXPECT_ONE_SOLUTION; } -moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) { - moveit_msgs::AttachedCollisionObject aco; +moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::msg::AttachedCollisionObject aco; aco.link_name = "panda_hand"; aco.object.header.frame_id = aco.link_name; aco.object.operation = aco.object.ADD; aco.object.id = id; aco.object.primitives.resize(1, [] { - shape_msgs::SolidPrimitive p; + shape_msgs::msg::SolidPrimitive p; p.type = p.SPHERE; p.dimensions.resize(1); p.dimensions[p.SPHERE_RADIUS] = 0.01; return p; }()); + aco.object.primitive_poses.resize(1); #if MOVEIT_HAS_OBJECT_POSE aco.object.pose.position.z = 0.2; aco.object.pose.orientation.w = 1.0; #else - aco.object.primitive_poses.resize(1); aco.object.primitive_poses[0].position.z = 0.2; aco.object.primitive_poses[0].orientation.w = 1.0; #endif @@ -129,7 +134,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) // If we don't have this, we also don't have subframe support aco.object.subframe_names.resize(1, "subframe"); aco.object.subframe_poses.resize(1, [] { - geometry_msgs::Pose p; + geometry_msgs::msg::Pose p; p.orientation.w = 1.0; return p; }()); @@ -162,9 +167,7 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "move_to_test"); - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 180bc70d5..976219bed 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -4,10 +4,12 @@ #include #include #include -#include +#include #include "stage_mockups.h" -#include + +#include + #include using namespace moveit::task_constructor; @@ -89,7 +91,7 @@ TEST(ModifyPlanningScene, allowCollisions) { void spawnObject(PlanningScene& scene, const std::string& name, int type, const std::vector& pos = { 0, 0, 0 }) { - moveit_msgs::CollisionObject o; + moveit_msgs::msg::CollisionObject o; o.id = name; o.header.frame_id = scene.getPlanningFrame(); o.primitive_poses.resize(1); @@ -102,13 +104,13 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type, o.primitives[0].type = type; switch (type) { - case shape_msgs::SolidPrimitive::CYLINDER: + case shape_msgs::msg::SolidPrimitive::CYLINDER: o.primitives[0].dimensions = { 0.1, 0.02 }; break; - case shape_msgs::SolidPrimitive::BOX: + case shape_msgs::msg::SolidPrimitive::BOX: o.primitives[0].dimensions = { 0.1, 0.2, 0.3 }; break; - case shape_msgs::SolidPrimitive::SPHERE: + case shape_msgs::msg::SolidPrimitive::SPHERE: o.primitives[0].dimensions = { 0.05 }; break; } @@ -116,10 +118,10 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type, } void attachObject(PlanningScene& scene, const std::string& object, const std::string& link, bool attach) { - moveit_msgs::AttachedCollisionObject obj; + moveit_msgs::msg::AttachedCollisionObject obj; obj.link_name = link; obj.object.operation = - attach ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE; + attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE; obj.object.id = object; scene.processAttachedCollisionObjectMsg(obj); } @@ -129,19 +131,19 @@ TEST(Connect, compatible) { auto scene = std::make_shared(getModel()); auto& state = scene->getCurrentStateNonConst(); state.setToDefaultValues(); - spawnObject(*scene, "object", shape_msgs::SolidPrimitive::CYLINDER); + spawnObject(*scene, "object", shape_msgs::msg::SolidPrimitive::CYLINDER); state.update(); auto other = scene->diff(); EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes"; - spawnObject(*other, "object", shape_msgs::SolidPrimitive::BOX); + spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::BOX); // EXPECT_FALSE(connect.compatible(scene, other)) << "different shapes"; - spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); + spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; - spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER); + spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER); EXPECT_TRUE(connect.compatible(scene, other)) << "same objects"; // attached objects @@ -152,7 +154,7 @@ TEST(Connect, compatible) { other = scene->diff(); EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object"; - spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); + spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); attachObject(*other, "object", "tip", true); EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; } From d7ceaa01dd96f1918f89293932eec5e460a066bc Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 16:59:36 +0300 Subject: [PATCH 5/8] Port capabilities to ROS2 --- capabilities/CMakeLists.txt | 56 ++++++------- .../capabilities_plugin_description.xml | 2 +- capabilities/package.xml | 7 +- .../src/execute_task_solution_capability.cpp | 83 +++++++++---------- .../src/execute_task_solution_capability.h | 22 +++-- 5 files changed, 87 insertions(+), 83 deletions(-) diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index 539ba41f7..ed8282109 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -1,40 +1,38 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_capabilities) -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) -find_package(catkin REQUIRED COMPONENTS - actionlib - moveit_core - moveit_ros_move_group - moveit_task_constructor_core - moveit_task_constructor_msgs - pluginlib - std_msgs -) - -catkin_package( - LIBRARIES $PROJECT_NAME} - CATKIN_DEPENDS - actionlib - moveit_task_constructor_msgs - std_msgs -) +find_package(ament_cmake REQUIRED) -include_directories( - ${catkin_INCLUDE_DIRS} -) +find_package(Boost REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_move_group REQUIRED) +find_package(moveit_task_constructor_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(std_msgs REQUIRED) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/execute_task_solution_capability.cpp ) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} + Boost + rclcpp_action + moveit_core + moveit_ros_move_group + moveit_task_constructor_msgs +) install(TARGETS ${PROJECT_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION lib ) -install(FILES capabilities_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml) +ament_export_dependencies(rclcpp_action) +ament_export_dependencies(moveit_core) +ament_export_dependencies(moveit_ros_move_group) +ament_export_dependencies(moveit_task_constructor_msgs) +ament_export_dependencies(pluginlib) +ament_export_dependencies(std_msgs) +ament_package() diff --git a/capabilities/capabilities_plugin_description.xml b/capabilities/capabilities_plugin_description.xml index eba669c8f..82e274436 100644 --- a/capabilities/capabilities_plugin_description.xml +++ b/capabilities/capabilities_plugin_description.xml @@ -1,4 +1,4 @@ - + Action server to execute solutions generated through the MoveIt Task Constructor. diff --git a/capabilities/package.xml b/capabilities/package.xml index 7ae112f2b..bdad2efa9 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -10,17 +10,18 @@ BSD - catkin + ament_cmake moveit_core moveit_ros_move_group - actionlib + moveit_ros_planning + rclcpp_action pluginlib std_msgs moveit_task_constructor_core moveit_task_constructor_msgs - + ament_cmake diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 482f52602..f74e84ea0 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -36,16 +36,16 @@ #include "execute_task_solution_capability.h" -#include - +#include #include #include #include #include #include -#if MOVEIT_HAS_MESSAGE_CHECKS #include -#endif +#include + +#include namespace { @@ -80,58 +80,63 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob } } // namespace +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution"); + namespace move_group { ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {} void ExecuteTaskSolutionCapability::initialize() { // configure the action server - as_.reset(new actionlib::SimpleActionServer( - root_node_handle_, "execute_task_solution", - std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false)); - as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this)); - as_->start(); + as_ = rclcpp_action::create_server( + context_->moveit_cpp_->getNode(), "execute_task_solution", + ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this, + std::placeholders::_1, std::placeholders::_2)), + ActionServerType::CancelCallback( + std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)), + ActionServerType::AcceptedCallback( + std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1))); } void ExecuteTaskSolutionCapability::goalCallback( - const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) { - moveit_task_constructor_msgs::ExecuteTaskSolutionResult result; + const std::shared_ptr> goal_handle) { + auto result = std::make_shared(); + const auto& goal = goal_handle->get_goal(); if (!context_->plan_execution_) { - const std::string response = "Cannot execute solution. ~allow_trajectory_execution was set to false"; - result.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED; - as_->setAborted(result, response); + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED; + goal_handle->abort(result); return; } plan_execution::ExecutableMotionPlan plan; if (!constructMotionPlan(goal->solution, plan)) - result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; else { - ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution"); - result.error_code = context_->plan_execution_->executeAndMonitor(plan); + RCLCPP_INFO(LOGGER, "Executing TaskSolution"); + result->error_code = context_->plan_execution_->executeAndMonitor(plan); } - const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code); - - if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) - as_->setSucceeded(result, response); - else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED) - as_->setPreempted(result, response); + if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + goal_handle->succeed(result); + else if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) + goal_handle->canceled(result); else - as_->setAborted(result, response); + goal_handle->abort(result); } -void ExecuteTaskSolutionCapability::preemptCallback() { +rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback( + const std::shared_ptr> goal_handle) { if (context_->plan_execution_) context_->plan_execution_->stop(); + return rclcpp_action::CancelResponse::ACCEPT; } -bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, +bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan) { - robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); + moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); - robot_state::RobotState state(model); + moveit::core::RobotState state(model); { planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_); state = scene->getCurrentState(); @@ -139,7 +144,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr plan.plan_components_.reserve(solution.sub_trajectory.size()); for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) { - const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory[i]; + const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i]; plan.plan_components_.emplace_back(); plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back(); @@ -156,12 +161,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr if (!joint_names.empty()) { group = findJointModelGroup(*model, joint_names); if (!group) { - ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" - << boost::algorithm::join(joint_names, ", ") << "}"); + RCLCPP_ERROR_STREAM(LOGGER, "Could not find JointModelGroup that actuates {" + << boost::algorithm::join(joint_names, ", ") << "}"); return false; } - ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution", - group->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str()); } } exec_traj.trajectory_ = std::make_shared(model, group); @@ -170,25 +174,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr /* TODO add action feedback and markers */ exec_traj.effect_on_success_ = [this, sub_traj, description](const plan_execution::ExecutableMotionPlan* /*plan*/) { -#if MOVEIT_HAS_MESSAGE_CHECKS if (!moveit::core::isEmpty(sub_traj.scene_diff)) { -#else - if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) { -#endif - ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description); + RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff); } return true; }; -#if MOVEIT_HAS_MESSAGE_CHECKS if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) && -#else - if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) && -#endif !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) { - ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", - "invalid intermediate robot state in scene diff of SubTrajectory " << description); + RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of SubTrajectory " << description); return false; } } diff --git a/capabilities/src/execute_task_solution_capability.h b/capabilities/src/execute_task_solution_capability.h index b027b5124..366347da7 100644 --- a/capabilities/src/execute_task_solution_capability.h +++ b/capabilities/src/execute_task_solution_capability.h @@ -42,9 +42,9 @@ #pragma once #include -#include +#include -#include +#include #include @@ -58,13 +58,23 @@ class ExecuteTaskSolutionCapability : public MoveGroupCapability void initialize() override; private: - bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, + using ExecuteTaskSolutionAction = moveit_task_constructor_msgs::action::ExecuteTaskSolution; + using ActionServerType = rclcpp_action::Server; + bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution, plan_execution::ExecutableMotionPlan& plan); - void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal); - void preemptCallback(); + void goalCallback(const std::shared_ptr> goal_handle); - std::unique_ptr> as_; + rclcpp_action::CancelResponse + preemptCallback(const std::shared_ptr> goal_handle); + + /** Always accept the goal */ + rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& uuid, + const ExecuteTaskSolutionAction::Goal::ConstSharedPtr goal) const { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + ActionServerType::SharedPtr as_; }; } // namespace move_group From 29703d0d6aa815efea05ba5ac960c3588600c72f Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 17:00:01 +0300 Subject: [PATCH 6/8] Port visualization to ROS2 --- visualization/CMakeLists.txt | 91 +++++------ .../properties/CMakeLists.txt | 11 +- .../properties/property_factory.cpp | 76 ++++----- .../properties/property_factory.h | 58 ++++--- .../properties/property_from_yaml.cpp | 89 +++++----- .../motion_planning_tasks/src/CMakeLists.txt | 13 +- .../src/factory_model.cpp | 22 +-- .../motion_planning_tasks/src/factory_model.h | 6 +- .../src/global_settings.ui | 6 +- .../motion_planning_tasks/src/job_queue.cpp | 6 +- .../src/local_task_model.cpp | 8 +- .../src/local_task_model.h | 6 +- .../motion_planning_tasks/src/plugin_init.cpp | 6 +- .../src/pluginlib_factory.h | 80 +++++---- .../src/remote_task_model.cpp | 100 +++++++----- .../src/remote_task_model.h | 24 +-- .../src/task_display.cpp | 107 +++++++----- .../motion_planning_tasks/src/task_display.h | 48 +++--- .../src/task_list_model.cpp | 26 ++- .../src/task_list_model.h | 30 ++-- .../motion_planning_tasks/src/task_panel.cpp | 129 ++++++++------- .../motion_planning_tasks/src/task_panel.h | 40 ++--- .../motion_planning_tasks/src/task_panel_p.h | 19 +-- .../motion_planning_tasks/src/task_view.ui | 6 +- .../motion_planning_tasks/test/CMakeLists.txt | 20 ++- .../test/test_task_model.cpp | 32 ++-- .../test/test_task_model.launch | 4 - .../test/test_task_model.launch.py | 52 ++++++ .../utils/CMakeLists.txt | 5 +- ...planning_tasks_rviz_plugin_description.xml | 6 +- visualization/package.xml | 19 ++- .../visualization_tools/CMakeLists.txt | 33 ++-- .../visualization_tools/display_solution.h | 8 +- .../marker_visualization.h | 35 ++-- .../visualization_tools/task_solution_panel.h | 6 +- .../task_solution_visualization.h | 72 +++++---- .../src/display_solution.cpp | 10 +- .../src/marker_visualization.cpp | 94 +++++------ .../src/task_solution_visualization.cpp | 153 ++++++++++-------- 39 files changed, 856 insertions(+), 700 deletions(-) delete mode 100644 visualization/motion_planning_tasks/test/test_task_model.launch create mode 100644 visualization/motion_planning_tasks/test/test_task_model.launch.py diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 5065bc9f9..a1d692d2e 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -1,69 +1,56 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_visualization) -find_package(catkin REQUIRED COMPONENTS - moveit_core - moveit_ros_visualization - moveit_task_constructor_core - moveit_task_constructor_msgs - roscpp - rviz -) - -# rviz transitively includes OGRE headers which break with `-Wall -Werror` -# so isolate these include dirs and add them as SYSTEM include where needed. -set(rviz_OGRE_INCLUDE_DIRS) -foreach(header IN ITEMS OgreRoot.h OgreOverlay.h) - find_path(include_dir ${header} - HINTS ${catkin_INCLUDE_DIRS} - NO_DEFAULT_PATH) - list(REMOVE_ITEM catkin_INCLUDE_DIRS "${include_dir}") - list(APPEND rviz_OGRE_INCLUDE_DIRS "${include_dir}") -endforeach() +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_visualization REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(moveit_task_constructor_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) # definition needed for boost/math/constants/constants.hpp included by Ogre to compile add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) # 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() +find_package(Qt5 REQUIRED COMPONENTS Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +macro(qt_wrap_ui) + qt5_wrap_ui(${ARGN}) +endmacro() set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_AUTOMOC ON) set(CMAKE_AUTORCC ON) -add_definitions(-DQT_NO_KEYWORDS) - -catkin_package( - LIBRARIES - moveit_task_visualization_tools - motion_planning_tasks_utils - INCLUDE_DIRS - visualization_tools/include - CATKIN_DEPENDS - moveit_core - moveit_task_constructor_msgs - roscpp - rviz -) -set(CMAKE_CXX_STANDARD 14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() add_subdirectory(visualization_tools) add_subdirectory(motion_planning_tasks) -install(FILES - motion_planning_tasks_rviz_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY icons DESTINATION share) -install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +pluginlib_export_plugin_description_file(rviz_common motion_planning_tasks_rviz_plugin_description.xml) + +ament_export_include_directories(include) +ament_export_libraries(motion_planning_tasks_utils + motion_planning_tasks_properties + motion_planning_tasks_rviz_plugin + moveit_task_visualization_tools +) +ament_export_dependencies(ament_cmake) +ament_export_dependencies(Boost) +ament_export_dependencies(moveit_core) +ament_export_dependencies(moveit_ros_visualization) +ament_export_dependencies(moveit_task_constructor_core) +ament_export_dependencies(moveit_task_constructor_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rviz_common) +ament_export_dependencies(rviz_default_plugins) +ament_export_dependencies(rviz_ogre_vendor) +ament_package() diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index b0fabdafd..d1351ab27 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -20,9 +20,14 @@ target_link_libraries(${MOVEIT_LIB_NAME} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS} + PRIVATE ${YAML_INCLUDE_DIRS} +) +ament_target_dependencies(${MOVEIT_LIB_NAME} + moveit_task_constructor_core + rviz_common ) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT export_${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 95542a74a..5d6079441 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -40,34 +40,34 @@ #include #include -#include -#include -#include +#include +#include +#include namespace mtc = ::moveit::task_constructor; namespace moveit_rviz_plugin { -static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, - const planning_scene::PlanningScene* /*unused*/, - rviz::DisplayContext* /*unused*/) { +static rviz_common::properties::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*unused*/, + rviz_common::DisplayContext* /*unused*/) { std::string value; if (!mtc_prop.value().empty()) value = boost::any_cast(mtc_prop.value()); - rviz::StringProperty* rviz_prop = - new rviz::StringProperty(name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); - QObject::connect(rviz_prop, &rviz::StringProperty::changed, + rviz_common::properties::StringProperty* rviz_prop = new rviz_common::properties::StringProperty( + name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); + QObject::connect(rviz_prop, &rviz_common::properties::StringProperty::changed, [rviz_prop, &mtc_prop]() { mtc_prop.setValue(rviz_prop->getStdString()); }); return rviz_prop; } template -static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, - const planning_scene::PlanningScene* /*unused*/, - rviz::DisplayContext* /*unused*/) { +static rviz_common::properties::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*unused*/, + rviz_common::DisplayContext* /*unused*/) { T value = !mtc_prop.value().empty() ? boost::any_cast(mtc_prop.value()) : T(); - rviz::FloatProperty* rviz_prop = - new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); - QObject::connect(rviz_prop, &rviz::FloatProperty::changed, + rviz_common::properties::FloatProperty* rviz_prop = + new rviz_common::properties::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); + QObject::connect(rviz_prop, &rviz_common::properties::FloatProperty::changed, [rviz_prop, &mtc_prop]() { mtc_prop.setValue(T(rviz_prop->getFloat())); }); return rviz_prop; } @@ -94,33 +94,34 @@ void PropertyFactory::registerStage(const std::type_index& type_index, const Pro stage_registry_.insert(std::make_pair(type_index, f)); } -rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) const { +rviz_common::properties::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) const { auto it = property_registry_.find(prop.typeName()); if (it == property_registry_.end()) return createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize()); return it->second(QString::fromStdString(prop_name), prop, scene, display_context); } -rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) { +rviz_common::properties::PropertyTreeModel* +PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) { auto it = stage_registry_.find(typeid(stage)); if (it == stage_registry_.end()) return defaultPropertyTreeModel(stage.properties(), scene, display_context); return it->second(stage.properties(), scene, display_context); } -rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) { - auto root = new rviz::Property(); +rviz_common::properties::PropertyTreeModel* +PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) { + auto root = new rviz_common::properties::Property(); addRemainingProperties(root, properties, scene, display_context); - return new rviz::PropertyTreeModel(root, nullptr); + return new rviz_common::properties::PropertyTreeModel(root, nullptr); } -static bool hasChild(rviz::Property* root, const QString& name) { +static bool hasChild(rviz_common::properties::Property* root, const QString& name) { for (int i = 0, end = root->numChildren(); i != end; ++i) { if (root->childAt(i)->getName() == name) return true; @@ -128,37 +129,38 @@ static bool hasChild(rviz::Property* root, const QString& name) { return false; } -void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::PropertyMap& properties, +void PropertyFactory::addRemainingProperties(rviz_common::properties::Property* root, mtc::PropertyMap& properties, const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) { + rviz_common::DisplayContext* display_context) { for (auto& prop : properties) { const QString& name = QString::fromStdString(prop.first); if (hasChild(root, name)) continue; - rviz::Property* rviz_prop = create(prop.first, prop.second, scene, display_context); + rviz_common::properties::Property* rviz_prop = create(prop.first, prop.second, scene, display_context); if (!rviz_prop) - rviz_prop = new rviz::Property(name); + rviz_prop = new rviz_common::properties::Property(name); root->addChild(rviz_prop); } // just to see something, when no properties are defined if (root->numChildren() == 0) - new rviz::Property("no properties", QVariant(), QString(), root); + new rviz_common::properties::Property("no properties", QVariant(), QString(), root); } #ifndef HAVE_YAML -rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, - const std::string& description, const std::string& value, - rviz::Property* old) { +rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, + const std::string& description, + const std::string& value, + rviz_common::properties::Property* old) { if (old) { // reuse existing Property? assert(old->getNameStd() == name); old->setDescription(QString::fromStdString(description)); old->setValue(QString::fromStdString(value)); return old; } else { // create new Property? - rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value), - QString::fromStdString(description)); + rviz_common::properties::Property* result = new rviz::StringProperty( + QString::fromStdString(name), QString::fromStdString(value), QString::fromStdString(description)); result->setReadOnly(true); return result; } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index 84c7c9167..112d108ad 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -44,11 +44,14 @@ #include -namespace rviz { +namespace rviz_common { +class DisplayContext; +namespace properties { class Property; class PropertyTreeModel; -class DisplayContext; -} // namespace rviz +} // namespace properties +} // namespace rviz_common + namespace planning_scene { class PlanningScene; } @@ -60,10 +63,10 @@ class Stage; namespace moveit_rviz_plugin { -/** Registry for rviz::Property and rviz::PropertyTreeModel creator functions. +/** Registry for rviz_common::properties::Property and rviz_common::properties::PropertyTreeModel creator functions. * * To inspect (and edit) properties of stages, our MTC properties are converted to rviz properties, - * which are finally shown in an rviz::PropertyTree. + * which are finally shown in an rviz_common::properties::PropertyTree. * To allow customization of property display, one can register creator functions for individual * properties as well as creator functions for a complete stage. The latter allows to fully customize * the display of stage properties, e.g. hiding specific properties, or returning a subclassed @@ -75,11 +78,11 @@ class PropertyFactory public: static PropertyFactory& instance(); - using PropertyFactoryFunction = - std::function; - using TreeFactoryFunction = std::function; + using PropertyFactoryFunction = std::function; + using TreeFactoryFunction = std::function; /// register a factory function for type T template @@ -94,27 +97,30 @@ class PropertyFactory registerStage(typeid(T), f); } - /// create rviz::Property for given MTC Property - rviz::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop, - const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context) const; - /// create rviz::Property for property of given name, type, description, and value - static rviz::Property* createDefault(const std::string& name, const std::string& type, - const std::string& description, const std::string& value, - rviz::Property* old = nullptr); + /// create rviz_common::properties::Property for given MTC Property + rviz_common::properties::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) const; + /// create rviz_common::properties::Property for property of given name, type, description, and value + static rviz_common::properties::Property* createDefault(const std::string& name, const std::string& type, + const std::string& description, const std::string& value, + rviz_common::properties::Property* old = nullptr); /// create PropertyTreeModel for given Stage - rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context); + rviz_common::properties::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context); - /// turn a PropertyMap into an rviz::PropertyTreeModel - rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context); + /// turn a PropertyMap into an rviz_common::properties::PropertyTreeModel + rviz_common::properties::PropertyTreeModel* + defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties, + const planning_scene::PlanningScene* scene, rviz_common::DisplayContext* display_context); /// add all properties from map that are not yet in root - void addRemainingProperties(rviz::Property* root, moveit::task_constructor::PropertyMap& properties, - const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context); + void addRemainingProperties(rviz_common::properties::Property* root, + moveit::task_constructor::PropertyMap& properties, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context); private: std::map property_registry_; diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp index a46d18bff..9d1ad862c 100644 --- a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -36,12 +36,12 @@ #include "property_factory.h" #include -#include -#include +#include +#include namespace mtc = ::moveit::task_constructor; -/** Implement PropertyFactory::createDefault(), creating an rviz::Property (tree) +/** Implement PropertyFactory::createDefault(), creating an rviz_common::properties::Property (tree) * from a YAML-serialized string. * As we cannot know the required data type for a field from YAML parsing, * we only distinguish numbers (FloatProperty) and all other YAML scalars (StringProperty). @@ -60,7 +60,7 @@ class ScopedYamlEvent yaml_event_t event_; }; -// Event-based YAML parser, creating an rviz::Property tree +// Event-based YAML parser, creating an rviz_common::properties::Property tree // https://www.wpsoftware.net/andrew/pages/libyaml.html class Parser { @@ -70,30 +70,35 @@ class Parser Parser(const std::string& value); ~Parser(); - rviz::Property* process(const QString& name, const QString& description, rviz::Property* old) const; + rviz_common::properties::Property* process(const QString& name, const QString& description, + rviz_common::properties::Property* old) const; private: - static rviz::Property* createScalar(const QString& name, const QString& description, const QByteArray& value, - rviz::Property* old); + static rviz_common::properties::Property* createScalar(const QString& name, const QString& description, + const QByteArray& value, + rviz_common::properties::Property* old); // return true if there was no error so far bool noError() const { return parser_.error == YAML_NO_ERROR; } // parse a single event and return it's type, YAML_ERROR_EVENT on parsing error int parse(yaml_event_t& event) const; // process events: scalar, start mapping, start sequence - rviz::Property* process(const yaml_event_t& event, const QString& name, const QString& description, - rviz::Property* old) const; + rviz_common::properties::Property* process(const yaml_event_t& event, const QString& name, + const QString& description, rviz_common::properties::Property* old) const; inline static QByteArray byteArray(const yaml_event_t& event) { assert(event.type == YAML_SCALAR_EVENT); return QByteArray::fromRawData(reinterpret_cast(event.data.scalar.value), event.data.scalar.length); } - // Try to set value of existing rviz::Property (expecting matching types). Return false on error. - static bool setValue(rviz::Property* old, const QByteArray& value); + // Try to set value of existing rviz_common::properties::Property (expecting matching types). Return false on error. + static bool setValue(rviz_common::properties::Property* old, const QByteArray& value); - static rviz::Property* createParent(const QString& name, const QString& description, rviz::Property* old); - rviz::Property* processMapping(const QString& name, const QString& description, rviz::Property* old) const; - rviz::Property* processSequence(const QString& name, const QString& description, rviz::Property* old) const; + static rviz_common::properties::Property* createParent(const QString& name, const QString& description, + rviz_common::properties::Property* old); + rviz_common::properties::Property* processMapping(const QString& name, const QString& description, + rviz_common::properties::Property* old) const; + rviz_common::properties::Property* processSequence(const QString& name, const QString& description, + rviz_common::properties::Property* old) const; private: mutable yaml_parser_t parser_; @@ -116,7 +121,8 @@ int Parser::parse(yaml_event_t& event) const { } // main processing function -rviz::Property* Parser::process(const QString& name, const QString& description, rviz::Property* old) const { +rviz_common::properties::Property* Parser::process(const QString& name, const QString& description, + rviz_common::properties::Property* old) const { bool stop = false; while (!stop) { ScopedYamlEvent event; @@ -141,8 +147,9 @@ rviz::Property* Parser::process(const QString& name, const QString& description, } // default processing for scalar, start mapping, start sequence events -rviz::Property* Parser::process(const yaml_event_t& event, const QString& name, const QString& description, - rviz::Property* old) const { +rviz_common::properties::Property* Parser::process(const yaml_event_t& event, const QString& name, + const QString& description, + rviz_common::properties::Property* old) const { switch (event.type) { case YAML_SEQUENCE_START_EVENT: return processSequence(name, description, old); @@ -153,18 +160,20 @@ rviz::Property* Parser::process(const yaml_event_t& event, const QString& name, default: throw std::runtime_error("Unhandled YAML event"); } + assert(false); // should not be reached + return nullptr; } // Try to set numeric or arbitrary scalar value from YAML node. Needs to match old's type. -bool Parser::setValue(rviz::Property* old, const QByteArray& value) { - if (rviz::FloatProperty* p = dynamic_cast(old)) { +bool Parser::setValue(rviz_common::properties::Property* old, const QByteArray& value) { + if (rviz_common::properties::FloatProperty* p = dynamic_cast(old)) { bool ok = true; double v = value.toDouble(&ok); if (ok) p->setValue(v); return ok; } - if (rviz::StringProperty* p = dynamic_cast(old)) { + if (rviz_common::properties::StringProperty* p = dynamic_cast(old)) { // value should be an arbitrary string. If not throws YAML::BadConversion p->setValue(value); return true; @@ -173,9 +182,10 @@ bool Parser::setValue(rviz::Property* old, const QByteArray& value) { } // Update existing old rviz:Property or create a new one from scalar YAML node -rviz::Property* Parser::createScalar(const QString& name, const QString& description, const QByteArray& value, - rviz::Property* old) { - // try to update value, expecting matching rviz::Property +rviz_common::properties::Property* Parser::createScalar(const QString& name, const QString& description, + const QByteArray& value, + rviz_common::properties::Property* old) { + // try to update value, expecting matching rviz_common::properties::Property if (old && setValue(old, value)) { // only if setValue succeeded, also update the rest old->setName(name); @@ -186,21 +196,23 @@ rviz::Property* Parser::createScalar(const QString& name, const QString& descrip bool ok = true; double v = value.toDouble(&ok); if (ok) // if value is a number, create a FloatProperty - old = new rviz::FloatProperty(name, v, description); + old = new rviz_common::properties::FloatProperty(name, v, description); else // otherwise create a StringProperty - old = new rviz::StringProperty(name, value, description); + old = new rviz_common::properties::StringProperty(name, value, description); old->setReadOnly(true); return old; } // Reuse old property (or create new one) as parent for a sequence or map -rviz::Property* Parser::createParent(const QString& name, const QString& description, rviz::Property* old) { +rviz_common::properties::Property* Parser::createParent(const QString& name, const QString& description, + rviz_common::properties::Property* old) { // don't reuse float or string properties (they are for scalars) - if (dynamic_cast(old) || dynamic_cast(old)) + if (dynamic_cast(old) || + dynamic_cast(old)) old = nullptr; if (!old) { - old = new rviz::Property(name, QVariant(), description); + old = new rviz_common::properties::Property(name, QVariant(), description); old->setReadOnly(true); } else { old->setName(name); @@ -210,7 +222,8 @@ rviz::Property* Parser::createParent(const QString& name, const QString& descrip } // Hierarchically create property from YAML map node -rviz::Property* Parser::processMapping(const QString& name, const QString& description, rviz::Property* root) const { +rviz_common::properties::Property* Parser::processMapping(const QString& name, const QString& description, + rviz_common::properties::Property* root) const { root = createParent(name, description, root); int index = 0; // current child index in root bool stop = false; @@ -233,11 +246,11 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr num = root->numChildren(); // if names differ, insert a new child, otherwise reuse existing - rviz::Property* old_child = index < num ? root->childAt(index) : nullptr; + rviz_common::properties::Property* old_child = index < num ? root->childAt(index) : nullptr; if (old_child && old_child->getName() != key) old_child = nullptr; - rviz::Property* new_child = nullptr; + rviz_common::properties::Property* new_child = nullptr; switch (parse(event)) { // parse value case YAML_MAPPING_START_EVENT: case YAML_SEQUENCE_START_EVENT: @@ -268,7 +281,8 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr } // Hierarchically create property from YAML sequence node. Items are named [#]. -rviz::Property* Parser::processSequence(const QString& name, const QString& description, rviz::Property* root) const { +rviz_common::properties::Property* Parser::processSequence(const QString& name, const QString& description, + rviz_common::properties::Property* root) const { root = createParent(name, description, root); int index = 0; // current child index in root bool stop = false; @@ -282,8 +296,8 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc case YAML_MAPPING_START_EVENT: case YAML_SEQUENCE_START_EVENT: case YAML_SCALAR_EVENT: { - rviz::Property* old_child = root->childAt(index); // nullptr for invalid index - rviz::Property* new_child = process(event, QString("[%1]").arg(index), "", old_child); + rviz_common::properties::Property* old_child = root->childAt(index); // nullptr for invalid index + rviz_common::properties::Property* new_child = process(event, QString("[%1]").arg(index), "", old_child); if (new_child != old_child) root->addChild(new_child, index); if (++index >= 10) @@ -305,9 +319,10 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc namespace moveit_rviz_plugin { -rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, - const std::string& description, const std::string& value, - rviz::Property* old) { +rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, + const std::string& description, + const std::string& value, + rviz_common::properties::Property* old) { QString qname = QString::fromStdString(name); QString qdesc = QString::fromStdString(description); Parser parser(value); diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index 832134c72..a46edd0ba 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -6,7 +6,7 @@ qt_wrap_ui(UIC_FILES global_settings.ui ) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED factory_model.cpp icons.cpp job_queue.cpp @@ -28,7 +28,7 @@ add_library(${MOVEIT_LIB_NAME} set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools - ${catkin_LIBRARIES} ${QT_LIBRARIES} + ${QT_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ @@ -37,12 +37,9 @@ target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ PUBLIC $ PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} -) -target_include_directories(${MOVEIT_LIB_NAME} SYSTEM - PUBLIC ${rviz_OGRE_INCLUDE_DIRS} ) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT export_${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/motion_planning_tasks/src/factory_model.cpp b/visualization/motion_planning_tasks/src/factory_model.cpp index 18054a450..445ed9034 100644 --- a/visualization/motion_planning_tasks/src/factory_model.cpp +++ b/visualization/motion_planning_tasks/src/factory_model.cpp @@ -35,29 +35,29 @@ /* Author: Robert Haschke */ #include "factory_model.h" -#include +#include #include #include namespace moveit_rviz_plugin { -FactoryModel::FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent) +FactoryModel::FactoryModel(rviz_common::Factory& factory, const QString& mime_type, QObject* parent) : QStandardItemModel(parent), mime_type_(mime_type) { setHorizontalHeaderLabels({ tr("Name") }); fillTree(factory); } -void FactoryModel::fillTree(rviz::Factory& factory) { - QIcon default_package_icon = rviz::loadPixmap("package://rviz/icons/default_package_icon.png"); +void FactoryModel::fillTree(rviz_common::Factory& factory) { + QIcon default_package_icon = rviz_common::loadPixmap("package://rviz/icons/default_package_icon.png"); - QStringList classes = factory.getDeclaredClassIds(); - classes.sort(); + auto plugins = factory.getDeclaredPlugins(); + std::sort(plugins.begin(), plugins.end()); // Map from package names to the corresponding top-level tree widget items. std::map package_items; - for (const QString& lookup_name : classes) { - QString package = factory.getClassPackage(lookup_name); + for (const auto& plugin : plugins) { + const QString& package = plugin.package; QStandardItem* package_item; auto mi = package_items.find(package); @@ -68,9 +68,9 @@ void FactoryModel::fillTree(rviz::Factory& factory) { } else { package_item = mi->second; } - QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name), factory.getClassName(lookup_name)); - class_item->setWhatsThis(factory.getClassDescription(lookup_name)); - class_item->setData(lookup_name, Qt::UserRole); + QStandardItem* class_item = new QStandardItem(plugin.icon, plugin.name); + class_item->setWhatsThis(plugin.description); + class_item->setData(plugin.id, Qt::UserRole); class_item->setDragEnabled(true); package_item->appendRow(class_item); } diff --git a/visualization/motion_planning_tasks/src/factory_model.h b/visualization/motion_planning_tasks/src/factory_model.h index cbdf991d1..634425aba 100644 --- a/visualization/motion_planning_tasks/src/factory_model.h +++ b/visualization/motion_planning_tasks/src/factory_model.h @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace moveit_rviz_plugin { @@ -47,10 +47,10 @@ namespace moveit_rviz_plugin { class FactoryModel : public QStandardItemModel { QString mime_type_; - void fillTree(rviz::Factory& factory); + void fillTree(rviz_common::Factory& factory); public: - FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent = nullptr); + FactoryModel(rviz_common::Factory& factory, const QString& mime_type, QObject* parent = nullptr); QStringList mimeTypes() const override; QMimeData* mimeData(const QModelIndexList& indexes) const override; diff --git a/visualization/motion_planning_tasks/src/global_settings.ui b/visualization/motion_planning_tasks/src/global_settings.ui index 4efb8c45e..a61bc45cc 100644 --- a/visualization/motion_planning_tasks/src/global_settings.ui +++ b/visualization/motion_planning_tasks/src/global_settings.ui @@ -51,7 +51,7 @@ - + @@ -59,9 +59,9 @@ - rviz::PropertyTreeWidget + rviz_common::properties::PropertyTreeWidget QTreeView -
rviz/properties/property_tree_widget.h
+
rviz_common/properties/property_tree_widget.hpp
 diff --git a/visualization/motion_planning_tasks/src/job_queue.cpp b/visualization/motion_planning_tasks/src/job_queue.cpp index 929bf4547..d49f2e4cc 100644 --- a/visualization/motion_planning_tasks/src/job_queue.cpp +++ b/visualization/motion_planning_tasks/src/job_queue.cpp @@ -35,7 +35,9 @@ /* Author: Robert Haschke */ #include "job_queue.h" -#include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.job_queue"); namespace moveit { namespace tools { @@ -71,7 +73,7 @@ void JobQueue::executeJobs() { try { fn(); } catch (std::exception& ex) { - ROS_ERROR("Exception caught executing main loop job: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught executing main loop job: %s", ex.what()); } ulock.lock(); } diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index 8fcb62467..bfe4be299 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -37,9 +37,7 @@ #include "local_task_model.h" #include "factory_model.h" #include "properties/property_factory.h" -#include - -#include +#include #include @@ -78,7 +76,7 @@ QModelIndex LocalTaskModel::index(Node* n) const { } LocalTaskModel::LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene, - rviz::DisplayContext* display_context, QObject* parent) + rviz_common::DisplayContext* display_context, QObject* parent) : BaseTaskModel(scene, display_context, parent), Task("", true, std::move(container)) { root_ = this; flags_ |= LOCAL_MODEL; @@ -235,7 +233,7 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex& /*index*/) { return DisplaySolutionPtr(); } -rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex& index) { +rviz_common::properties::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex& index) { Node* n = node(index); if (!n) return nullptr; diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index dd34bd22d..c01832d58 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -47,14 +47,14 @@ class LocalTaskModel : public BaseTaskModel, public moveit::task_constructor::Ta using Node = moveit::task_constructor::Stage; Node* root_; StageFactoryPtr stage_factory_; - std::map properties_; + std::map properties_; inline Node* node(const QModelIndex& index) const; QModelIndex index(Node* n) const; public: LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene, - rviz::DisplayContext* display_context, QObject* parent = nullptr); + rviz_common::DisplayContext* display_context, QObject* parent = nullptr); int rowCount(const QModelIndex& parent = QModelIndex()) const override; QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override; @@ -76,6 +76,6 @@ class LocalTaskModel : public BaseTaskModel, public moveit::task_constructor::Ta QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex& index) override; - rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; + rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/plugin_init.cpp b/visualization/motion_planning_tasks/src/plugin_init.cpp index 1bf297082..660bee24b 100644 --- a/visualization/motion_planning_tasks/src/plugin_init.cpp +++ b/visualization/motion_planning_tasks/src/plugin_init.cpp @@ -34,9 +34,9 @@ /* Author: Robert Haschke */ -#include +#include #include "task_display.h" #include "task_panel.h" -PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz::Display) -PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz::Panel) +PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz_common::Panel) diff --git a/visualization/motion_planning_tasks/src/pluginlib_factory.h b/visualization/motion_planning_tasks/src/pluginlib_factory.h index 77b91206a..9c325edb4 100644 --- a/visualization/motion_planning_tasks/src/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/src/pluginlib_factory.h @@ -45,12 +45,13 @@ #include #ifndef Q_MOC_RUN -#include -#include +#include +#include #include #endif -#include +#include +#include namespace moveit_rviz_plugin { @@ -58,7 +59,7 @@ namespace moveit_rviz_plugin { * This is a slightly modified version of rviz::PluginlibFactory, providing a custom mime type. */ template -class PluginlibFactory : public rviz::Factory +class PluginlibFactory : public rviz_common::Factory { private: struct BuiltInClassRecord @@ -80,41 +81,39 @@ class PluginlibFactory : public rviz::Factory /// retrieve mime type used for given factory QString mimeType() const { return mime_type_; } - QStringList getDeclaredClassIds() override { - QStringList ids; - for (const auto& record : built_ins_) - ids.push_back(record.class_id_); + std::vector getDeclaredPlugins() override { + std::vector plugins; + for (auto iter = built_ins_.cbegin(); iter != built_ins_.cend(); ++iter) + plugins.emplace_back(getPluginInfo(iter.key())); for (const auto& id : class_loader_->getDeclaredClasses()) { - QString sid = QString::fromStdString(id); - if (ids.contains(sid)) + auto sid = QString::fromStdString(id); + if (std::find_if(plugins.cbegin(), plugins.cend(), [&sid](const rviz_common::PluginInfo& plugin_info) { + return plugin_info.id == sid; + }) != plugins.cend()) continue; // built_in take precedence - ids.push_back(sid); + plugins.emplace_back(getPluginInfo(QString::fromStdString(id))); } - return ids; + return plugins; } - QString getClassDescription(const QString& class_id) const override { - auto it = built_ins_.find(class_id); - if (it != built_ins_.end()) { - return it->description_; - } - return QString::fromStdString(class_loader_->getClassDescription(class_id.toStdString())); - } - - QString getClassName(const QString& class_id) const override { - auto it = built_ins_.find(class_id); - if (it != built_ins_.end()) { - return it->name_; - } - return QString::fromStdString(class_loader_->getName(class_id.toStdString())); - } - - QString getClassPackage(const QString& class_id) const override { - auto it = built_ins_.find(class_id); - if (it != built_ins_.end()) { - return it->package_; + rviz_common::PluginInfo getPluginInfo(const QString& class_id) const override { + rviz_common::PluginInfo info; + const auto iter = built_ins_.find(class_id); + if (iter != built_ins_.end()) { + info.id = iter->class_id_; + info.name = iter->name_; + info.package = iter->package_; + info.description = iter->description_; + info.icon = getIcon(info); + return info; } - return QString::fromStdString(class_loader_->getClassPackage(class_id.toStdString())); + auto class_id_std = class_id.toStdString(); + info.id = class_id; + info.name = QString::fromStdString(class_loader_->getName(class_id_std)); + info.package = QString::fromStdString(class_loader_->getClassPackage(class_id_std)); + info.description = QString::fromStdString(class_loader_->getClassDescription(class_id_std)); + info.icon = getIcon(info); + return info; } virtual QString getPluginManifestPath(const QString& class_id) const { @@ -125,14 +124,12 @@ class PluginlibFactory : public rviz::Factory return QString::fromStdString(class_loader_->getPluginManifestPath(class_id.toStdString())); } - QIcon getIcon(const QString& class_id) const override { - QString package = getClassPackage(class_id); - QString class_name = getClassName(class_id); - QIcon icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".svg"); + QIcon getIcon(const rviz_common::PluginInfo& info) const { + QIcon icon = rviz_common::loadPixmap("package://" + info.package + "/icons/classes/" + info.name + ".svg"); if (icon.isNull()) { - icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".png"); + icon = rviz_common::loadPixmap("package://" + info.package + "/icons/classes/" + info.name + ".png"); if (icon.isNull()) { - icon = rviz::loadPixmap("package://rviz/icons/default_class_icon.png"); + icon = rviz_common::loadPixmap("package://rviz/icons/default_class_icon.png"); } } return icon; @@ -175,8 +172,9 @@ class PluginlibFactory : public rviz::Factory try { return class_loader_->createUnmanagedInstance(class_id.toStdString()); } catch (pluginlib::PluginlibException& ex) { - ROS_ERROR("PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id), - ex.what()); + RCLCPP_ERROR(rclcpp::get_logger("moveit_task_constructor_visualization.pluginlib_factory"), + "PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id), + ex.what()); if (error_return) { *error_return = QString::fromStdString(ex.what()); } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 79347694a..de4057b4d 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -41,15 +41,16 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_list_model"); + using namespace moveit::task_constructor; namespace moveit_rviz_plugin { @@ -69,12 +70,12 @@ struct RemoteTaskModel::Node InterfaceFlags interface_flags_; NodeFlags node_flags_; std::unique_ptr solutions_; - std::unique_ptr property_tree_; + std::unique_ptr property_tree_; std::map properties_; inline Node(Node* parent) : parent_(parent) { solutions_.reset(new RemoteSolutionModel()); - property_tree_.reset(new rviz::PropertyTreeModel(new rviz::Property())); + property_tree_.reset(new rviz_common::properties::PropertyTreeModel(new rviz_common::properties::Property())); } bool setName(const QString& name) { @@ -84,18 +85,20 @@ struct RemoteTaskModel::Node return true; } - void setProperties(const std::vector& props, - const planning_scene::PlanningSceneConstPtr& scene_, rviz::DisplayContext* display_context_); - rviz::Property* createProperty(const moveit_task_constructor_msgs::Property& prop, rviz::Property* old, - const planning_scene::PlanningSceneConstPtr& scene_, - rviz::DisplayContext* display_context_); + void setProperties(const std::vector& props, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz_common::DisplayContext* display_context_); + rviz_common::properties::Property* createProperty(const moveit_task_constructor_msgs::msg::Property& prop, + rviz_common::properties::Property* old, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz_common::DisplayContext* display_context_); }; -void RemoteTaskModel::Node::setProperties(const std::vector& props, +void RemoteTaskModel::Node::setProperties(const std::vector& props, const planning_scene::PlanningSceneConstPtr& scene_, - rviz::DisplayContext* display_context_) { + rviz_common::DisplayContext* display_context_) { // insert properties in same order as reported in description - rviz::Property* root = property_tree_->getRoot(); + rviz_common::properties::Property* root = property_tree_->getRoot(); int index = 0; // current child index in root for (const auto& prop : props) { int num = root->numChildren(); @@ -108,11 +111,11 @@ void RemoteTaskModel::Node::setProperties(const std::vectornumChildren(); // if names differ, insert a new child, otherwise reuse existing - rviz::Property* old_child = index < num ? root->childAt(index) : nullptr; + rviz_common::properties::Property* old_child = index < num ? root->childAt(index) : nullptr; if (old_child && old_child->getName().toStdString() != prop.name) old_child = nullptr; - rviz::Property* new_child = createProperty(prop, old_child, scene_, display_context_); + rviz_common::properties::Property* new_child = createProperty(prop, old_child, scene_, display_context_); if (new_child != old_child) root->addChild(new_child, index); ++index; @@ -121,25 +124,26 @@ void RemoteTaskModel::Node::setProperties(const std::vectorremoveChildren(index, root->numChildren() - index); } -rviz::Property* RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Property& prop, - rviz::Property* old, - const planning_scene::PlanningSceneConstPtr& scene_, - rviz::DisplayContext* display_context_) { +rviz_common::properties::Property* RemoteTaskModel::Node::createProperty( + const moveit_task_constructor_msgs::msg::Property& prop, rviz_common::properties::Property* old, + const planning_scene::PlanningSceneConstPtr& scene_, rviz_common::DisplayContext* display_context_) { auto& factory = PropertyFactory::instance(); // try to deserialize from msg (using registered functions) boost::any value = Property::deserialize(prop.type, prop.value); - if (!value.empty()) { // if successful, create rviz::Property from mtc::Property using factory methods + if (!value.empty()) { // if successful, create rviz_common::properties::Property from mtc::Property using factory + // methods auto it = properties_.insert(std::make_pair(prop.name, Property())).first; it->second.setDescription(prop.description); it->second.setValue(value); - if (rviz::Property* rviz_prop = factory.create(prop.name, it->second, scene_.get(), display_context_)) { + if (rviz_common::properties::Property* rviz_prop = + factory.create(prop.name, it->second, scene_.get(), display_context_)) { rviz_prop->setReadOnly(true); return rviz_prop; } else properties_.erase(it); } - // otherwise create default, read-only rviz::Property by parsing serialized YAML + // otherwise create default, read-only rviz_common::properties::Property by parsing serialized YAML return factory.createDefault(prop.name, prop.type, prop.description, prop.value, old); } @@ -149,7 +153,7 @@ RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex& index) const { return root_; if (index.model() != this) { - ROS_ERROR_NAMED("TaskModel", "invalid model in QModelIndex"); + RCLCPP_ERROR(LOGGER, "invalid model in QModelIndex"); return nullptr; } @@ -180,13 +184,18 @@ QModelIndex RemoteTaskModel::index(const Node* n) const { return QModelIndex(); } -RemoteTaskModel::RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, - const planning_scene::PlanningSceneConstPtr& scene, - rviz::DisplayContext* display_context, QObject* parent) +RemoteTaskModel::RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene, + rviz_common::DisplayContext* display_context, QObject* parent) : BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) { id_to_stage_[0] = root_; // root node has ID 0 + // Add random ID to prevent warnings about multiple publishers within the same node + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-r", + "__node:=get_solution_node_" + std::to_string(reinterpret_cast(this)), "-r", + "__ns:=/moveit_task_constructor/remote_task_model" }); + node_ = rclcpp::Node::make_shared("_", options); // service to request solutions - get_solution_client_ = nh.serviceClient(service_name); + get_solution_client_ = node_->create_client(service_name); } RemoteTaskModel::~RemoteTaskModel() { @@ -277,13 +286,14 @@ QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const { return n ? index(n) : QModelIndex(); } -void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg) { +void RemoteTaskModel::processStageDescriptions( + const moveit_task_constructor_msgs::msg::TaskDescription::_stages_type& msg) { // iterate over descriptions and create new / update existing nodes where needed for (const auto& s : msg) { // find parent node for stage s, this should always exist auto parent_it = id_to_stage_.find(s.parent_id); if (parent_it == id_to_stage_.end()) { - ROS_ERROR_NAMED("TaskListModel", "No parent found for stage %d (%s)", s.id, s.name.c_str()); + RCLCPP_ERROR(LOGGER, "No parent found for stage %d (%s)", s.id, s.name.c_str()); continue; } Node* parent = parent_it->second; @@ -336,13 +346,14 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg } } -void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg) { +void RemoteTaskModel::processStageStatistics( + const moveit_task_constructor_msgs::msg::TaskStatistics::_stages_type& msg) { // iterate over statistics and update node's solutions where needed for (const auto& s : msg) { // find node for stage s, this should always exist auto it = id_to_stage_.find(s.id); if (it == id_to_stage_.end()) { - ROS_ERROR_NAMED("TaskListModel", "No stage %d", s.id); + RCLCPP_ERROR(LOGGER, "No stage %d", s.id); continue; } Node* n = it->second; @@ -356,14 +367,14 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs: } } -void RemoteTaskModel::setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info) { +void RemoteTaskModel::setSolutionData(const moveit_task_constructor_msgs::msg::SolutionInfo& info) { if (info.id == 0) return; if (RemoteSolutionModel* m = getSolutionModel(info.stage_id)) m->setSolutionData(info.id, info.cost, QString::fromStdString(info.comment)); } -DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) { +DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) { DisplaySolutionPtr s(new DisplaySolution); s->setFromMessage(scene_->diff(), msg); @@ -416,18 +427,21 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) { if (it == id_to_solution_.cend()) { // TODO: try to assemble (and cache) the solution from known leaves // to avoid some communication overhead - DisplaySolutionPtr result; if (!(flags_ & IS_DESTROYED)) { - // request solution via service - moveit_task_constructor_msgs::GetSolution srv; - srv.request.solution_id = id; - if (get_solution_client_.call(srv)) { - id_to_solution_[id] = result = processSolutionMessage(srv.response.solution); - return result; + if (get_solution_client_->service_is_ready()) { + // request solution via service + auto request = std::make_shared(); + request->solution_id = id; + auto result_future = get_solution_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) { + id_to_solution_[id] = result = processSolutionMessage(result_future.get()->solution); + return result; + } } // on failure mark remote task as destroyed: don't retrieve more solutions - get_solution_client_.shutdown(); + get_solution_client_.reset(); + node_.reset(); flags_ |= IS_DESTROYED; } return result; @@ -435,7 +449,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) { return it->second; } -rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) { +rviz_common::properties::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) { Node* n = node(index); if (!n) return nullptr; diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index 8db812beb..6343ecd6f 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -38,7 +38,8 @@ #include "task_list_model.h" #include -#include +#include +#include #include #include @@ -54,7 +55,11 @@ class RemoteTaskModel : public BaseTaskModel Q_OBJECT struct Node; Node* const root_; - ros::ServiceClient get_solution_client_; + rclcpp::Client::SharedPtr get_solution_client_; + // TODO(JafarAbdi): We shouldn't need this, replace with callback groups (should be fully available in Galactic) + // RViz have a single threaded executor which is causing the get_solution_client_ to timeout without + // getting the result + rclcpp::Node::SharedPtr node_; std::map id_to_stage_; std::map id_to_solution_; @@ -64,12 +69,11 @@ class RemoteTaskModel : public BaseTaskModel Node* node(uint32_t stage_id) const; inline RemoteSolutionModel* getSolutionModel(uint32_t stage_id) const; - void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info); + void setSolutionData(const moveit_task_constructor_msgs::msg::SolutionInfo& info); public: - RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, - const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, - QObject* parent = nullptr); + RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene, + rviz_common::DisplayContext* display_context, QObject* parent = nullptr); ~RemoteTaskModel() override; int rowCount(const QModelIndex& parent = QModelIndex()) const override; @@ -81,14 +85,14 @@ class RemoteTaskModel : public BaseTaskModel bool setData(const QModelIndex& index, const QVariant& value, int role = Qt::EditRole) override; QModelIndex indexFromStageId(size_t id) const override; - void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg); - void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg); - DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg); + void processStageDescriptions(const moveit_task_constructor_msgs::msg::TaskDescription::_stages_type& msg); + void processStageStatistics(const moveit_task_constructor_msgs::msg::TaskStatistics::_stages_type& msg); + DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg); QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex& index) override; - rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; + rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; /** Model representing solutions of a remote task */ diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 8a3e875a9..53d716aa9 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -44,19 +44,22 @@ #include #include #include -#include +#include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_display"); + namespace moveit_rviz_plugin { TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_description_(false) { @@ -71,12 +74,12 @@ TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_d connect(task_list_model_.get(), SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(onTaskDataChanged(QModelIndex, QModelIndex))); - robot_description_property_ = new rviz::StringProperty( + robot_description_property_ = new rviz_common::properties::StringProperty( "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", this, SLOT(changedRobotDescription()), this); - task_solution_topic_property_ = new rviz::RosTopicProperty( - "Task Solution Topic", "", ros::message_traits::datatype(), + task_solution_topic_property_ = new rviz_common::properties::RosTopicProperty( + "Task Solution Topic", "", rosidl_generator_traits::data_type(), "The topic on which task solutions (moveit_msgs::Solution messages) are received", this, SLOT(changedTaskSolutionTopic()), this); @@ -84,7 +87,8 @@ TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_d connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)), task_list_model_.get(), SLOT(highlightStage(size_t))); - tasks_property_ = new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); + tasks_property_ = + new rviz_common::properties::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); } TaskDisplay::~TaskDisplay() { @@ -94,6 +98,8 @@ TaskDisplay::~TaskDisplay() { void TaskDisplay::onInitialize() { Display::onInitialize(); + rviz_ros_node_ = context_->getRosNodeAbstraction(); + task_solution_topic_property_->initialize(rviz_ros_node_); trajectory_visual_->onInitialize(scene_node_, context_); task_list_model_->setDisplayContext(context_); } @@ -109,18 +115,19 @@ inline void TaskDisplay::requestPanel() { } void TaskDisplay::loadRobotModel() { - rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); + rdf_loader_.reset( + new rdf_loader::RDFLoader(rviz_ros_node_.lock()->get_raw_node(), robot_description_property_->getStdString())); if (!rdf_loader_->getURDF()) { - this->setStatus(rviz::StatusProperty::Error, "Robot Model", + this->setStatus(rviz_common::properties::StatusProperty::Error, "Robot Model", "Failed to load from parameter " + robot_description_property_->getString()); return; } - this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded"); + this->setStatus(rviz_common::properties::StatusProperty::Ok, "Robot Model", "Successfully loaded"); const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); // Send to child class trajectory_visual_->onRobotModelLoaded(robot_model_); @@ -139,11 +146,11 @@ void TaskDisplay::reset() { trajectory_visual_->reset(); } -void TaskDisplay::save(rviz::Config config) const { +void TaskDisplay::save(rviz_common::Config config) const { Display::save(config); } -void TaskDisplay::load(const rviz::Config& config) { +void TaskDisplay::load(const rviz_common::Config& config) { Display::load(config); } @@ -170,7 +177,8 @@ void TaskDisplay::calculateOffsetPosition() { Ogre::Vector3 position; Ogre::Quaternion orientation; - context_->getFrameManager()->getTransform(robot_model_->getModelFrame(), ros::Time(0), position, orientation); + context_->getFrameManager()->getTransform(robot_model_->getModelFrame(), rclcpp::Time(0, 0, RCL_ROS_TIME), position, + orientation); scene_node_->setPosition(position); scene_node_->setOrientation(orientation); @@ -183,11 +191,6 @@ void TaskDisplay::update(float wall_dt, float ros_dt) { trajectory_visual_->update(wall_dt, ros_dt); } -void TaskDisplay::setName(const QString& name) { - BoolProperty::setName(name); - trajectory_visual_->setName(name); -} - void TaskDisplay::changedRobotDescription() { if (isEnabled()) reset(); @@ -195,28 +198,37 @@ void TaskDisplay::changedRobotDescription() { loadRobotModel(); } -void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); +void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg) { + setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); requestPanel(); - task_list_model_->processTaskDescriptionMessage(*msg, update_nh_, - base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); + task_list_model_->processTaskDescriptionMessage(*msg, base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); // Start listening to other topics if this is the first description // Waiting for the description ensures we do not receive data that cannot be interpreted yet if (!received_task_description_ && !msg->stages.empty()) { + auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_INFO(LOGGER, "Unable to lock weak_ptr from DisplayContext in taskDescriptionCB"); + return; + } + auto node = ros_node_abstraction->get_raw_node(); received_task_description_ = true; - task_statistics_sub = update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); - task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this); + task_statistics_sub = node->create_subscription( + base_ns_ + STATISTICS_TOPIC, rclcpp::QoS(2).transient_local(), + std::bind(&TaskDisplay::taskStatisticsCB, this, std::placeholders::_1)); + task_solution_sub = node->create_subscription( + base_ns_ + SOLUTION_TOPIC, rclcpp::QoS(2).transient_local(), + std::bind(&TaskDisplay::taskSolutionCB, this, std::placeholders::_1)); } } -void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg) { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); +void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg) { + setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); task_list_model_->processTaskStatisticsMessage(*msg); } -void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); +void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg) { + setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); try { const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg); if (s) @@ -224,7 +236,7 @@ void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionCon else setSolutionStatus(false); } catch (const std::invalid_argument& e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(LOGGER, e.what()); setSolutionStatus(false, e.what()); } } @@ -234,33 +246,41 @@ void TaskDisplay::changedTaskSolutionTopic() { if (!trajectory_visual_->getScene()) return; - task_description_sub.shutdown(); - task_statistics_sub.shutdown(); - task_solution_sub.shutdown(); + task_description_sub.reset(); + task_statistics_sub.reset(); + task_solution_sub.reset(); received_task_description_ = false; // generate task monitoring topics from solution topic const QString& solution_topic = task_solution_topic_property_->getString(); if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) { - setStatus(rviz::StatusProperty::Error, "Task Monitor", + setStatus(rviz_common::properties::StatusProperty::Error, "Task Monitor", QString("Invalid topic. Expecting a name ending on \"/%1\"").arg(SOLUTION_TOPIC)); return; } base_ns_ = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC)); + auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_INFO(LOGGER, "Unable to lock weak_ptr from DisplayContext in changedTaskSolutionTopic"); + return; + } // listen to task descriptions updates - task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 10, &TaskDisplay::taskDescriptionCB, this); + task_description_sub = + ros_node_abstraction->get_raw_node()->create_subscription( + base_ns_ + DESCRIPTION_TOPIC, rclcpp::QoS(10).transient_local(), + std::bind(&TaskDisplay::taskDescriptionCB, this, std::placeholders::_1)); - setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Task Monitor", "No messages received"); } void TaskDisplay::setSolutionStatus(bool ok, const char* msg) { if (ok) - setStatus(rviz::StatusProperty::Ok, "Solution", "Ok"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Solution", "Ok"); else - setStatus(rviz::StatusProperty::Warn, "Solution", msg ? msg : "Retrieval failed"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Solution", msg ? msg : "Retrieval failed"); } void TaskDisplay::onTasksInserted(const QModelIndex& parent, int first, int last) { @@ -270,7 +290,8 @@ void TaskDisplay::onTasksInserted(const QModelIndex& parent, int first, int last TaskListModel* m = static_cast(sender()); for (; first <= last; ++first) { QModelIndex idx = m->index(first, 0, parent); - tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first); + tasks_property_->addChild( + new rviz_common::properties::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first); } } @@ -289,7 +310,7 @@ void TaskDisplay::onTaskDataChanged(const QModelIndex& topLeft, const QModelInde return; // only handle top-level items for (int row = topLeft.row(); row <= bottomRight.row(); ++row) { - rviz::Property* child = tasks_property_->childAt(row); + rviz_common::properties::Property* child = tasks_property_->childAt(row); assert(child); if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index ec38dd8c2..f91587c2c 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -38,22 +38,26 @@ #pragma once -#include +#include +#include #include #ifndef Q_MOC_RUN #include "job_queue.h" +#include "local_task_model.h" #include -#include -#include -#include -#include +#include +#include +#include +#include #endif -namespace rviz { +namespace rviz_common { +namespace properties { class StringProperty; class RosTopicProperty; -} // namespace rviz +} // namespace properties +} // namespace rviz_common namespace moveit { namespace core { @@ -69,7 +73,7 @@ namespace moveit_rviz_plugin { MOVEIT_CLASS_FORWARD(DisplaySolution); class TaskListModel; -class TaskDisplay : public rviz::Display +class TaskDisplay : public rviz_common::Display { Q_OBJECT @@ -81,10 +85,9 @@ class TaskDisplay : public rviz::Display void update(float wall_dt, float ros_dt) override; void reset() override; - void save(rviz::Config config) const override; - void load(const rviz::Config& config) override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config& config) override; - void setName(const QString& name) override; void setSolutionStatus(bool ok, const char* msg = ""); TaskListModel& getTaskListModel() { return *task_list_model_; } @@ -113,14 +116,19 @@ private Q_SLOTS: void onTasksRemoved(const QModelIndex& parent, int first, int last); void onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight); - void taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg); - void taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg); - void taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg); + void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg); + void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg); + void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg); protected: - ros::Subscriber task_solution_sub; - ros::Subscriber task_description_sub; - ros::Subscriber task_statistics_sub; + /** @brief A Node which is registered with the main executor (used in the "update" thread). + * + * This is configured after the constructor within the initialize() method of Display. */ + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; + + rclcpp::Subscription::SharedPtr task_solution_sub; + rclcpp::Subscription::SharedPtr task_description_sub; + rclcpp::Subscription::SharedPtr task_statistics_sub; // The trajectory playback component std::unique_ptr trajectory_visual_; @@ -138,9 +146,9 @@ private Q_SLOTS: bool received_task_description_; // Properties - rviz::StringProperty* robot_description_property_; - rviz::RosTopicProperty* task_solution_topic_property_; - rviz::Property* tasks_property_; + rviz_common::properties::StringProperty* robot_description_property_; + rviz_common::properties::RosTopicProperty* task_solution_topic_property_; + rviz_common::properties::Property* tasks_property_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 485318ef3..21a542612 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -42,8 +42,6 @@ #include "factory_model.h" #include "icons.h" -#include - #include #include #include @@ -54,7 +52,7 @@ using namespace moveit::task_constructor; namespace moveit_rviz_plugin { -static const std::string LOGNAME("TaskListModel"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_list_model"); QVariant TaskListModel::horizontalHeader(int column, int role) { switch (role) { @@ -145,7 +143,7 @@ StageFactoryPtr getStageFactory() { factory = result; // remember for future uses return result; } catch (const std::exception& e) { - ROS_ERROR("Failed to initialize StageFactory"); + RCLCPP_ERROR(LOGGER, "Failed to initialize StageFactory"); return StageFactoryPtr(); } } @@ -164,12 +162,12 @@ void TaskListModel::onRemoveModel(QAbstractItemModel* model) { TaskListModel::TaskListModel(QObject* parent) : FlatMergeProxyModel(parent), old_task_handling_(TaskView::OLD_TASK_REPLACE) { - ROS_DEBUG_NAMED(LOGNAME, "created TaskListModel: %p", this); + RCLCPP_DEBUG(LOGGER, "created TaskListModel: %p", this); setStageFactory(getStageFactory()); } TaskListModel::~TaskListModel() { - ROS_DEBUG_NAMED(LOGNAME, "destroying TaskListModel: %p", this); + RCLCPP_DEBUG(LOGGER, "destroying TaskListModel: %p", this); // inform MetaTaskListModel that we will remove our stuff removeRows(0, rowCount()); // free RemoteTaskModels @@ -181,7 +179,7 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr& scene) scene_ = scene; } -void TaskListModel::setDisplayContext(rviz::DisplayContext* display_context) { +void TaskListModel::setDisplayContext(rviz_common::DisplayContext* display_context) { display_context_ = display_context; } @@ -241,8 +239,8 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const { // process a task description message: // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one -void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, - ros::NodeHandle& nh, const std::string& service_name) { +void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::msg::TaskDescription& msg, + const std::string& service_name) { // retrieve existing or insert new remote task for given task id auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr)); const auto& task_it = it_inserted.first; @@ -267,9 +265,9 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ remote_task->processStageDescriptions(msg.stages); } else if (!remote_task) { // create new task model, if ID was not known before // the model is managed by this instance via Qt's parent-child mechanism - remote_task = new RemoteTaskModel(nh, service_name, scene_, display_context_, this); + remote_task = new RemoteTaskModel(service_name, scene_, display_context_, this); remote_task->processStageDescriptions(msg.stages); - ROS_DEBUG_NAMED(LOGNAME, "received new task: %s (%s)", msg.stages[0].name.c_str(), msg.task_id.c_str()); + RCLCPP_DEBUG(LOGGER, "received new task: %s (%s)", msg.stages[0].name.c_str(), msg.task_id.c_str()); // insert newly created model into this' model instance insertModel(remote_task, -1); @@ -280,10 +278,10 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ } // process a task statistics message -void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) { +void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::msg::TaskStatistics& msg) { auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) { - ROS_WARN("unknown task: %s", msg.task_id.c_str()); + RCLCPP_WARN(LOGGER, "unknown task: %s", msg.task_id.c_str()); return; } @@ -294,7 +292,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m remote_task->processStageStatistics(msg.stages); } -DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) { +DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) { auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) return DisplaySolutionPtr(); // unkown task diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 0386543d2..b6e6d6c1b 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -42,10 +42,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -53,10 +53,12 @@ #include #include -namespace rviz { +namespace rviz_common { +namespace properties { class PropertyTreeModel; +} class DisplayContext; -} // namespace rviz +} // namespace rviz_common namespace moveit_rviz_plugin { @@ -74,7 +76,7 @@ class BaseTaskModel : public QAbstractItemModel protected: unsigned int flags_ = 0; planning_scene::PlanningSceneConstPtr scene_; - rviz::DisplayContext* display_context_; + rviz_common::DisplayContext* display_context_; public: enum TaskModelFlag @@ -85,7 +87,7 @@ class BaseTaskModel : public QAbstractItemModel IS_RUNNING = 0x08, }; - BaseTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, + BaseTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz_common::DisplayContext* display_context, QObject* parent = nullptr) : QAbstractItemModel(parent), scene_(scene), display_context_(display_context) {} @@ -106,7 +108,7 @@ class BaseTaskModel : public QAbstractItemModel virtual DisplaySolutionPtr getSolution(const QModelIndex& index) = 0; /// get property model for given stage index - virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; + virtual rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; }; /** The TaskListModel maintains a list of multiple BaseTaskModels, local and/or remote. @@ -124,7 +126,7 @@ class TaskListModel : public utils::FlatMergeProxyModel // planning scene / robot model used by all tasks in this model planning_scene::PlanningSceneConstPtr scene_; // rviz::DisplayContext used to show (interactive) markers by the property models - rviz::DisplayContext* display_context_ = nullptr; + rviz_common::DisplayContext* display_context_ = nullptr; // map from remote task IDs to (active) tasks // if task is destroyed remotely, it is marked with flag IS_DESTROYED @@ -146,7 +148,7 @@ class TaskListModel : public utils::FlatMergeProxyModel ~TaskListModel() override; void setScene(const planning_scene::PlanningSceneConstPtr& scene); - void setDisplayContext(rviz::DisplayContext* display_context); + void setDisplayContext(rviz_common::DisplayContext* display_context); void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } int columnCount(const QModelIndex& /*parent*/ = QModelIndex()) const override { return 4; } @@ -155,12 +157,12 @@ class TaskListModel : public utils::FlatMergeProxyModel QVariant data(const QModelIndex& index, int role) const override; /// process an incoming task description message - only call in Qt's main loop - void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, ros::NodeHandle& nh, + void processTaskDescriptionMessage(const moveit_task_constructor_msgs::msg::TaskDescription& msg, const std::string& service_name); /// process an incoming task description message - only call in Qt's main loop - void processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg); + void processTaskStatisticsMessage(const moveit_task_constructor_msgs::msg::TaskStatistics& msg); /// process an incoming solution message - only call in Qt's main loop - DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg); + DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg); /// insert a TaskModel, pos is relative to modelCount() bool insertModel(BaseTaskModel* model, int pos = -1); diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index b73e2e15b..3c829f4fb 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -48,21 +48,25 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_view"); + namespace moveit_rviz_plugin { -rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) { - static QPointer widget = nullptr; +rviz_common::PanelDockWidget* getStageDockWidget(rviz_common::WindowManagerInterface* mgr) { + static QPointer widget = nullptr; if (!widget && mgr) { // create widget StageFactoryPtr factory = getStageFactory(); if (!factory) @@ -83,7 +87,7 @@ static QPointer SINGLETON; // count active TaskDisplays static uint DISPLAY_COUNT = 0; -TaskPanel::TaskPanel(QWidget* parent) : rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this)) { +TaskPanel::TaskPanel(QWidget* parent) : rviz_common::Panel(parent), d_ptr(new TaskPanelPrivate(this)) { Q_D(TaskPanel); // sync checked tool button with displayed widget @@ -148,10 +152,10 @@ void TaskPanel::addSubPanel(SubPanel* w, const QString& title, const QIcon& icon * will never be called if the display is disabled... */ -void TaskPanel::request(rviz::WindowManagerInterface* window_manager) { +void TaskPanel::request(rviz_common::WindowManagerInterface* window_manager) { ++DISPLAY_COUNT; - rviz::VisualizationFrame* vis_frame = dynamic_cast(window_manager); + rviz_common::VisualizationFrame* vis_frame = dynamic_cast(window_manager); if (SINGLETON || !vis_frame) return; // already defined, nothing to do @@ -173,23 +177,23 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) { tool_buttons_group->setExclusive(true); button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); - property_root = new rviz::Property("Global Settings"); + property_root = new rviz_common::properties::Property("Global Settings"); } void TaskPanel::onInitialize() { - d_ptr->window_manager_ = vis_manager_->getWindowManager(); + d_ptr->window_manager_ = getDisplayContext()->getWindowManager(); } -void TaskPanel::save(rviz::Config config) const { - rviz::Panel::save(config); +void TaskPanel::save(rviz_common::Config config) const { + rviz_common::Panel::save(config); for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) { SubPanel* w = static_cast(d_ptr->stackedWidget->widget(i)); w->save(config.mapMakeChild(w->windowTitle())); } } -void TaskPanel::load(const rviz::Config& config) { - rviz::Panel::load(config); +void TaskPanel::load(const rviz_common::Config& config) { + rviz_common::Panel::load(config); for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) { SubPanel* w = static_cast(d_ptr->stackedWidget->widget(i)); w->load(config.mapGetChild(w->windowTitle())); @@ -197,7 +201,7 @@ void TaskPanel::load(const rviz::Config& config) { } void TaskPanel::showStageDockWidget() { - rviz::PanelDockWidget* dock = getStageDockWidget(d_ptr->window_manager_); + rviz_common::PanelDockWidget* dock = getStageDockWidget(d_ptr->window_manager_); if (dock) dock->show(); } @@ -216,9 +220,15 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep view->setExpanded(index, expand); } -TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view), exec_action_client_("execute_task_solution") { +TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) { setupUi(view); + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-r", "__node:=task_view_private" }); + node_ = rclcpp::Node::make_shared("_", "", options); + exec_action_client_ = rclcpp_action::create_client( + node_, "execute_task_solution"); + MetaTaskListModel* meta_model = &MetaTaskListModel::instance(); StageFactoryPtr factory = getStageFactory(); if (factory) @@ -292,7 +302,7 @@ void TaskViewPrivate::lock(TaskDisplay* display) { locked_display_ = display; } -TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) +TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz_common::properties::Property* root) : SubPanel(parent), d_ptr(new TaskViewPrivate(this)) { Q_D(TaskView); @@ -318,23 +328,24 @@ TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) SIGNAL(configChanged())); // configuration settings - auto configs = new rviz::Property("Task View Settings", QVariant(), QString(), root); - initial_task_expand = - new rviz::EnumProperty("Task Expansion", "All Expanded", "Configure how to initially expand new tasks", configs); + auto configs = new rviz_common::properties::Property("Task View Settings", QVariant(), QString(), root); + initial_task_expand = new rviz_common::properties::EnumProperty( + "Task Expansion", "All Expanded", "Configure how to initially expand new tasks", configs); initial_task_expand->addOption("Top-level Expanded", EXPAND_TOP); initial_task_expand->addOption("All Expanded", EXPAND_ALL); initial_task_expand->addOption("All Closed", EXPAND_NONE); - old_task_handling = - new rviz::EnumProperty("Old task handling", "Keep", - "Configure what to do with old tasks whose solutions cannot be queried anymore", configs); + old_task_handling = new rviz_common::properties::EnumProperty( + "Old task handling", "Keep", "Configure what to do with old tasks whose solutions cannot be queried anymore", + configs); old_task_handling->addOption("Keep", OLD_TASK_KEEP); old_task_handling->addOption("Replace", OLD_TASK_REPLACE); old_task_handling->addOption("Remove", OLD_TASK_REMOVE); - connect(old_task_handling, &rviz::Property::changed, this, &TaskView::onOldTaskHandlingChanged); + connect(old_task_handling, &rviz_common::properties::Property::changed, this, &TaskView::onOldTaskHandlingChanged); - show_time_column = new rviz::BoolProperty("Show Computation Times", true, "Show the 'time' column", configs); - connect(show_time_column, &rviz::Property::changed, this, &TaskView::onShowTimeChanged); + show_time_column = + new rviz_common::properties::BoolProperty("Show Computation Times", true, "Show the 'time' column", configs); + connect(show_time_column, &rviz_common::properties::Property::changed, this, &TaskView::onShowTimeChanged); d_ptr->configureExistingModels(); } @@ -343,11 +354,11 @@ TaskView::~TaskView() { delete d_ptr; } -void TaskView::save(rviz::Config config) { +void TaskView::save(rviz_common::Config config) { auto write_splitter_sizes = [&config](QSplitter* splitter, const QString& key) { - rviz::Config group = config.mapMakeChild(key); + rviz_common::Config group = config.mapMakeChild(key); for (int s : splitter->sizes()) { - rviz::Config item = group.listAppendNew(); + rviz_common::Config item = group.listAppendNew(); item.setValue(s); } }; @@ -355,9 +366,9 @@ void TaskView::save(rviz::Config config) { write_splitter_sizes(d_ptr->tasks_solutions_splitter, "solutions_splitter"); auto write_column_sizes = [&config](QHeaderView* view, const QString& key) { - rviz::Config group = config.mapMakeChild(key); + rviz_common::Config group = config.mapMakeChild(key); for (int c = 0, end = view->count(); c != end; ++c) { - rviz::Config item = group.listAppendNew(); + rviz_common::Config item = group.listAppendNew(); item.setValue(view->sectionSize(c)); } }; @@ -365,21 +376,21 @@ void TaskView::save(rviz::Config config) { write_column_sizes(d_ptr->solutions_view->header(), "solutions_view_columns"); const QHeaderView* view = d_ptr->solutions_view->header(); - rviz::Config group = config.mapMakeChild("solution_sorting"); + rviz_common::Config group = config.mapMakeChild("solution_sorting"); group.mapSetValue("column", view->sortIndicatorSection()); group.mapSetValue("order", view->sortIndicatorOrder()); } -void TaskView::load(const rviz::Config& config) { +void TaskView::load(const rviz_common::Config& config) { if (!config.isValid()) return; auto read_sizes = [&config](const QString& key) { - rviz::Config group = config.mapGetChild(key); + rviz_common::Config group = config.mapGetChild(key); QList sizes, empty; for (int i = 0; i < group.listLength(); ++i) { - rviz::Config item = group.listChildAt(i); - if (item.getType() != rviz::Config::Value) + rviz_common::Config item = group.listChildAt(i); + if (item.getType() != rviz_common::Config::Value) return empty; QVariant value = item.getValue(); bool ok = false; @@ -401,7 +412,7 @@ void TaskView::load(const rviz::Config& config) { d_ptr->tasks_view->setColumnWidth(++column, w); QTreeView* view = d_ptr->solutions_view; - rviz::Config group = config.mapGetChild("solution_sorting"); + rviz_common::Config group = config.mapGetChild("solution_sorting"); int order = 0; if (group.mapGetInt("column", &column) && group.mapGetInt("order", &order)) view->sortByColumn(column, static_cast(order)); @@ -488,7 +499,7 @@ void TaskView::onCurrentSolutionChanged(const QModelIndex& current, const QModel solution = task->getSolution(current); display->setSolutionStatus(bool(solution)); } catch (const std::invalid_argument& e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(LOGGER, e.what()); display->setSolutionStatus(false, e.what()); } vis->interruptCurrentDisplay(); @@ -511,7 +522,7 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection& /*selected*/, co solution = task->getSolution(index); display->setSolutionStatus(bool(solution)); } catch (const std::invalid_argument& e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(LOGGER, e.what()); display->setSolutionStatus(false, e.what()); } display->addMarkers(solution); @@ -528,14 +539,21 @@ void TaskView::onExecCurrentSolution() const { const DisplaySolutionPtr& solution = task->getSolution(current); - if (!d_ptr->exec_action_client_.waitForServer(ros::Duration(0.1))) { - ROS_ERROR("Failed to connect to task execution action"); + if (!d_ptr->exec_action_client_->wait_for_action_server(std::chrono::milliseconds(100))) { + RCLCPP_ERROR(LOGGER, "Failed to connect to task execution action"); return; } - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; + moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal; solution->fillMessage(goal.solution); - d_ptr->exec_action_client_.sendGoal(goal); + auto goal_handle_future = d_ptr->exec_action_client_->async_send_goal(goal); + if (rclcpp::spin_until_future_complete(d_ptr->node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(LOGGER, "send goal call failed"); + return; + } + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) + RCLCPP_ERROR(LOGGER, "Goal was rejected by server"); } void TaskView::onShowTimeChanged() { @@ -550,30 +568,33 @@ void TaskView::onOldTaskHandlingChanged() { Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt()); } -GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* widget, rviz::Property* root) +GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* widget, + rviz_common::properties::Property* root) : q_ptr(widget) { setupUi(widget); - properties = new rviz::PropertyTreeModel(root, widget); + properties = new rviz_common::properties::PropertyTreeModel(root, widget); view->setModel(properties); } -GlobalSettingsWidget::GlobalSettingsWidget(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) +GlobalSettingsWidget::GlobalSettingsWidget(moveit_rviz_plugin::TaskPanel* parent, + rviz_common::properties::Property* root) : SubPanel(parent), d_ptr(new GlobalSettingsWidgetPrivate(this, root)) { Q_D(GlobalSettingsWidget); d->view->expandAll(); - connect(d->properties, &rviz::PropertyTreeModel::configChanged, this, &GlobalSettingsWidget::configChanged); + connect(d->properties, &rviz_common::properties::PropertyTreeModel::configChanged, this, + &GlobalSettingsWidget::configChanged); } GlobalSettingsWidget::~GlobalSettingsWidget() { delete d_ptr; } -void GlobalSettingsWidget::save(rviz::Config config) { +void GlobalSettingsWidget::save(rviz_common::Config config) { d_ptr->properties->getRoot()->save(config); } -void GlobalSettingsWidget::load(const rviz::Config& config) { +void GlobalSettingsWidget::load(const rviz_common::Config& config) { d_ptr->properties->getRoot()->load(config); } } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index f87d2d05c..efd95d263 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -38,18 +38,21 @@ #pragma once -#include +#include #include #include class QItemSelection; class QIcon; -namespace rviz { +namespace rviz_common { class WindowManagerInterface; +class VisualizationManager; +namespace properties { class Property; class BoolProperty; class EnumProperty; -} // namespace rviz +} // namespace properties +} // namespace rviz_common namespace moveit_rviz_plugin { @@ -64,16 +67,15 @@ class SubPanel : public QWidget public: SubPanel(QWidget* parent = nullptr) : QWidget(parent) {} - virtual void save(rviz::Config /*config*/) {} // NOLINT(performance-unnecessary-value-param) - virtual void load(const rviz::Config& /*config*/) {} - + virtual void save(rviz_common::Config /*config*/) {} // NOLINT(performance-unnecessary-value-param) + virtual void load(const rviz_common::Config& /*config*/) {} Q_SIGNALS: void configChanged(); }; /** The TaskPanel is the central panel of this plugin, collecting various sub panels. */ class TaskPanelPrivate; -class TaskPanel : public rviz::Panel +class TaskPanel : public rviz_common::Panel { Q_OBJECT Q_DECLARE_PRIVATE(TaskPanel) @@ -91,12 +93,12 @@ class TaskPanel : public rviz::Panel * If not yet done, an instance is created. If use count drops to zero, * the global instance is destroyed. */ - static void request(rviz::WindowManagerInterface* window_manager); + static void request(rviz_common::WindowManagerInterface* window_manager); static void release(); void onInitialize() 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; protected Q_SLOTS: void showStageDockWidget(); @@ -125,9 +127,9 @@ class TaskView : public SubPanel EXPAND_NONE }; - rviz::EnumProperty* initial_task_expand; - rviz::EnumProperty* old_task_handling; - rviz::BoolProperty* show_time_column; + rviz_common::properties::EnumProperty* initial_task_expand; + rviz_common::properties::EnumProperty* old_task_handling; + rviz_common::properties::BoolProperty* show_time_column; public: enum OldTaskHandling @@ -137,11 +139,11 @@ class TaskView : public SubPanel OLD_TASK_REMOVE }; - TaskView(TaskPanel* parent, rviz::Property* root); + TaskView(TaskPanel* parent, rviz_common::properties::Property* root); ~TaskView() override; - void save(rviz::Config config) override; - void load(const rviz::Config& config) override; + void save(rviz_common::Config config) override; + void load(const rviz_common::Config& config) override; public Q_SLOTS: void addTask(); @@ -170,10 +172,10 @@ class GlobalSettingsWidget : public SubPanel GlobalSettingsWidgetPrivate* d_ptr; public: - GlobalSettingsWidget(TaskPanel* parent, rviz::Property* root); + GlobalSettingsWidget(TaskPanel* parent, rviz_common::properties::Property* root); ~GlobalSettingsWidget() override; - void save(rviz::Config config) override; - void load(const rviz::Config& config) override; + void save(rviz_common::Config config) override; + void load(const rviz_common::Config& config) override; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index abd9bf5e1..2d9885a7f 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -42,11 +42,11 @@ #include "ui_task_panel.h" #include "ui_task_view.h" #include "ui_global_settings.h" -#include -#include +#include +#include -#include -#include +#include +#include #include namespace moveit_rviz_plugin { @@ -62,9 +62,9 @@ class TaskPanelPrivate : public Ui_TaskPanel TaskPanel* q_ptr; QButtonGroup* tool_buttons_group; - rviz::Property* property_root; + rviz_common::properties::Property* property_root; - rviz::WindowManagerInterface* window_manager_; + rviz_common::WindowManagerInterface* window_manager_; }; class TaskViewPrivate : public Ui_TaskView @@ -91,15 +91,16 @@ class TaskViewPrivate : public Ui_TaskView TaskView* q_ptr; QPointer locked_display_; - actionlib::SimpleActionClient exec_action_client_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Client::SharedPtr exec_action_client_; }; class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget { public: - GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root); + GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz_common::properties::Property* root); GlobalSettingsWidget* q_ptr; - rviz::PropertyTreeModel* properties; + rviz_common::properties::PropertyTreeModel* properties; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_view.ui b/visualization/motion_planning_tasks/src/task_view.ui index 1ce79f03e..00783d4a4 100644 --- a/visualization/motion_planning_tasks/src/task_view.ui +++ b/visualization/motion_planning_tasks/src/task_view.ui @@ -129,7 +129,7 @@ - + @@ -174,9 +174,9 @@
task_list_model.h
- rviz::PropertyTreeWidget + rviz_common::properties::PropertyTreeWidget QTreeView -
rviz/properties/property_tree_widget.h
+
rviz_common/properties/property_tree_widget.hpp
 moveit_rviz_plugin::SolutionListView diff --git a/visualization/motion_planning_tasks/test/CMakeLists.txt b/visualization/motion_planning_tasks/test/CMakeLists.txt index a35c0373f..97667690e 100644 --- a/visualization/motion_planning_tasks/test/CMakeLists.txt +++ b/visualization/motion_planning_tasks/test/CMakeLists.txt @@ -3,18 +3,22 @@ ############# ## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) +if (BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) - catkin_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp) + ament_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp) target_link_libraries(${PROJECT_NAME}-test-merge-models - motion_planning_tasks_utils gtest_main) + motion_planning_tasks_utils) - catkin_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp) + ament_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp) target_link_libraries(${PROJECT_NAME}-test-solution-models - motion_planning_tasks_rviz_plugin gtest_main) + motion_planning_tasks_rviz_plugin) - add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp) + ament_add_gtest_executable(${PROJECT_NAME}-test-task_model test_task_model.cpp) target_link_libraries(${PROJECT_NAME}-test-task_model - motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest) + motion_planning_tasks_rviz_plugin) + add_launch_test(test_task_model.launch.py + ARGS "test_binary_dir:=$") endif() diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 86909c200..53f625ada 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include #include @@ -50,19 +50,18 @@ using namespace moveit::task_constructor; class TaskListModelTest : public ::testing::Test { protected: - ros::NodeHandle nh; moveit_rviz_plugin::TaskListModel model; int children = 0; int num_inserts = 0; int num_updates = 0; - moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name, - const std::string& task_id = std::string()) { - moveit_task_constructor_msgs::TaskDescription t; + moveit_task_constructor_msgs::msg::TaskDescription genMsg(const std::string& name, + const std::string& task_id = std::string()) { + moveit_task_constructor_msgs::msg::TaskDescription t; uint id = 0, root_id; t.task_id = task_id.empty() ? name : task_id; - moveit_task_constructor_msgs::StageDescription desc; + moveit_task_constructor_msgs::msg::StageDescription desc; desc.parent_id = id; desc.id = root_id = ++id; desc.name = name; @@ -129,7 +128,7 @@ class TaskListModelTest : public ::testing::Test SCOPED_TRACE("first i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task @@ -143,7 +142,7 @@ class TaskListModelTest : public ::testing::Test SCOPED_TRACE("second i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage(genMsg("second"), nh, "get_solution"); // 1 notify for inserted task + model.processTaskDescriptionMessage(genMsg("second"), "get_solution"); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); @@ -165,17 +164,13 @@ class TaskListModelTest : public ::testing::Test TEST_F(TaskListModelTest, remoteTaskModel) { children = 3; planning_scene::PlanningSceneConstPtr scene; - moveit_rviz_plugin::RemoteTaskModel m(nh, "get_solution", scene, nullptr); + moveit_rviz_plugin::RemoteTaskModel m("get_solution", scene, nullptr); m.processStageDescriptions(genMsg("first").stages); SCOPED_TRACE("first"); validate(m, { "first" }); } TEST_F(TaskListModelTest, localTaskModel) { - int argc = 0; - char* argv = nullptr; - ros::init(argc, &argv, "testLocalTaskModel"); - children = 3; const char* task_name = "task pipeline"; moveit_rviz_plugin::LocalTaskModel m(std::make_unique(task_name), @@ -187,6 +182,9 @@ TEST_F(TaskListModelTest, localTaskModel) { SCOPED_TRACE("localTaskModel"); validate(m, { task_name }); } + // There's a bug where cancelling the executor in IntrospectionPrivate is happening before the call to spin causing + // the it to stuck in the destructor when calling executor_thread_.join() + rclcpp::sleep_for(std::chrono::milliseconds(100)); } TEST_F(TaskListModelTest, noChildren) { @@ -202,13 +200,13 @@ TEST_F(TaskListModelTest, threeChildren) { TEST_F(TaskListModelTest, visitedPopulate) { // first population without children children = 0; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); validate(model, { "first" }); // validation visits root node EXPECT_EQ(num_inserts, 1); children = 3; num_inserts = 0; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); validate(model, { "first" }); // second population with children should emit insert notifies for them EXPECT_EQ(num_inserts, 3); @@ -217,7 +215,7 @@ TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, deletion) { children = 3; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); auto m = model.getModel(model.index(0, 0)).first; int num_deletes = 0; QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; }); @@ -232,6 +230,6 @@ TEST_F(TaskListModelTest, deletion) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_task_model"); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch b/visualization/motion_planning_tasks/test/test_task_model.launch deleted file mode 100644 index 3aebf2981..000000000 --- a/visualization/motion_planning_tasks/test/test_task_model.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch.py b/visualization/motion_planning_tasks/test/test_task_model.launch.py new file mode 100644 index 000000000..40cf5cbca --- /dev/null +++ b/visualization/motion_planning_tasks/test/test_task_model.launch.py @@ -0,0 +1,52 @@ +import unittest + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +import launch_testing +from launch_testing.asserts import assertExitCodes +from launch_testing.util import KeepAliveProc +from launch_testing.actions import ReadyToTest, GTest + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + test_task_model = GTest( + path=[ + PathJoinSubstitution( + [ + LaunchConfiguration("test_binary_dir"), + "moveit_task_constructor_visualization-test-task_model", + ] + ) + ], + output="screen", + ) + return ( + LaunchDescription( + [ + DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package containing test executables", + ), + test_task_model, + KeepAliveProc(), + ReadyToTest(), + ] + ), + {"test_task_model": test_task_model}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, test_task_model): + proc_info.assertWaitForShutdown(process=test_task_model, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TaskModelTestAfterShutdown(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/visualization/motion_planning_tasks/utils/CMakeLists.txt b/visualization/motion_planning_tasks/utils/CMakeLists.txt index cab82caee..436145dcc 100644 --- a/visualization/motion_planning_tasks/utils/CMakeLists.txt +++ b/visualization/motion_planning_tasks/utils/CMakeLists.txt @@ -12,9 +12,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} ) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/motion_planning_tasks_rviz_plugin_description.xml b/visualization/motion_planning_tasks_rviz_plugin_description.xml index 0dd6e4c24..fd8f3f783 100644 --- a/visualization/motion_planning_tasks_rviz_plugin_description.xml +++ b/visualization/motion_planning_tasks_rviz_plugin_description.xml @@ -1,14 +1,14 @@ - + + base_class_type="rviz_common::Panel"> A panel widget to monitor and edit motion planning tasks + base_class_type="rviz_common::Display"> Monitor motion planning tasks and animate their solution trajectories diff --git a/visualization/package.xml b/visualization/package.xml index 196503bf6..088389009 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -7,21 +7,24 @@ Robert Haschke Michael Goerner - catkin + ament_cmake qtbase5-dev moveit_core moveit_task_constructor_msgs moveit_task_constructor_core moveit_ros_visualization - roscpp - rviz + rclcpp + rviz2 - rosunit - rostest - moveit_resources_fanuc_moveit_config + ament_cmake_gmock + ament_cmake_gtest + launch + launch_testing + launch_testing_ament_cmake + launch_testing_ros - - + ament_cmake + diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 49116a1ec..8c35ec20a 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -2,11 +2,6 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools) -# TODO: Remove when Kinetic support is dropped -if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2? - add_definitions(-DRVIZ_TF1) -endif() - set(HEADERS ${PROJECT_INCLUDE}/display_solution.h ${PROJECT_INCLUDE}/marker_visualization.h @@ -14,7 +9,7 @@ set(HEADERS ${PROJECT_INCLUDE}/task_solution_visualization.h ) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED ${HEADERS} src/display_solution.cpp @@ -23,25 +18,27 @@ add_library(${MOVEIT_LIB_NAME} src/task_solution_visualization.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - target_link_libraries(${MOVEIT_LIB_NAME} - ${catkin_LIBRARIES} - ${rviz_DEFAULT_PLUGIN_LIBRARIES} - ${OGRE_LIBRARIES} ${QT_LIBRARIES} - ${Boost_LIBRARIES} + rviz_ogre_vendor::OgreMain ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include - PRIVATE ${catkin_INCLUDE_DIRS} ) -target_include_directories(${MOVEIT_LIB_NAME} SYSTEM - PUBLIC ${rviz_OGRE_INCLUDE_DIRS} +ament_target_dependencies(${MOVEIT_LIB_NAME} + Boost + pluginlib + moveit_task_constructor_msgs + moveit_ros_visualization + moveit_core + rclcpp + rviz_common + rviz_default_plugins ) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT export_${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 408b30221..fae2d866b 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace moveit { @@ -53,7 +53,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory); namespace Ogre { class SceneNode; } -namespace rviz { +namespace rviz_common { class DisplayContext; } @@ -125,7 +125,7 @@ class DisplaySolution const MarkerVisualizationPtr markersOfSubTrajectory(size_t index) const { return data_.at(index).markers_; } void setFromMessage(const planning_scene::PlanningScenePtr& start_scene, - const moveit_task_constructor_msgs::Solution& msg); - void fillMessage(moveit_task_constructor_msgs::Solution& msg) const; + const moveit_task_constructor_msgs::msg::Solution& msg); + void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg) const; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index fe743bf08..1db0d028e 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -1,8 +1,8 @@ #pragma once -#include +#include #include -#include +#include #include #include #include @@ -11,10 +11,17 @@ namespace Ogre { class SceneNode; } -namespace rviz { +namespace rviz_common { class DisplayContext; +} // namespace rviz_common + +namespace rviz_default_plugins { +namespace displays { +namespace markers { class MarkerBase; -} // namespace rviz +} +} // namespace displays +} // namespace rviz_default_plugins namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -41,10 +48,10 @@ class MarkerVisualization // list of all markers, attached to scene nodes in namespaces_ struct MarkerData { - visualization_msgs::MarkerPtr msg_; - std::shared_ptr marker_; + visualization_msgs::msg::Marker::SharedPtr msg_; + std::shared_ptr marker_; - MarkerData(const visualization_msgs::Marker& marker); + MarkerData(const visualization_msgs::msg::Marker& marker); }; struct NamespaceData { @@ -64,14 +71,14 @@ class MarkerVisualization bool markers_created_ = false; public: - MarkerVisualization(const std::vector& markers, + MarkerVisualization(const std::vector& markers, const planning_scene::PlanningScene& end_scene); ~MarkerVisualization(); /// did we successfully created all markers (and scene nodes)? bool created() const { return markers_created_; } /// create markers (placed at planning frame of scene) - bool createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node); + bool createMarkers(rviz_common::DisplayContext* context, Ogre::SceneNode* scene_node); /// update marker position/orientation based on frames of given scene + robot_state void update(const planning_scene::PlanningScene& end_scene, const moveit::core::RobotState& robot_state); @@ -88,22 +95,22 @@ class MarkerVisualization * The class remembers which MarkerVisualization instances are currently hosted * and provides the user interaction to toggle marker visibility by namespace. */ -class MarkerVisualizationProperty : public rviz::BoolProperty +class MarkerVisualizationProperty : public rviz_common::properties::BoolProperty { Q_OBJECT - rviz::DisplayContext* context_ = nullptr; + rviz_common::DisplayContext* context_ = nullptr; Ogre::SceneNode* parent_scene_node_ = nullptr; // scene node provided externally Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to - std::map namespaces_; // rviz properties for encountered namespaces + std::map namespaces_; // rviz properties for encountered namespaces std::list hosted_markers_; // list of hosted MarkerVisualization instances - rviz::BoolProperty* all_markers_at_once_; + rviz_common::properties::BoolProperty* all_markers_at_once_; public: MarkerVisualizationProperty(const QString& name, Property* parent = nullptr); ~MarkerVisualizationProperty() override; - void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context); + void onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context); /// remove all hosted markers from display void clearMarkers(); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h index e74aeb828..4e6e31eee 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h @@ -37,17 +37,17 @@ #pragma once #ifndef Q_MOC_RUN -#include +#include #endif -#include +#include #include #include #include namespace moveit_rviz_plugin { -class TaskSolutionPanel : public rviz::Panel +class TaskSolutionPanel : public rviz_common::Panel { Q_OBJECT diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index d78dab5f1..101903cef 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #include #include @@ -47,10 +47,13 @@ namespace Ogre { class SceneNode; } -namespace rviz { -class Display; -class DisplayContext; +namespace rviz_default_plugins { +namespace robot { class Robot; +} +} // namespace rviz_default_plugins +namespace rviz_common { +namespace properties { class Property; class IntProperty; class StringProperty; @@ -60,8 +63,11 @@ class RosTopicProperty; class EnumProperty; class EditableEnumProperty; class ColorProperty; +} // namespace properties +class Display; +class DisplayContext; class PanelDockWidget; -} // namespace rviz +} // namespace rviz_common namespace moveit { namespace core { @@ -91,24 +97,24 @@ class TaskSolutionVisualization : public QObject public: /** * \brief Playback a trajectory from a planned path - * \param parent - either a rviz::Display or rviz::Property + * \param parent - either a rviz::Display _commonorproperties:: rviz::Property * \param display - the rviz::Display from the parent * \return true on success */ - TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display); + TaskSolutionVisualization(rviz_common::properties::Property* parent, rviz_common::Display* display); ~TaskSolutionVisualization() override; virtual void update(float wall_dt, float ros_dt); virtual void reset(); - void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context); + void onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context); void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); void onEnable(); void onDisable(); void setName(const QString& name); planning_scene::PlanningSceneConstPtr getScene() const { return scene_; } - void showTrajectory(const moveit_task_constructor_msgs::Solution& msg); + void showTrajectory(const moveit_task_constructor_msgs::msg::Solution& msg); void showTrajectory(const moveit_rviz_plugin::DisplaySolutionPtr& s, bool lock); void unlock(); @@ -156,12 +162,12 @@ private Q_SLOTS: MarkerVisualizationProperty* marker_visual_; // Handle colouring of robot - void setRobotColor(rviz::Robot* robot, const QColor& color); - void unsetRobotColor(rviz::Robot* robot); + void setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color); + void unsetRobotColor(rviz_default_plugins::robot::Robot* robot); DisplaySolutionPtr displaying_solution_; DisplaySolutionPtr next_solution_to_display_; - std::vector trail_; + std::vector trail_; bool animating_ = false; // auto-progressing the current waypoint? bool drop_displaying_solution_ = false; bool locked_ = false; @@ -172,36 +178,36 @@ private Q_SLOTS: planning_scene::PlanningScenePtr scene_; // Pointers from parent display that we save - rviz::Display* display_; // the parent display that this class populates + rviz_common::Display* display_; // the parent display that this class populates Ogre::SceneNode* parent_scene_node_; // parent scene node provided by display Ogre::SceneNode* main_scene_node_; // to be added/removed to/from scene_node_ Ogre::SceneNode* trail_scene_node_; // to be added/removed to/from scene_node_ - rviz::DisplayContext* context_; + rviz_common::DisplayContext* context_; TaskSolutionPanel* slider_panel_ = nullptr; - rviz::PanelDockWidget* slider_dock_panel_ = nullptr; + rviz_common::PanelDockWidget* slider_dock_panel_ = nullptr; bool slider_panel_was_visible_ = false; // Trajectory Properties - rviz::Property* robot_property_; - rviz::BoolProperty* robot_visual_enabled_property_; - rviz::BoolProperty* robot_collision_enabled_property_; - rviz::FloatProperty* robot_alpha_property_; - rviz::ColorProperty* robot_color_property_; - rviz::BoolProperty* enable_robot_color_property_; - - rviz::EditableEnumProperty* state_display_time_property_; - rviz::BoolProperty* loop_display_property_; - rviz::BoolProperty* trail_display_property_; - rviz::BoolProperty* interrupt_display_property_; - rviz::IntProperty* trail_step_size_property_; + rviz_common::properties::Property* robot_property_; + rviz_common::properties::BoolProperty* robot_visual_enabled_property_; + rviz_common::properties::BoolProperty* robot_collision_enabled_property_; + rviz_common::properties::FloatProperty* robot_alpha_property_; + rviz_common::properties::ColorProperty* robot_color_property_; + rviz_common::properties::BoolProperty* enable_robot_color_property_; + + rviz_common::properties::EditableEnumProperty* state_display_time_property_; + rviz_common::properties::BoolProperty* loop_display_property_; + rviz_common::properties::BoolProperty* trail_display_property_; + rviz_common::properties::BoolProperty* interrupt_display_property_; + rviz_common::properties::IntProperty* trail_step_size_property_; // PlanningScene Properties - rviz::BoolProperty* scene_enabled_property_; - rviz::FloatProperty* scene_alpha_property_; - rviz::ColorProperty* scene_color_property_; - rviz::ColorProperty* attached_body_color_property_; - rviz::EnumProperty* octree_render_property_; - rviz::EnumProperty* octree_coloring_property_; + rviz_common::properties::BoolProperty* scene_enabled_property_; + rviz_common::properties::FloatProperty* scene_alpha_property_; + rviz_common::properties::ColorProperty* scene_color_property_; + rviz_common::properties::ColorProperty* attached_body_color_property_; + rviz_common::properties::EnumProperty* octree_render_property_; + rviz_common::properties::EnumProperty* octree_coloring_property_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 3d5dee7a7..ce3ac3bfa 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -38,8 +38,10 @@ #include #include #include -#include #include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.display_solution"); namespace moveit_rviz_plugin { @@ -65,7 +67,7 @@ float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair& idx_pair return data_[idx_pair.first].trajectory_->getWayPointDurationFromPrevious(idx_pair.second); } -const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { +const moveit::core::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { return data_[idx_pair.first].trajectory_->getWayPointPtr(idx_pair.second); } @@ -87,7 +89,7 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind } void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene, - const moveit_task_constructor_msgs::Solution& msg) { + const moveit_task_constructor_msgs::msg::Solution& msg) { if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) { static boost::format fmt("Solution for model '%s' but model '%s' was expected"); fmt % msg.start_scene.robot_model_name.c_str() % start_scene->getRobotModel()->getName().c_str(); @@ -127,7 +129,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta } } -void DisplaySolution::fillMessage(moveit_task_constructor_msgs::Solution& msg) const { +void DisplaySolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg) const { start_scene_->getPlanningSceneMsg(msg.start_scene); msg.sub_trajectory.resize(data_.size()); auto traj_it = msg.sub_trajectory.begin(); diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index b36717729..e7a88cfb0 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -1,36 +1,41 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -#ifndef RVIZ_TF1 -#include +#include +#include +#if __has_include() +#include +#else +#include #endif -#include -#include -#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.marker_visualization"); namespace moveit_rviz_plugin { // create MarkerData with nil marker_ pointer, just with a copy of message -MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker) { - msg_.reset(new visualization_msgs::Marker(marker)); - msg_->header.stamp = ros::Time(); +MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::msg::Marker& marker) { + msg_.reset(new visualization_msgs::msg::Marker(marker)); + msg_->header.stamp = rclcpp::Time(RCL_ROS_TIME); msg_->frame_locked = false; } -MarkerVisualization::MarkerVisualization(const std::vector& markers, +MarkerVisualization::MarkerVisualization(const std::vector& markers, const planning_scene::PlanningScene& end_scene) { planning_frame_ = end_scene.getPlanningFrame(); // remember marker message, postpone rviz::MarkerBase creation until later for (const auto& marker : markers) { if (!end_scene.knowsFrameTransform(marker.header.frame_id)) { - ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'", marker.header.frame_id.c_str(), - marker.ns.c_str()); + RCLCPP_WARN_ONCE(LOGGER, "unknown frame '%s' for solution marker in namespace '%s'", + marker.header.frame_id.c_str(), marker.ns.c_str()); continue; // ignore markers with unknown frame } @@ -62,7 +67,7 @@ void MarkerVisualization::setVisible(const QString& ns, Ogre::SceneNode* parent_ setVisibility(it->second.ns_node_, parent_scene_node, visible); } -bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* parent_scene_node) { +bool MarkerVisualization::createMarkers(rviz_common::DisplayContext* context, Ogre::SceneNode* parent_scene_node) { if (markers_created_) return true; // already called before @@ -72,25 +77,18 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce Ogre::Vector3 pos; try { -#ifdef RVIZ_TF1 - tf::TransformListener* tf = context->getFrameManager()->getTFClient(); - tf::StampedTransform tm; - tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm); - auto q = tm.getRotation(); - auto p = tm.getOrigin(); - quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); - pos = Ogre::Vector3(p.x(), p.y(), p.z()); -#else - std::shared_ptr tf = context->getFrameManager()->getTF2BufferPtr(); - geometry_msgs::TransformStamped tm; - tm = tf->lookupTransform(planning_frame_, fixed_frame, ros::Time()); - auto q = tm.transform.rotation; - auto p = tm.transform.translation; - quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z); - pos = Ogre::Vector3(p.x, p.y, p.z); -#endif + auto tf_wrapper = std::dynamic_pointer_cast( + context->getFrameManager()->getConnector().lock()); + if (tf_wrapper) { + geometry_msgs::msg::TransformStamped tm; + tm = tf_wrapper->lookupTransform(planning_frame_, fixed_frame, tf2::TimePointZero); + auto q = tm.transform.rotation; + auto p = tm.transform.translation; + quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z); + pos = Ogre::Vector3(p.x, p.y, p.z); + } } catch (const tf2::TransformException& e) { - ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what()); + RCLCPP_WARN_STREAM(LOGGER, e.what()); return false; } @@ -110,7 +108,9 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce frame_it->second = node->createChildSceneNode(); node = frame_it->second; - data.marker_.reset(rviz::createMarker(data.msg_->type, nullptr, context, node)); + rviz_default_plugins::displays::markers::MarkerFactory marker_factory; + marker_factory.initialize(nullptr, context, node); + data.marker_ = marker_factory.createMarkerForType(data.msg_->type); if (!data.marker_) continue; // failed to create marker @@ -136,7 +136,7 @@ void MarkerVisualization::update(MarkerData& data, const planning_scene::Plannin const moveit::core::RobotState& robot_state) const { Q_ASSERT(scene.getPlanningFrame() == planning_frame_); - const visualization_msgs::Marker& marker = *data.msg_; + const visualization_msgs::msg::Marker& marker = *data.msg_; if (marker.header.frame_id == scene.getPlanningFrame()) return; // no need to transform nodes placed at planning frame @@ -147,8 +147,8 @@ void MarkerVisualization::update(MarkerData& data, const planning_scene::Plannin else if (scene.knowsFrameTransform(marker.header.frame_id)) pose = scene.getFrameTransform(marker.header.frame_id); else { - ROS_WARN_ONCE_NAMED("MarkerVisualization", "unknown frame '%s' for solution marker in namespace '%s'", - marker.header.frame_id.c_str(), marker.ns.c_str()); + RCLCPP_WARN_ONCE(LOGGER, "unknown frame '%s' for solution marker in namespace '%s'", + marker.header.frame_id.c_str(), marker.ns.c_str()); return; // ignore markers with unknown frame } @@ -169,11 +169,11 @@ void MarkerVisualization::update(const planning_scene::PlanningScene& end_scene, update(data, end_scene, robot_state); } -MarkerVisualizationProperty::MarkerVisualizationProperty(const QString& name, rviz::Property* parent) - : rviz::BoolProperty(name, true, "Enable/disable markers", parent) { - all_markers_at_once_ = - new rviz::BoolProperty("All at once?", false, "Show all markers of multiple subsolutions at once?", this, - SLOT(onAllAtOnceChanged()), this); +MarkerVisualizationProperty::MarkerVisualizationProperty(const QString& name, rviz_common::properties::Property* parent) + : rviz_common::properties::BoolProperty(name, true, "Enable/disable markers", parent) { + all_markers_at_once_ = new rviz_common::properties::BoolProperty( + "All at once?", false, "Show all markers of multiple subsolutions at once?", this, SLOT(onAllAtOnceChanged()), + this); connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); } @@ -183,7 +183,7 @@ MarkerVisualizationProperty::~MarkerVisualizationProperty() { marker_scene_node_->getCreator()->destroySceneNode(marker_scene_node_); } -void MarkerVisualizationProperty::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) { +void MarkerVisualizationProperty::onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context) { context_ = context; parent_scene_node_ = scene_node; marker_scene_node_ = parent_scene_node_->createChildSceneNode(); @@ -212,8 +212,8 @@ void MarkerVisualizationProperty::addMarkers(const MarkerVisualizationPtr& marke // create sub property for newly encountered namespace, enabling visibility by default auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first; if (ns_it->second == nullptr) { - ns_it->second = new rviz::BoolProperty(ns, true, "Show/hide markers of this namespace", this, - SLOT(onNSEnableChanged()), this); + ns_it->second = new rviz_common::properties::BoolProperty(ns, true, "Show/hide markers of this namespace", + this, SLOT(onNSEnableChanged()), this); } Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers() @@ -241,7 +241,7 @@ void MarkerVisualizationProperty::onEnableChanged() { } void MarkerVisualizationProperty::onNSEnableChanged() { - rviz::BoolProperty* ns_property = static_cast(sender()); + rviz_common::properties::BoolProperty* ns_property = static_cast(sender()); const QString& ns = ns_property->getName(); bool visible = ns_property->getBool(); // for all hosted markers, set visibility of given namespace diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 1c666a52c..d5af657ad 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -47,111 +47,122 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include +#include + +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_task_constructor_visualization.task_solution_visualization"); namespace moveit_rviz_plugin { -TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display) +TaskSolutionVisualization::TaskSolutionVisualization(rviz_common::properties::Property* parent, + rviz_common::Display* display) : display_(display) { // trajectory properties - interrupt_display_property_ = new rviz::BoolProperty("Interrupt Display", false, - "Immediately show newly planned trajectory, " - "interrupting the currently displayed one.", - parent); + interrupt_display_property_ = new rviz_common::properties::BoolProperty("Interrupt Display", false, + "Immediately show newly planned trajectory, " + "interrupting the currently displayed one.", + parent); - loop_display_property_ = new rviz::BoolProperty( + loop_display_property_ = new rviz_common::properties::BoolProperty( "Loop Animation", false, "Indicates whether the last received path is to be animated in a loop", parent, SLOT(changedLoopDisplay()), this); - trail_display_property_ = - new rviz::BoolProperty("Show Trail", false, "Show a path trail", parent, SLOT(changedTrail()), this); + trail_display_property_ = new rviz_common::properties::BoolProperty("Show Trail", false, "Show a path trail", parent, + SLOT(changedTrail()), this); state_display_time_property_ = - new rviz::EditableEnumProperty("State Display Time", "0.05 s", - "The amount of wall-time to wait in between displaying " - "states along a received trajectory path", - parent); + new rviz_common::properties::EditableEnumProperty("State Display Time", "0.05 s", + "The amount of wall-time to wait in between displaying " + "states along a received trajectory path", + parent); state_display_time_property_->addOptionStd("REALTIME"); state_display_time_property_->addOptionStd("0.05 s"); state_display_time_property_->addOptionStd("0.1 s"); state_display_time_property_->addOptionStd("0.5 s"); - trail_step_size_property_ = new rviz::IntProperty( + trail_step_size_property_ = new rviz_common::properties::IntProperty( "Trail Step Size", 1, "Specifies the step size of the samples shown in the trajectory trail.", parent, SLOT(changedTrail()), this); trail_step_size_property_->setMin(1); // robot properties - robot_property_ = new rviz::Property("Robot", QString(), QString(), parent); - robot_visual_enabled_property_ = new rviz::BoolProperty("Show Robot Visual", true, - "Indicates whether the geometry of the robot as defined for " - "visualisation purposes should be displayed", - robot_property_, SLOT(changedRobotVisualEnabled()), this); + robot_property_ = new rviz_common::properties::Property("Robot", QString(), QString(), parent); + robot_visual_enabled_property_ = + new rviz_common::properties::BoolProperty("Show Robot Visual", true, + "Indicates whether the geometry of the robot as defined for " + "visualisation purposes should be displayed", + robot_property_, SLOT(changedRobotVisualEnabled()), this); robot_collision_enabled_property_ = - new rviz::BoolProperty("Show Robot Collision", false, - "Indicates whether the geometry of the robot as defined " - "for collision detection purposes should be displayed", - robot_property_, SLOT(changedRobotCollisionEnabled()), this); - - robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", - robot_property_, SLOT(changedRobotAlpha()), this); + new rviz_common::properties::BoolProperty("Show Robot Collision", false, + "Indicates whether the geometry of the robot as defined " + "for collision detection purposes should be displayed", + robot_property_, SLOT(changedRobotCollisionEnabled()), this); + + robot_alpha_property_ = + new rviz_common::properties::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", + robot_property_, SLOT(changedRobotAlpha()), this); robot_alpha_property_->setMin(0.0); robot_alpha_property_->setMax(1.0); - robot_color_property_ = - new rviz::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), "The color of the animated robot", - robot_property_, SLOT(changedRobotColor()), this); + robot_color_property_ = new rviz_common::properties::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), + "The color of the animated robot", + robot_property_, SLOT(changedRobotColor()), this); - enable_robot_color_property_ = new rviz::BoolProperty("Use Fixed Robot Color", false, - "Specifies whether the fixed robot color should be used." - " If not, the original color is used.", - robot_property_, SLOT(enabledRobotColor()), this); + enable_robot_color_property_ = + new rviz_common::properties::BoolProperty("Use Fixed Robot Color", false, + "Specifies whether the fixed robot color should be used." + " If not, the original color is used.", + robot_property_, SLOT(enabledRobotColor()), this); // planning scene properties - scene_enabled_property_ = - new rviz::BoolProperty("Scene", true, "Show Planning Scene", parent, SLOT(changedSceneEnabled()), this); + scene_enabled_property_ = new rviz_common::properties::BoolProperty("Scene", true, "Show Planning Scene", parent, + SLOT(changedSceneEnabled()), this); - scene_alpha_property_ = new rviz::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry", - scene_enabled_property_, SLOT(renderCurrentScene()), this); + scene_alpha_property_ = + new rviz_common::properties::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry", + scene_enabled_property_, SLOT(renderCurrentScene()), this); scene_alpha_property_->setMin(0.0); scene_alpha_property_->setMax(1.0); - scene_color_property_ = new rviz::ColorProperty( + scene_color_property_ = new rviz_common::properties::ColorProperty( "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)", scene_enabled_property_, SLOT(renderCurrentScene()), this); - attached_body_color_property_ = - new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", - scene_enabled_property_, SLOT(changedAttachedBodyColor()), this); + attached_body_color_property_ = new rviz_common::properties::ColorProperty( + "Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", scene_enabled_property_, + SLOT(changedAttachedBodyColor()), this); - octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", - scene_enabled_property_, SLOT(renderCurrentScene()), this); + octree_render_property_ = + new rviz_common::properties::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", + scene_enabled_property_, SLOT(renderCurrentScene()), this); octree_render_property_->addOption("Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS); octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS); octree_render_property_->addOption("All Voxels", OCTOMAP_FREE_VOXELS | OCTOMAP_OCCUPIED_VOXELS); - octree_coloring_property_ = new rviz::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode", - scene_enabled_property_, SLOT(renderCurrentScene()), this); + octree_coloring_property_ = + new rviz_common::properties::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode", + scene_enabled_property_, SLOT(renderCurrentScene()), this); octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR); octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); @@ -174,7 +185,7 @@ TaskSolutionVisualization::~TaskSolutionVisualization() { main_scene_node_->getCreator()->destroySceneNode(main_scene_node_); } -void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) { +void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context) { // Save pointers for later use parent_scene_node_ = scene_node; context_ = context; @@ -194,7 +205,7 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz:: marker_visual_->onInitialize(main_scene_node_, context_); - rviz::WindowManagerInterface* window_context = context_->getWindowManager(); + rviz_common::WindowManagerInterface* window_context = context_->getWindowManager(); if (window_context) { slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow()); slider_dock_panel_ = window_context->addPane(display_->getName() + " - Slider", slider_panel_); @@ -209,10 +220,10 @@ void TaskSolutionVisualization::setName(const QString& name) { slider_dock_panel_->setWindowTitle(name + " - Slider"); } -void TaskSolutionVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) { +void TaskSolutionVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) { // Error check if (!robot_model) { - ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found"); + RCLCPP_ERROR(LOGGER, "No robot model found"); return; } @@ -268,8 +279,8 @@ void TaskSolutionVisualization::changedTrail() { trail_.resize(t->getWayPointCount() / stepsize); for (std::size_t i = 0; i < trail_.size(); i++) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point - rviz::Robot* r = - new rviz::Robot(trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), nullptr); + rviz_default_plugins::robot::Robot* r = new rviz_default_plugins::robot::Robot( + trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), nullptr); r->load(*scene_->getRobotModel()->getURDF()); r->setVisualVisible(robot_visual_enabled_property_->getBool()); r->setCollisionVisible(robot_collision_enabled_property_->getBool()); @@ -486,7 +497,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) } QColor attached_color = attached_body_color_property_->getColor(); - std_msgs::ColorRGBA color; + std_msgs::msg::ColorRGBA color; color.r = attached_color.redF(); color.g = attached_color.greenF(); color.b = attached_color.blueF(); @@ -506,16 +517,16 @@ void TaskSolutionVisualization::renderPlanningScene(const planning_scene::Planni return; QColor color = scene_color_property_->getColor(); - rviz::Color env_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF()); color = attached_body_color_property_->getColor(); - rviz::Color attached_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF()); scene_render_->renderPlanningScene( scene, env_color, attached_color, static_cast(octree_render_property_->getOptionInt()), static_cast(octree_coloring_property_->getOptionInt()), scene_alpha_property_->getFloat()); } -void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::Solution& msg) { +void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::msg::Solution& msg) { DisplaySolutionPtr s(new DisplaySolution); s->setFromMessage(scene_, msg); showTrajectory(s, false); @@ -565,12 +576,12 @@ void TaskSolutionVisualization::changedAttachedBodyColor() { renderCurrentWayPoint(); } -void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot) { +void TaskSolutionVisualization::unsetRobotColor(rviz_default_plugins::robot::Robot* robot) { for (auto& link : robot->getLinks()) link.second->unsetColor(); } -void TaskSolutionVisualization::setRobotColor(rviz::Robot* robot, const QColor& color) { +void TaskSolutionVisualization::setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color) { for (auto& link : robot->getLinks()) link.second->setColor(color.redF(), color.greenF(), color.blueF()); } From a90eac5a78c0734e0a16f6b96d754c7e89de5e70 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 17:00:12 +0300 Subject: [PATCH 7/8] Port demo to ROS2 --- demo/CMakeLists.txt | 68 ++-- demo/README.md | 2 +- demo/config/mtc.rviz | 296 +++++++++++++----- demo/config/panda_config.yaml | 94 +++--- .../pick_place_task.h | 24 +- demo/launch/alternative_path_costs.launch.py | 73 +++++ demo/launch/cartesian.launch.py | 55 ++++ demo/launch/demo.launch | 13 - demo/launch/demo.launch.py | 188 +++++++++++ demo/launch/ik_clearance_cost.launch.py | 55 ++++ demo/launch/modular.launch.py | 55 ++++ demo/launch/pickplace.launch | 8 - demo/launch/pickplace.launch.py | 78 +++++ demo/package.xml | 17 +- demo/src/alternative_path_costs.cpp | 15 +- demo/src/cartesian.cpp | 34 +- demo/src/ik_clearance_cost.cpp | 29 +- demo/src/modular.cpp | 23 +- demo/src/pick_place_demo.cpp | 37 +-- demo/src/pick_place_task.cpp | 141 ++++----- 20 files changed, 973 insertions(+), 332 deletions(-) create mode 100644 demo/launch/alternative_path_costs.launch.py create mode 100644 demo/launch/cartesian.launch.py delete mode 100644 demo/launch/demo.launch create mode 100644 demo/launch/demo.launch.py create mode 100644 demo/launch/ik_clearance_cost.launch.py create mode 100644 demo/launch/modular.launch.py delete mode 100644 demo/launch/pickplace.launch create mode 100644 demo/launch/pickplace.launch.py diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 4634461ef..442dd22b6 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,48 +1,51 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_demo) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -find_package(catkin REQUIRED COMPONENTS - roscpp - moveit_core - moveit_task_constructor_core - moveit_ros_planning_interface - rosparam_shortcuts -) +find_package(ament_cmake REQUIRED) -catkin_package( - CATKIN_DEPENDS roscpp -) +find_package(rclcpp REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(moveit_task_constructor_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(rosparam_shortcuts REQUIRED) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + moveit_core + moveit_task_constructor_core + moveit_task_constructor_msgs + moveit_ros_planning + moveit_ros_planning_interface + tf2_eigen + rosparam_shortcuts) -add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp) -target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME}_pick_place_task ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_library(${PROJECT_NAME}_pick_place_task SHARED src/pick_place_task.cpp) +ament_target_dependencies(${PROJECT_NAME}_pick_place_task ${THIS_PACKAGE_INCLUDE_DEPENDS}) install(TARGETS ${PROJECT_NAME}_pick_place_task - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +include_directories(include) # declare a demo consisting of a single cpp file function(demo name) add_executable(${PROJECT_NAME}_${name} src/${name}.cpp) - add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES}) + ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS}) set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "") install(TARGETS ${PROJECT_NAME}_${name} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) endfunction() @@ -55,7 +58,10 @@ demo(pick_place_demo) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -add_subdirectory(test) +# TODO: Port tests +# add_subdirectory(test) + +ament_package() diff --git a/demo/README.md b/demo/README.md index f0dd6421d..c50f36453 100644 --- a/demo/README.md +++ b/demo/README.md @@ -8,4 +8,4 @@ Developed by Henning Kayser & Simon Goldstein at [PickNik Consulting](http://pic Run demo - roslaunch moveit_task_constructor_demo demo.launch + ros2 launch moveit_task_constructor_demo demo.launch.py diff --git a/demo/config/mtc.rviz b/demo/config/mtc.rviz index 340ef2cf2..3ce33d8eb 100644 --- a/demo/config/mtc.rviz +++ b/demo/config/mtc.rviz @@ -1,15 +1,24 @@ Panels: - - Class: rviz/Displays - Help Height: 84 + - Class: rviz_common/Displays + Help Height: 78 Name: Displays Property Tree Widget: Expanded: + - /Global Options1 + - /Status1 - /Motion Planning Tasks1 - Splitter Ratio: 0.5393258333206177 - Tree Height: 533 - - Class: rviz/Help - Name: Help - - Class: rviz/Views + - /PlanningScene1 + Splitter Ratio: 0.5 + Tree Height: 439 + - 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 Name: Views @@ -22,32 +31,26 @@ Panels: Name: Motion Planning Tasks Tasks View: property_splitter: - - 540 - - 0 + - 197 + - 121 solution_sorting: - column: 1 - order: 0 + column: 0 + order: 1 solutions_splitter: - - 306 - - 98 - solutions_view_columns: - - 38 - - 52 - - 0 + - 397 + - 198 + solutions_view_columns: ~ tasks_view_columns: - - 226 - - 38 - - 38 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 + - 295 + - 26 + - 26 + - 48 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -63,31 +66,6 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - 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 - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Class: moveit_task_constructor/Motion Planning Tasks Enabled: true Interrupt Display: false @@ -104,7 +82,66 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - Robot Alpha: 1 + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 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 + Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true Use Fixed Robot Color: false @@ -119,62 +156,175 @@ Visualization Manager: Voxel Rendering: Occupied Voxels Show Trail: false State Display Time: 0.05 s - Task Solution Topic: /mtc_tutorial/solution + Task Solution Topic: /solution Tasks: - Cartesian Path: 1 + Cartesian Path: 0 Trail Step Size: 1 Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + 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_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 + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: panda_link0 Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select + - 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 + 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/XYOrbit - Distance: 1.3878222703933716 + Class: rviz_default_plugins/Orbit + Distance: 3.8008623123168945 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0.30880630016326904 - Y: -0.1259305477142334 - Z: 1.3560062370743253e-6 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.264797568321228 - Target Frame: panda_link0 - Yaw: 4.939944744110107 + Pitch: 0.645397961139679 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7453980445861816 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 768 - Help: - collapsed: false + Height: 1043 Hide Left Dock: false Hide Right Dock: false Motion Planning Tasks: collapsed: false Motion Planning Tasks - Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false Views: collapsed: false - Width: 1621 - X: 249 - Y: 25 + Width: 1920 + X: 0 + Y: 0 diff --git a/demo/config/panda_config.yaml b/demo/config/panda_config.yaml index 090cc6d12..1c2b69bbe 100644 --- a/demo/config/panda_config.yaml +++ b/demo/config/panda_config.yaml @@ -1,46 +1,48 @@ -# Total planning attempts -max_solutions: 10 - -# Planning group and link names -arm_group_name: "panda_arm" -eef_name: "hand" -hand_group_name: "hand" -hand_frame: "panda_link8" - -# Poses -hand_open_pose: "open" -hand_close_pose: "close" -arm_home_pose: "ready" - -# Scene frames -world_frame: "world" -table_reference_frame: "world" -object_reference_frame: "world" -surface_link: "table" - -# Collision object for picking -# CYLINDER object specifications -object_name: "object" -object_dimensions: [0.25, 0.02] # [height, radius] -object_pose: [0.5, -0.25, 0.0, 0, 0, 0] - -# Table model -spawn_table: true -table_name: "table" -table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] -table_pose: [0.5, -0.25, 0, 0, 0, 0] - -# Gripper grasp frame transform [x,y,z,r,p,y] -grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] - -# Place pose [x,y,z,r,p,y] -place_pose: [0.6, -0.15, 0, 0, 0, 0] -place_surface_offset: 0.0001 # place offset from table - -# Valid distance range when approaching an object for picking -approach_object_min_dist: 0.1 -approach_object_max_dist: 0.15 - -# Valid height range when lifting an object after pick -lift_object_min_dist: 0.01 -lift_object_max_dist: 0.1 +/**: + ros__parameters: + # Total planning attempts + max_solutions: 10 + + # Planning group and link names + arm_group_name: "panda_arm" + eef_name: "hand" + hand_group_name: "hand" + hand_frame: "panda_link8" + + # Poses + hand_open_pose: "open" + hand_close_pose: "close" + arm_home_pose: "ready" + + # Scene frames + world_frame: "world" + table_reference_frame: "world" + object_reference_frame: "world" + surface_link: "table" + + # Collision object for picking + # CYLINDER object specifications + object_name: "object" + object_dimensions: [0.25, 0.02] # [height, radius] + object_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0] + + # Table model + spawn_table: true + table_name: "table" + table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] + table_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0] + + # Gripper grasp frame transform [x,y,z,r,p,y] + grasp_frame_transform: [0.0, 0.0, 0.1, 1.571, 0.785, 1.571] + + # Place pose [x,y,z,r,p,y] + place_pose: [0.6, -0.15, 0.0, 0.0, 0.0, 0.0] + place_surface_offset: 0.0001 # place offset from table + + # Valid distance range when approaching an object for picking + approach_object_min_dist: 0.1 + approach_object_max_dist: 0.15 + + # Valid height range when lifting an object after pick + lift_object_min_dist: 0.01 + lift_object_max_dist: 0.1 diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index 5301fb6dd..cbb452270 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -31,11 +31,11 @@ *********************************************************************/ /* Author: Henning Kayser, Simon Goldstein - Desc: A demo to show MoveIt Task Constructor in action + Desc: A demo to show MoveIt Task Constructor in action */ // ROS -#include +#include // MoveIt #include @@ -56,22 +56,26 @@ #include #include #include -#include +#include -#include +#if __has_include() +#include +#else +#include +#endif #pragma once namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; -// prepare a demo environment from ROS parameters under pnh -void setupDemoScene(ros::NodeHandle& pnh); +// prepare a demo environment from ROS parameters under node +void setupDemoScene(const rclcpp::Node::SharedPtr& node); class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh); + PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& pnh); ~PickPlaceTask() = default; bool init(); @@ -83,9 +87,7 @@ class PickPlaceTask private: void loadParameters(); - static constexpr char LOGNAME[]{ "pick_place_task" }; - - ros::NodeHandle pnh_; + rclcpp::Node::SharedPtr node_; std::string task_name_; moveit::task_constructor::TaskPtr task_; @@ -117,7 +119,7 @@ class PickPlaceTask double lift_object_max_dist_; // Place metrics - geometry_msgs::Pose place_pose_; + geometry_msgs::msg::Pose place_pose_; double place_surface_offset_; }; } // namespace moveit_task_constructor_demo diff --git a/demo/launch/alternative_path_costs.launch.py b/demo/launch/alternative_path_costs.launch.py new file mode 100644 index 000000000..e72e8ceb9 --- /dev/null +++ b/demo/launch/alternative_path_costs.launch.py @@ -0,0 +1,73 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # Get URDF and SRDF + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + cartesian_task = Node( + package="moveit_task_constructor_demo", + executable="alternative_path_costs", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + ], + ) + + return LaunchDescription([cartesian_task]) diff --git a/demo/launch/cartesian.launch.py b/demo/launch/cartesian.launch.py new file mode 100644 index 000000000..8773ece61 --- /dev/null +++ b/demo/launch/cartesian.launch.py @@ -0,0 +1,55 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # Get URDF and SRDF + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + cartesian_task = Node( + package="moveit_task_constructor_demo", + executable="cartesian", + output="screen", + parameters=[robot_description, robot_description_semantic, kinematics_yaml], + ) + + return LaunchDescription([cartesian_task]) diff --git a/demo/launch/demo.launch b/demo/launch/demo.launch deleted file mode 100644 index fec17bd07..000000000 --- a/demo/launch/demo.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/demo/launch/demo.launch.py b/demo/launch/demo.launch.py new file mode 100644 index 000000000..2756703a9 --- /dev/null +++ b/demo/launch/demo.launch.py @@ -0,0 +1,188 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import xacro + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # planning_context + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation + move_group_capabilities = { + "capabilities": "move_group/ExecuteTaskSolutionCapability" + } + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + move_group_capabilities, + ], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_task_constructor_demo") + "/config/mtc.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda_ros_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + # Load controllers + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + rviz_node, + static_tf, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/demo/launch/ik_clearance_cost.launch.py b/demo/launch/ik_clearance_cost.launch.py new file mode 100644 index 000000000..e97f2a8ba --- /dev/null +++ b/demo/launch/ik_clearance_cost.launch.py @@ -0,0 +1,55 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # Get URDF and SRDF + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + cartesian_task = Node( + package="moveit_task_constructor_demo", + executable="ik_clearance_cost", + output="screen", + parameters=[robot_description, robot_description_semantic, kinematics_yaml], + ) + + return LaunchDescription([cartesian_task]) diff --git a/demo/launch/modular.launch.py b/demo/launch/modular.launch.py new file mode 100644 index 000000000..673dfe97e --- /dev/null +++ b/demo/launch/modular.launch.py @@ -0,0 +1,55 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # Get URDF and SRDF + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + modular_task = Node( + package="moveit_task_constructor_demo", + executable="modular", + output="screen", + parameters=[robot_description, robot_description_semantic, kinematics_yaml], + ) + + return LaunchDescription([modular_task]) diff --git a/demo/launch/pickplace.launch b/demo/launch/pickplace.launch deleted file mode 100644 index 72c44c5a7..000000000 --- a/demo/launch/pickplace.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/demo/launch/pickplace.launch.py b/demo/launch/pickplace.launch.py new file mode 100644 index 000000000..ebf16e91a --- /dev/null +++ b/demo/launch/pickplace.launch.py @@ -0,0 +1,78 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # Get URDF and SRDF + robot_description_config = load_file( + "moveit_resources_panda_description", "urdf/panda.urdf" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "moveit_resources_panda_moveit_config", "config/panda.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + ) + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + pick_place_demo = Node( + package="moveit_task_constructor_demo", + executable="pick_place_demo", + output="screen", + parameters=[ + os.path.join( + get_package_share_directory("moveit_task_constructor_demo"), + "config", + "panda_config.yaml", + ), + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + ], + ) + + return LaunchDescription([pick_place_demo]) diff --git a/demo/package.xml b/demo/package.xml index c4ccda3dc..e0578b942 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -10,18 +10,15 @@ BSD - catkin - - roscpp - rosparam_shortcuts - - moveit_core - moveit_ros_planning_interface - + ament_cmake moveit_task_constructor_core - + moveit_ros_planning_interface + moveit_core + rosparam_shortcuts moveit_task_constructor_capabilities moveit_resources_panda_moveit_config - rostest + + ament_cmake + diff --git a/demo/src/alternative_path_costs.cpp b/demo/src/alternative_path_costs.cpp index 1ec77b524..f9c302312 100644 --- a/demo/src/alternative_path_costs.cpp +++ b/demo/src/alternative_path_costs.cpp @@ -1,4 +1,4 @@ -#include +#include #include @@ -16,11 +16,15 @@ using namespace moveit::task_constructor; /* FixedState - Connect - FixedState */ int main(int argc, char** argv) { - ros::init(argc, argv, "mtc_tutorial"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("mtc_tutorial", node_options); + std::thread spinning_thread([node] { rclcpp::spin(node); }); Task t; t.stages()->setName("alternative path costs"); - t.loadRobotModel(); + t.loadRobotModel(node); assert(t.getRobotModel()->getName() == "panda"); @@ -33,7 +37,7 @@ int main(int argc, char** argv) { initial->setState(scene); t.add(std::move(initial)); - auto pipeline{ std::make_shared() }; + auto pipeline{ std::make_shared(node) }; auto alternatives{ std::make_unique("connect") }; { @@ -76,7 +80,8 @@ int main(int argc, char** argv) { std::cout << e << std::endl; } - ros::spin(); + // keep alive for interactive inspection in rviz + spinning_thread.join(); return 0; } diff --git a/demo/src/cartesian.cpp b/demo/src/cartesian.cpp index 5975e9689..bf7a1dbec 100644 --- a/demo/src/cartesian.cpp +++ b/demo/src/cartesian.cpp @@ -41,12 +41,12 @@ #include #include -#include +#include #include using namespace moveit::task_constructor; -Task createTask() { +Task createTask(const rclcpp::Node::SharedPtr& node) { Task t; t.stages()->setName("Cartesian Path"); @@ -59,7 +59,7 @@ Task createTask() { auto joint_interpolation = std::make_shared(); // start from a fixed robot state - t.loadRobotModel(); + t.loadRobotModel(node); auto scene = std::make_shared(t.getRobotModel()); { auto& state = scene->getCurrentStateNonConst(); @@ -73,7 +73,7 @@ Task createTask() { { auto stage = std::make_unique("x +0.2", cartesian_interpolation); stage->setGroup(group); - geometry_msgs::Vector3Stamped direction; + geometry_msgs::msg::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.x = 0.2; stage->setDirection(direction); @@ -83,7 +83,7 @@ Task createTask() { { auto stage = std::make_unique("y -0.3", cartesian_interpolation); stage->setGroup(group); - geometry_msgs::Vector3Stamped direction; + geometry_msgs::msg::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.y = -0.3; stage->setDirection(direction); @@ -93,7 +93,7 @@ Task createTask() { { // rotate about TCP auto stage = std::make_unique("rz +45°", cartesian_interpolation); stage->setGroup(group); - geometry_msgs::TwistStamped twist; + geometry_msgs::msg::TwistStamped twist; twist.header.frame_id = "world"; twist.twist.angular.z = M_PI / 4.; stage->setDirection(twist); @@ -108,16 +108,8 @@ Task createTask() { t.add(std::move(stage)); } - { // move gripper into predefined open state - auto stage = std::make_unique("open gripper", joint_interpolation); - stage->setGroup(eef); - stage->setGoal("open"); - t.add(std::move(stage)); - } - { // move from reached state back to the original state, using joint interpolation - // specifying two groups (arm and hand) will try to merge both trajectories - stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, joint_interpolation } }; + stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } }; auto connect = std::make_unique("connect", planners); t.add(std::move(connect)); } @@ -132,12 +124,11 @@ Task createTask() { } int main(int argc, char** argv) { - ros::init(argc, argv, "mtc_tutorial"); - // run an asynchronous spinner to communicate with the move_group node and rviz - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("mtc_tutorial"); + std::thread spinning_thread([node] { rclcpp::spin(node); }); - auto task = createTask(); + auto task = createTask(node); try { if (task.plan()) task.introspection().publishSolution(*task.solutions().front()); @@ -145,6 +136,7 @@ int main(int argc, char** argv) { std::cerr << "planning failed with exception" << std::endl << ex << task; } - ros::waitForShutdown(); // keep alive for interactive inspection in rviz + // keep alive for interactive inspection in rviz + spinning_thread.join(); return 0; } diff --git a/demo/src/ik_clearance_cost.cpp b/demo/src/ik_clearance_cost.cpp index 08c3077a5..9429368cc 100644 --- a/demo/src/ik_clearance_cost.cpp +++ b/demo/src/ik_clearance_cost.cpp @@ -1,4 +1,4 @@ -#include +#include #include @@ -8,24 +8,23 @@ #include #include +#include using namespace moveit::task_constructor; /* ComputeIK(FixedState) */ int main(int argc, char** argv) { - ros::init(argc, argv, "mtc_tutorial"); - ros::NodeHandle nh{ "~" }; - - ros::AsyncSpinner spinner{ 1 }; - spinner.start(); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo"); + std::thread spinning_thread([node] { rclcpp::spin(node); }); Task t; t.stages()->setName("clearance IK"); // announce new task (in case previous run was restarted) - ros::Duration(0.5).sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(500)); - t.loadRobotModel(); + t.loadRobotModel(node); assert(t.getRobotModel()->getName() == "panda"); auto scene = std::make_shared(t.getRobotModel()); @@ -33,10 +32,10 @@ int main(int argc, char** argv) { robot_state.setToDefaultValues(); robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended"); - moveit_msgs::CollisionObject co; + moveit_msgs::msg::CollisionObject co; co.id = "obstacle"; co.primitives.emplace_back(); - co.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + co.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; co.primitives[0].dimensions.resize(1); co.primitives[0].dimensions[0] = 0.1; co.header.frame_id = "world"; @@ -56,8 +55,10 @@ int main(int argc, char** argv) { ik->setMaxIKSolutions(100); auto cl_cost{ std::make_unique() }; - cl_cost->cumulative = nh.param("cumulative", false); // sum up pairwise distances? - cl_cost->with_world = nh.param("with_world", true); // consider distance to world objects? + cl_cost->cumulative = false; + rosparam_shortcuts::get(node, "cumulative", cl_cost->cumulative); // sum up pairwise distances? + cl_cost->with_world = true; + rosparam_shortcuts::get(node, "with_world", cl_cost->cumulative); // consider distance to world objects? ik->setCostTerm(std::move(cl_cost)); t.add(std::move(ik)); @@ -68,7 +69,7 @@ int main(int argc, char** argv) { std::cout << e << std::endl; } - ros::waitForShutdown(); - + // Keep introspection alive + spinning_thread.join(); return 0; } diff --git a/demo/src/modular.cpp b/demo/src/modular.cpp index aa2cad750..d841f2b96 100644 --- a/demo/src/modular.cpp +++ b/demo/src/modular.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include using namespace moveit::task_constructor; @@ -59,7 +59,7 @@ std::unique_ptr createModule(const std::string& group) { { auto stage = std::make_unique("x +0.2", cartesian); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - geometry_msgs::Vector3Stamped direction; + geometry_msgs::msg::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.x = 0.2; stage->setDirection(direction); @@ -69,7 +69,7 @@ std::unique_ptr createModule(const std::string& group) { { auto stage = std::make_unique("y -0.3", cartesian); stage->properties().configureInitFrom(Stage::PARENT); - geometry_msgs::Vector3Stamped direction; + geometry_msgs::msg::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.y = -0.3; stage->setDirection(direction); @@ -79,7 +79,7 @@ std::unique_ptr createModule(const std::string& group) { { // rotate about TCP auto stage = std::make_unique("rz +45°", cartesian); stage->properties().configureInitFrom(Stage::PARENT); - geometry_msgs::TwistStamped twist; + geometry_msgs::msg::TwistStamped twist; twist.header.frame_id = "world"; twist.twist.angular.z = M_PI / 4.; stage->setDirection(twist); @@ -95,8 +95,9 @@ std::unique_ptr createModule(const std::string& group) { return c; } -Task createTask() { +Task createTask(const rclcpp::Node::SharedPtr& node) { Task t; + t.loadRobotModel(node); t.stages()->setName("Reusable Containers"); t.add(std::make_unique("current")); @@ -111,12 +112,11 @@ Task createTask() { } int main(int argc, char** argv) { - ros::init(argc, argv, "mtc_tutorial"); - // run an asynchronous spinner to communicate with the move_group node and rviz - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("mtc_tutorial"); + std::thread spinning_thread([node] { rclcpp::spin(node); }); - auto task = createTask(); + auto task = createTask(node); try { if (task.plan()) task.introspection().publishSolution(*task.solutions().front()); @@ -124,6 +124,7 @@ int main(int argc, char** argv) { std::cerr << "planning failed with exception" << std::endl << ex << task; } - ros::waitForShutdown(); // keep alive for interactive inspection in rviz + // keep alive for interactive inspection in rviz + spinning_thread.join(); return 0; } diff --git a/demo/src/pick_place_demo.cpp b/demo/src/pick_place_demo.cpp index a14c425e5..14a560d8d 100644 --- a/demo/src/pick_place_demo.cpp +++ b/demo/src/pick_place_demo.cpp @@ -31,47 +31,48 @@ *********************************************************************/ /* Author: Henning Kayser, Simon Goldstein - Desc: A demo to show MoveIt Task Constructor in action + Desc: A demo to show MoveIt Task Constructor in action */ // ROS -#include +#include // MTC pick/place demo implementation #include -constexpr char LOGNAME[] = "moveit_task_constructor_demo"; +#include -int main(int argc, char** argv) { - ros::init(argc, argv, "mtc_tutorial"); - ros::NodeHandle nh, pnh("~"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); - // Handle Task introspection requests from RViz & feedback during execution - ros::AsyncSpinner spinner(1); - spinner.start(); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo", node_options); + std::thread spinning_thread([node] { rclcpp::spin(node); }); - moveit_task_constructor_demo::setupDemoScene(pnh); + moveit_task_constructor_demo::setupDemoScene(node); // Construct and run pick/place task - moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", node); if (!pick_place_task.init()) { - ROS_INFO_NAMED(LOGNAME, "Initialization failed"); + RCLCPP_INFO(LOGGER, "Initialization failed"); return 1; } if (pick_place_task.plan()) { - ROS_INFO_NAMED(LOGNAME, "Planning succeded"); - if (pnh.param("execute", false)) { + RCLCPP_INFO(LOGGER, "Planning succeded"); + if (bool execute = false; rosparam_shortcuts::get(node, "execute", execute) && execute) { pick_place_task.execute(); - ROS_INFO_NAMED(LOGNAME, "Execution complete"); + RCLCPP_INFO(LOGGER, "Execution complete"); } else { - ROS_INFO_NAMED(LOGNAME, "Execution disabled"); + RCLCPP_INFO(LOGGER, "Execution disabled"); } } else { - ROS_INFO_NAMED(LOGNAME, "Planning failed"); + RCLCPP_INFO(LOGGER, "Planning failed"); } // Keep introspection alive - ros::waitForShutdown(); + spinning_thread.join(); return 0; } diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 72b3cf690..43eee00cb 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -31,77 +31,77 @@ *********************************************************************/ /* Author: Henning Kayser, Simon Goldstein - Desc: A demo to show MoveIt Task Constructor in action + Desc: A demo to show MoveIt Task Constructor in action */ #include #include -namespace moveit_task_constructor_demo { +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); -constexpr char LOGNAME[] = "moveit_task_constructor_demo"; -constexpr char PickPlaceTask::LOGNAME[]; +namespace moveit_task_constructor_demo { -void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { +void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, + const moveit_msgs::msg::CollisionObject& object) { if (!psi.applyCollisionObject(object)) throw std::runtime_error("Failed to spawn object: " + object.id); } -moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) { +moveit_msgs::msg::CollisionObject createTable(const rclcpp::Node::SharedPtr& node) { std::string table_name, table_reference_frame; std::vector table_dimensions; - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; std::size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); + errors += !rosparam_shortcuts::get(node, "table_name", table_name); + errors += !rosparam_shortcuts::get(node, "table_reference_frame", table_reference_frame); + errors += !rosparam_shortcuts::get(node, "table_dimensions", table_dimensions); + errors += !rosparam_shortcuts::get(node, "table_pose", pose); + rosparam_shortcuts::shutdownIfError(errors); - moveit_msgs::CollisionObject object; + moveit_msgs::msg::CollisionObject object; object.id = table_name; object.header.frame_id = table_reference_frame; object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - object.primitives[0].dimensions = table_dimensions; + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + object.primitives[0].dimensions = { table_dimensions.at(0), table_dimensions.at(1), table_dimensions.at(2) }; pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world object.primitive_poses.push_back(pose); return object; } -moveit_msgs::CollisionObject createObject(ros::NodeHandle& pnh) { +moveit_msgs::msg::CollisionObject createObject(const rclcpp::Node::SharedPtr& node) { std::string object_name, object_reference_frame; std::vector object_dimensions; - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; std::size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); + error += !rosparam_shortcuts::get(node, "object_name", object_name); + error += !rosparam_shortcuts::get(node, "object_reference_frame", object_reference_frame); + error += !rosparam_shortcuts::get(node, "object_dimensions", object_dimensions); + error += !rosparam_shortcuts::get(node, "object_pose", pose); + rosparam_shortcuts::shutdownIfError(error); - moveit_msgs::CollisionObject object; + moveit_msgs::msg::CollisionObject object; object.id = object_name; object.header.frame_id = object_reference_frame; object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = object_dimensions; + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = { object_dimensions.at(0), object_dimensions.at(1) }; pose.position.z += 0.5 * object_dimensions[0]; object.primitive_poses.push_back(pose); return object; } -void setupDemoScene(ros::NodeHandle& pnh) { +void setupDemoScene(const rclcpp::Node::SharedPtr& node) { // Add table and object to planning scene - ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service + rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service moveit::planning_interface::PlanningSceneInterface psi; - if (pnh.param("spawn_table", true)) - spawnObject(psi, createTable(pnh)); - spawnObject(psi, createObject(pnh)); + if (bool spawn_table = true; rosparam_shortcuts::get(node, "spawn_table", spawn_table) && spawn_table) + spawnObject(psi, createTable(node)); + spawnObject(psi, createObject(node)); } -PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh) - : pnh_(pnh), task_name_(task_name) { +PickPlaceTask::PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& node) + : node_(node), task_name_(task_name) { loadParameters(); } @@ -111,41 +111,41 @@ void PickPlaceTask::loadParameters() { * Load Parameters * * * ***************************************************/ - ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); + RCLCPP_INFO(LOGGER, "Loading task parameters"); // Planning group properties size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_); + errors += !rosparam_shortcuts::get(node_, "arm_group_name", arm_group_name_); + errors += !rosparam_shortcuts::get(node_, "hand_group_name", hand_group_name_); + errors += !rosparam_shortcuts::get(node_, "eef_name", eef_name_); + errors += !rosparam_shortcuts::get(node_, "hand_frame", hand_frame_); + errors += !rosparam_shortcuts::get(node_, "world_frame", world_frame_); + errors += !rosparam_shortcuts::get(node_, "grasp_frame_transform", grasp_frame_transform_); // Predefined pose targets - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_); + errors += !rosparam_shortcuts::get(node_, "hand_open_pose", hand_open_pose_); + errors += !rosparam_shortcuts::get(node_, "hand_close_pose", hand_close_pose_); + errors += !rosparam_shortcuts::get(node_, "arm_home_pose", arm_home_pose_); // Target object - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_); + errors += !rosparam_shortcuts::get(node_, "object_name", object_name_); + errors += !rosparam_shortcuts::get(node_, "object_dimensions", object_dimensions_); + errors += !rosparam_shortcuts::get(node_, "object_reference_frame", object_reference_frame_); + errors += !rosparam_shortcuts::get(node_, "surface_link", surface_link_); support_surfaces_ = { surface_link_ }; // Pick/Place metrics - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); + errors += !rosparam_shortcuts::get(node_, "approach_object_min_dist", approach_object_min_dist_); + errors += !rosparam_shortcuts::get(node_, "approach_object_max_dist", approach_object_max_dist_); + errors += !rosparam_shortcuts::get(node_, "lift_object_min_dist", lift_object_min_dist_); + errors += !rosparam_shortcuts::get(node_, "lift_object_max_dist", lift_object_max_dist_); + errors += !rosparam_shortcuts::get(node_, "place_surface_offset", place_surface_offset_); + errors += !rosparam_shortcuts::get(node_, "place_pose", place_pose_); + rosparam_shortcuts::shutdownIfError(errors); } bool PickPlaceTask::init() { - ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); + RCLCPP_INFO(LOGGER, "Initializing task pipeline"); const std::string object = object_name_; // Reset ROS introspection before constructing the new object @@ -155,10 +155,10 @@ bool PickPlaceTask::init() { Task& t = *task_; t.stages()->setName(task_name_); - t.loadRobotModel(); + t.loadRobotModel(node_); // Sampling planner - auto sampling_planner = std::make_shared(); + auto sampling_planner = std::make_shared(node_); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); // Cartesian planner @@ -245,7 +245,7 @@ bool PickPlaceTask::init() { stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); // Set hand forward direction - geometry_msgs::Vector3Stamped vec; + geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = hand_frame_; vec.vector.z = 1.0; stage->setDirection(vec); @@ -326,7 +326,7 @@ bool PickPlaceTask::init() { stage->properties().set("marker_ns", "lift_object"); // Set upward direction - geometry_msgs::Vector3Stamped vec; + geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = world_frame_; vec.vector.z = 1.0; stage->setDirection(vec); @@ -380,7 +380,7 @@ bool PickPlaceTask::init() { stage->setMinMaxDistance(.03, .13); // Set downward direction - geometry_msgs::Vector3Stamped vec; + geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = world_frame_; vec.vector.z = -1.0; stage->setDirection(vec); @@ -398,7 +398,7 @@ bool PickPlaceTask::init() { stage->setObject(object); // Set target pose - geometry_msgs::PoseStamped p; + geometry_msgs::msg::PoseStamped p; p.header.frame_id = object_reference_frame_; p.pose = place_pose_; p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; @@ -451,7 +451,7 @@ bool PickPlaceTask::init() { stage->setMinMaxDistance(.12, .25); stage->setIKFrame(hand_frame_); stage->properties().set("marker_ns", "retreat"); - geometry_msgs::Vector3Stamped vec; + geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = hand_frame_; vec.vector.z = -1.0; stage->setDirection(vec); @@ -479,7 +479,7 @@ bool PickPlaceTask::init() { try { t.init(); } catch (InitStageException& e) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); + RCLCPP_ERROR_STREAM(LOGGER, "Initialization failed: " << e); return false; } @@ -487,27 +487,28 @@ bool PickPlaceTask::init() { } bool PickPlaceTask::plan() { - ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); - int max_solutions = pnh_.param("max_solutions", 10); + RCLCPP_INFO(LOGGER, "Start searching for task solutions"); + int max_solutions = 10; + rosparam_shortcuts::get(node_, "max_solutions", max_solutions); return task_->plan(max_solutions); } bool PickPlaceTask::execute() { - ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); - moveit_msgs::MoveItErrorCodes execute_result; + RCLCPP_INFO(LOGGER, "Executing solution trajectory"); + moveit_msgs::msg::MoveItErrorCodes execute_result; execute_result = task_->execute(*task_->solutions().front()); // // If you want to inspect the goal message, use this instead: - // actionlib::SimpleActionClient + // actionlib::SimpleActionClient // execute("execute_task_solution", true); execute.waitForServer(); - // moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + // moveit_task_constructor_msgs::msg::ExecuteTaskSolutionGoal execute_goal; // task_->solutions().front()->fillMessage(execute_goal.solution); // execute.sendGoalAndWait(execute_goal); // execute_result = execute.getResult()->error_code; - if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val); + if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val); return false; } From 71a604cda0f1d0a6e9de9b8b0eacd050fee1ae58 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 18:17:00 +0300 Subject: [PATCH 8/8] Add ROS1 to ROS2 migration guide --- doc/MIGRATION_GUIDE.md | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 doc/MIGRATION_GUIDE.md diff --git a/doc/MIGRATION_GUIDE.md b/doc/MIGRATION_GUIDE.md new file mode 100644 index 000000000..c208ee2fa --- /dev/null +++ b/doc/MIGRATION_GUIDE.md @@ -0,0 +1,6 @@ +# Migration Guide from ROS1 + +* Default C++ standard set to 17 +* CMake version set to 3.5 +* PipelinePlanner's constructor now have a node `rclcpp::Node::SharedPtr` as an argument to load the `PlanningPipeline`'s parameters +* `Task::loadRobotModel` have a node as a parameter and the user have to call loadRobotModel explicitly otherwise init will throw an exception