From c62e7938aa9f897a14808879d3d19d6ba935f4b6 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 23 Nov 2021 16:59:13 +0300 Subject: [PATCH] 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 d7afda22..19b67eaa 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 -) +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) -catkin_package( - LIBRARIES - ${PROJECT_NAME} - INCLUDE_DIRS - include - CATKIN_DEPENDS - geometry_msgs - visualization_msgs - roscpp - rviz -) - -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 f4e3e55d..5533690a 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 17b0dc40..c53924e0 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 8d68b2ff..7221899d 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; }