Port rviz_marker_tools to ROS2

This commit is contained in:
JafarAbdi 2021-11-23 16:59:13 +03:00
parent e095e501ec
commit c62e7938aa
4 changed files with 116 additions and 95 deletions

View File

@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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()

View File

@ -1,7 +1,7 @@
#pragma once
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/msg/marker.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <Eigen/Geometry>
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 <typename T>
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;

View File

@ -6,10 +6,18 @@
<license>BSD</license>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
<build_depend>eigen</build_depend>
<depend>visualization_msgs</depend>
<depend>geometry_msgs</depend>
<depend>roscpp</depend>
<depend>rviz</depend>
<depend>rclcpp</depend>
<depend>rviz2</depend>
<depend>tf2_eigen</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

View File

@ -1,13 +1,19 @@
#include <rviz_marker_tools/marker_creation.h>
#include <urdf_model/link.h>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
#endif
#include <rclcpp/logging.hpp>
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<const urdf::Sphere&>(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;
}