diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b45187..2292ca3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -231,12 +231,6 @@ ament_export_dependencies(geometry_msgs rviz_ogre_vendor ) ament_export_include_directories(include) -ament_export_libraries( - ${PROJECT_NAME} - ${PROJECT_NAME}_remote_control - ${PROJECT_NAME}_imarker_simple - ${PROJECT_NAME}_gui -) ########## ## TEST ## @@ -256,4 +250,28 @@ if (BUILD_TESTING) ) endif() +#################### +## Python binding ## +#################### + +find_package(Python3 REQUIRED COMPONENTS Interpreter Development) +find_package(pybind11_vendor REQUIRED) +find_package(pybind11 REQUIRED) +find_package(py_binding_tools REQUIRED) + +ament_python_install_package(rviz_visual_tools PACKAGE_DIR python/rviz_visual_tools) + +pybind11_add_module(pyrviz_visual_tools python/src/rviz_visual_tools.cpp) +target_link_libraries(pyrviz_visual_tools PRIVATE rviz_visual_tools py_binding_tools::py_binding_tools) +ament_target_dependencies(pyrviz_visual_tools pybind11) + +install(DIRECTORY python/include/ DESTINATION include/rviz_visual_tools) +install(TARGETS pyrviz_visual_tools + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/rviz_visual_tools) +install(PROGRAMS + python/examples/rviz_visual_tools_demo.py + DESTINATION lib/${PROJECT_NAME} +) +ament_export_dependencies(py_binding_tools) + ament_package() diff --git a/README.md b/README.md index 26c74d2..18f4e04 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,7 @@ To see random shapes generated in Rviz, first launch Rviz: Then start the RViz Visual Tools demo: - ros2 run rviz_visual_tools rviz_visual_tools_demo + ros2 run rviz_visual_tools rviz_visual_tools_demo # Or rviz_visual_tools_demo.py for the python version ## Code API diff --git a/package.xml b/package.xml index baa6449..71aa3fb 100644 --- a/package.xml +++ b/package.xml @@ -38,9 +38,11 @@ visualization_msgs std_msgs trajectory_msgs + py_binding_tools qtbase5-dev eigen + pybind11_vendor eigen libqt5-widgets diff --git a/python/examples/rviz_visual_tools_demo.py b/python/examples/rviz_visual_tools_demo.py new file mode 100755 index 0000000..f7a6495 --- /dev/null +++ b/python/examples/rviz_visual_tools_demo.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 + +import rviz_visual_tools as rvt +from rclpy.logging import get_logger +from geometry_msgs.msg import Point, Pose, Quaternion +from threading import Thread +import numpy as np +from copy import deepcopy +import rclcpp + +LOGGER = get_logger("rviz_visual_tools_demo") + +rclcpp.init() +node = rclcpp.Node("rviz_visual_tools_demo") +visual_tools = rvt.RvizVisualTools(node, "world", "/rviz_visual_tools") +visual_tools.load_marker_publisher(True) +visual_tools.delete_all_markers() +visual_tools.enable_batch_publishing(True) + +point = Point(x=0.0, y=0.0, z=0.0) + +step_x = 0.1 + +LOGGER.info("Displaying range of colors red->green") +visual_tools.publish_text( + Pose(), + "Shpere-Color-Range", + rvt.Colors.WHITE, + rvt.Scales.XXLARGE, + False, +) +for color in (rvt.Colors.BLACK, rvt.Colors.GREY, rvt.Colors.WHITE, rvt.Colors.RED): + visual_tools.publish_sphere(point, scale=rvt.Scales.XLARGE, color=color) + point.x += step_x +visual_tools.trigger() + + +LOGGER.info("Displaying Coordinate Axis") +visual_tools.publish_text( + Pose(position=Point(x=0.0, y=-0.2, z=0.0)), + "Coordinate-Axis", + rvt.Colors.WHITE, + rvt.Scales.XXLARGE, + False, +) +visual_tools.publish_axis(Pose(position=Point(x=0.0, y=-0.2, z=0.0))) +visual_tools.publish_axis( + Pose( + position=Point(x=0.25, y=-0.2, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=1.0, w=0.0), + ) +) +visual_tools.trigger() + +LOGGER.info("Displaying Arrows") +visual_tools.publish_text( + Pose(position=Point(x=0.0, y=-0.4, z=0.0)), + "Arrows", + rvt.Colors.WHITE, + rvt.Scales.XXLARGE, + False, +) +visual_tools.publish_x_arrow( + Pose( + position=Point(x=0.0, y=-0.4, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) +) +visual_tools.publish_y_arrow( + Pose( + position=Point(x=0.25, y=-0.4, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) +) +visual_tools.publish_z_arrow( + Pose( + position=Point(x=0.5, y=-0.4, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) +) +visual_tools.trigger() + +cuboid_max_size = 0.075 +cuboid_min_size = 0.01 +point1 = Point(x=0.0, y=-0.6, z=0.0) +LOGGER.info("Displaying Rectangular Cuboid") +visual_tools.publish_text( + Pose(position=point1), "Cuboid", rvt.Colors.WHITE, rvt.Scales.XXLARGE, False +) +for i in np.linspace(0, 1.0, 10): + point2 = deepcopy(point1) + point2.x += i * cuboid_max_size + cuboid_min_size + point2.y += i * cuboid_max_size + cuboid_min_size + point2.z += i * cuboid_max_size + cuboid_min_size + visual_tools.publish_cuboid(point1, point2, color=rvt.Colors.RAND) + point1.x += step_x +visual_tools.trigger() + +line_max_size = 0.075 +line_min_size = 0.01 +point1 = Point(x=0.0, y=-0.8, z=0.0) +LOGGER.info("Displaying Lines") +visual_tools.publish_text( + Pose(position=point1), "Line", rvt.Colors.WHITE, rvt.Scales.XXLARGE, False +) +for i in np.linspace(0, 1.0, 10): + point2 = deepcopy(point1) + point2.x += i * line_max_size + line_min_size + point2.y += i * line_max_size + line_min_size + point2.z += i * line_max_size + line_min_size + visual_tools.publish_line(point1, point2, color=rvt.Colors.RAND) + point1.x += step_x +visual_tools.trigger() + +LOGGER.info("Displaying Cylinder") +visual_tools.publish_cylinder( + Pose(position=Point(x=0.0, y=0.2, z=0.0)), color=rvt.Colors.RAND +) +visual_tools.trigger() + +LOGGER.info("Displaying Cone") +visual_tools.publish_cone( + Pose(position=Point(x=0.0, y=0.4, z=0.0)), + angle=np.pi, + color=rvt.Colors.RAND, + scale=0.1, +) +visual_tools.trigger() + +LOGGER.info("Displaying Planes") +plane_pose = Pose(position=Point(x=0.0, y=0.6, z=0.0)) +visual_tools.publish_xy_plane(plane_pose, color=rvt.Colors.RED, scale=0.1) +visual_tools.publish_xz_plane(plane_pose, color=rvt.Colors.GREEN, scale=0.1) +visual_tools.publish_yz_plane(plane_pose, color=rvt.Colors.BLUE, scale=0.1) +visual_tools.trigger() + +LOGGER.info("Displaying Labeled Coordinate Axis") +visual_tools.publish_axis_labeled(Pose(position=Point(x=0.0, y=0.8, z=0.0)), "MyAxis") +visual_tools.trigger() + +LOGGER.info("Displaying Path") +visual_tools.publish_path( + [Point(x=0.0, y=1.0, z=0.0), Point(x=0.2, y=1.0, z=0.0), Point(x=0.4, y=1.1, z=0.0)] +) +visual_tools.trigger() diff --git a/python/rviz_visual_tools/__init__.py b/python/rviz_visual_tools/__init__.py new file mode 100644 index 0000000..76e32fd --- /dev/null +++ b/python/rviz_visual_tools/__init__.py @@ -0,0 +1,6 @@ +from rviz_visual_tools.pyrviz_visual_tools import ( + Scales, + Colors, + EulerConvention, + RvizVisualTools, +) diff --git a/python/rviz_visual_tools/py.typed b/python/rviz_visual_tools/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/python/src/rviz_visual_tools.cpp b/python/src/rviz_visual_tools.cpp new file mode 100644 index 0000000..8334af5 --- /dev/null +++ b/python/src/rviz_visual_tools.cpp @@ -0,0 +1,186 @@ +#include +#include +#include +#include +#include + +namespace py = pybind11; +using py::literals::operator""_a; + +namespace rviz_visual_tools +{ +PYBIND11_MODULE(pyrviz_visual_tools, m) +{ + py::enum_(m, "Colors") + .value("BLACK", Colors::BLACK) + .value("BROWN", Colors::BROWN) + .value("BLUE", Colors::BLUE) + .value("CYAN", Colors::CYAN) + .value("GREY", Colors::GREY) + .value("DARK_GREY", Colors::DARK_GREY) + .value("GREEN", Colors::GREEN) + .value("LIME_GREEN", Colors::LIME_GREEN) + .value("MAGENTA", Colors::MAGENTA) + .value("ORANGE", Colors::ORANGE) + .value("PURPLE", Colors::PURPLE) + .value("RED", Colors::RED) + .value("PINK", Colors::PINK) + .value("WHITE", Colors::WHITE) + .value("YELLOW", Colors::YELLOW) + .value("TRANSLUCENT", Colors::TRANSLUCENT) + .value("TRANSLUCENT_LIGHT", Colors::TRANSLUCENT_LIGHT) + .value("TRANSLUCENT_DARK", Colors::TRANSLUCENT_DARK) + .value("RAND", Colors::RAND) + .value("CLEAR", Colors::CLEAR) + .value("DEFAULT", Colors::DEFAULT); + + py::enum_(m, "Scales") + .value("XXXXSMALL", Scales::XXXXSMALL) + .value("XXXSMALL", Scales::XXXSMALL) + .value("XXSMALL", Scales::XXSMALL) + .value("XSMALL", Scales::XSMALL) + .value("SMALL", Scales::SMALL) + .value("MEDIUM", Scales::MEDIUM) + .value("LARGE", Scales::LARGE) + .value("XLARGE", Scales::XLARGE) + .value("XXLARGE", Scales::XXLARGE) + .value("XXXLARGE", Scales::XXXLARGE) + .value("XXXXLARGE", Scales::XXXXLARGE); + + py::enum_(m, "EulerConvention") + .value("XYZ", EulerConvention::XYZ) + .value("ZYX", EulerConvention::ZYX) + .value("ZXZ", EulerConvention::ZXZ); + + py::class_(m, "RvizVisualTools") + .def(py::init([](const rclcpp::Node::SharedPtr& node, const std::string& base_frame, + const std::string& marker_topic) { + return RvizVisualTools(base_frame, marker_topic, node); + })) + .def("delete_marker", &RvizVisualTools::deleteMarker) + .def("delete_all_markers", py::overload_cast<>(&RvizVisualTools::deleteAllMarkers)) + .def("delete_all_markers", + py::overload_cast(&RvizVisualTools::deleteAllMarkers)) + .def("reset_marker_counts", &RvizVisualTools::resetMarkerCounts) + .def("load_rviz_markers", &RvizVisualTools::loadRvizMarkers) + .def("set_marker_topic", &RvizVisualTools::setMarkerTopic) + .def("load_marker_publisher", &RvizVisualTools::loadMarkerPub) + .def("wait_for_marker_subscriber", &RvizVisualTools::waitForMarkerSub) + .def("set_alpha", &RvizVisualTools::setAlpha) + .def("set_lifetime", &RvizVisualTools::setLifetime) + .def_static("get_random_color", &RvizVisualTools::getRandColor) + .def("get_color", &RvizVisualTools::getColor) + .def("get_color_scale", &RvizVisualTools::getColorScale) + .def("get_scale", &RvizVisualTools::getScale) + .def_property("base_frame", &RvizVisualTools::getBaseFrame, &RvizVisualTools::setBaseFrame) + .def_property("global_scale", &RvizVisualTools::getGlobalScale, + &RvizVisualTools::setGlobalScale) + .def("enable_batch_publishing", &RvizVisualTools::enableBatchPublishing, "enable"_a = true) + .def("enable_frame_locking", &RvizVisualTools::enableFrameLocking) + .def("trigger_every", &RvizVisualTools::triggerEvery) + .def("trigger", &RvizVisualTools::trigger) + .def("publish_markers", &RvizVisualTools::publishMarkers) + .def("publish_cone", + py::overload_cast( + &RvizVisualTools::publishCone), + "pose"_a, "angle"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_abcd_plane", &RvizVisualTools::publishABCDPlane, "A"_a, "B"_a, "C"_a, "D"_a, + "color"_a = Colors::TRANSLUCENT, "x_width"_a = 1.0, "y_width"_a = 1.0) + .def("publish_normal_and_distance_plane", &RvizVisualTools::publishNormalAndDistancePlane, + "normal"_a, "distance"_a, "color"_a = Colors::TRANSLUCENT, "x_width"_a = 1.0, + "y_width"_a = 1.0) + .def("publish_xy_plane", + py::overload_cast( + &RvizVisualTools::publishXYPlane), + "pose"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_xz_plane", + py::overload_cast( + &RvizVisualTools::publishXZPlane), + "pose"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_yz_plane", + py::overload_cast( + &RvizVisualTools::publishYZPlane), + "pose"_a, "color"_a = Colors::TRANSLUCENT, "scale"_a = 1.0) + .def("publish_sphere", + py::overload_cast(&RvizVisualTools::publishSphere), + "point"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "ns"_a = "Sphere", + "id"_a = 0) + .def("publish_sphere", + py::overload_cast(&RvizVisualTools::publishSphere), + "pose"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "ns"_a = "Sphere", + "id"_a = 0) + .def("publish_spheres", + py::overload_cast&, Colors, Scales, + const std::string&>(&RvizVisualTools::publishSpheres), + "points"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "ns"_a = "Spheres") + .def("publish_x_arrow", + py::overload_cast( + &RvizVisualTools::publishXArrow), + "pose"_a, "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_x_arrow", + py::overload_cast( + &RvizVisualTools::publishXArrow), + "pose_stamped"_a, "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_y_arrow", + py::overload_cast( + &RvizVisualTools::publishYArrow), + "pose"_a, "color"_a = Colors::GREEN, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_y_arrow", + py::overload_cast( + &RvizVisualTools::publishYArrow), + "pose_stamped"_a, "color"_a = Colors::GREEN, "scale"_a = Scales::MEDIUM, + "length"_a = 0.0) + .def("publish_z_arrow", + py::overload_cast( + &RvizVisualTools::publishZArrow), + "pose"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_z_arrow", + py::overload_cast( + &RvizVisualTools::publishZArrow), + "pose_stamped"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM, "length"_a = 0.0) + .def("publish_cuboid", + py::overload_cast( + &RvizVisualTools::publishCuboid), + "point1"_a, "point2"_a, "color"_a = Colors::BLUE, "ns"_a = "Cuboid", "id"_a = 0) + .def("publish_cuboid", + py::overload_cast( + &RvizVisualTools::publishCuboid), + "pose"_a, "depth"_a, "width"_a, "height"_a, "color"_a = Colors::BLUE) + .def("publish_line", + py::overload_cast(&RvizVisualTools::publishLine), + "point1"_a, "point2"_a, "color"_a = Colors::BLUE, "scale"_a = Scales::MEDIUM) + .def("publish_path", + py::overload_cast&, Colors, Scales, + const std::string&>(&RvizVisualTools::publishPath), + "path"_a, "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "ns"_a = "Path") + .def("publish_polygon", &RvizVisualTools::publishPolygon, "polygon"_a, + "color"_a = Colors::RED, "scale"_a = Scales::MEDIUM, "ns"_a = "Polygon") + .def("publish_axis_labeled", + py::overload_cast( + &RvizVisualTools::publishAxisLabeled), + "pose"_a, "label"_a, "scale"_a = Scales::MEDIUM, "color"_a = Colors::WHITE) + .def("publish_axis", + py::overload_cast( + &RvizVisualTools::publishAxis), + "pose"_a, "scale"_a = Scales::MEDIUM, "ns"_a = "Axis") + .def("publish_cylinder", + py::overload_cast(&RvizVisualTools::publishCylinder), + "pose"_a, "color"_a = Colors::BLUE, "height"_a = 0.1, "radius"_a = 0.01, + "ns"_a = "Cylinder") + .def("publish_mesh", + py::overload_cast(&RvizVisualTools::publishMesh), + "pose"_a, "filename"_a, "color"_a = Colors::CLEAR, "scale"_a = 1.0, "ns"_a = "Mesh", + "id"_a = 0) + .def("publish_text", + py::overload_cast(&RvizVisualTools::publishText), + "pose"_a, "text"_a, "color"_a = Colors::WHITE, "scale"_a = Scales::MEDIUM, + "static_id"_a = true); +} +} // namespace rviz_visual_tools