Add python bindings

CMakeLists.txt

@@ -100,4 +100,24 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+####################
+## Python binding ##
+####################
+
+find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
+find_package(pybind11_vendor REQUIRED)
+find_package(pybind11 REQUIRED)
+
+ament_python_install_package(moveit_visual_tools PACKAGE_DIR python/moveit_visual_tools)
+
+pybind11_add_module(pymoveit_visual_tools python/src/moveit_visual_tools.cpp)
+target_link_libraries(pymoveit_visual_tools PUBLIC moveit_visual_tools)
+ament_target_dependencies(pymoveit_visual_tools PUBLIC pybind11 rviz_visual_tools)
+install(TARGETS pymoveit_visual_tools
+        LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/moveit_visual_tools)
+install(PROGRAMS
+  python/examples/moveit_visual_tools_demo.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()

README.md

@@ -51,6 +51,10 @@ To run some demos displaying robot states and collision objects:
 
     ros2 launch moveit_visual_tools demo_rviz.launch.py
 
+To run the python binding demo:
+
+    ros2 launch moveit_visual_tools demo_rviz.launch.py launch_python_demo:=true
+
 ## Code API
 
 See [VisualTools Class Reference](http://docs.ros.org/kinetic/api/moveit_visual_tools/html/classmoveit__visual__tools_1_1MoveItVisualTools.html)

launch/demo_rviz.launch.py

@@ -32,6 +32,9 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
 
 
 def load_file(package_name, file_path):
@@ -62,6 +65,18 @@ def generate_launch_description():
         package="moveit_visual_tools",
         executable="moveit_visual_tools_demo",
         output="screen",
+        condition=UnlessCondition(LaunchConfiguration("launch_python_demo")),
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+        ],
+    )
+
+    moveit_visual_tools_demo_py = Node(
+        package="moveit_visual_tools",
+        executable="moveit_visual_tools_demo.py",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("launch_python_demo")),
         parameters=[
             robot_description,
             robot_description_semantic,
@@ -94,4 +109,16 @@ def generate_launch_description():
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"],
     )
 
-    return LaunchDescription([rviz_node, static_tf, moveit_visual_tools_demo])
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "launch_python_demo",
+                default_value="False",
+                description="Launch the Python demo",
+            ),
+            rviz_node,
+            static_tf,
+            moveit_visual_tools_demo,
+            moveit_visual_tools_demo_py,
+        ]
+    )

package.xml

@@ -29,6 +29,8 @@
   <depend>trajectory_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <build_depend>pybind11_vendor</build_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

python/examples/moveit_visual_tools_demo.py

@@ -0,0 +1,131 @@
+#!/usr/bin/env python3
+
+import moveit_visual_tools as mvt
+import rviz_visual_tools as rvt
+import sys
+import time
+from rclpy import logging
+import numpy as np
+from ament_index_python.packages import get_package_share_directory
+from copy import deepcopy
+
+logger = logging.get_logger("moveit_visual_tools_demo")
+
+from geometry_msgs.msg import Pose, Point, Quaternion
+
+rvt.init()
+
+PLANNING_GROUP_NAME = "arm"
+
+node = rvt.RvizVisualToolsNode("moveit_visual_tools_demo")
+node.start_spin_thread()
+visual_tools = mvt.MoveItVisualTools(node, "world", "/moveit_visual_tools")
+visual_tools.load_planning_scene_monitor()
+visual_tools.load_marker_publisher(True)
+visual_tools.load_robot_state_publisher("display_robot_state")
+visual_tools.set_manual_scene_updating()
+
+robot_state = visual_tools.get_shared_robot_state()
+robot_model = visual_tools.get_robot_model()
+jmg = robot_model.get_joint_model_group(PLANNING_GROUP_NAME)
+
+visual_tools.delete_all_markers()
+visual_tools.remove_all_collision_objects()
+visual_tools.trigger_planning_scene_update()
+
+visual_tools.publish_text(
+    Pose(position=Point(x=0.0, y=0.0, z=4.0)),
+    "MoveIt-Visual-Tools",
+    scale=rvt.Scales.XXLARGE,
+)
+visual_tools.trigger()
+
+logger.info("Showing 5 random robot states")
+for _ in range(5):
+    robot_state.set_to_random_positions(jmg)
+    visual_tools.publish_robot_state(robot_state)
+    time.sleep(1.0)
+
+logger.info("Showing 5 random robot states in different colors")
+for _ in range(5):
+    robot_state.set_to_random_positions(jmg)
+    visual_tools.publish_robot_state(robot_state, color=visual_tools.get_random_color())
+    time.sleep(1.0)
+
+logger.info("Hiding robot state")
+visual_tools.hide_robot()
+time.sleep(1.0)
+
+logger.info("Showing robot state")
+visual_tools.publish_robot_state(robot_state, color=rvt.Colors.DEFAULT)
+time.sleep(1.0)
+
+logger.info("Publishing Collision Floor")
+visual_tools.publish_collision_floor(-0.5, "Floor", color=rvt.Colors.GREY)
+visual_tools.trigger_planning_scene_update()
+time.sleep(1.0)
+
+logger.info("Publishing Collision Wall")
+visual_tools.publish_collision_wall(
+    -4.0, -1.0, 0.0, np.pi * 1.1, 6.0, 4.0, "Wall", color=rvt.Colors.GREY
+)
+visual_tools.trigger_planning_scene_update()
+time.sleep(1.0)
+
+
+logger.info("Publishing Collision Mesh")
+mesh_pose = Pose()
+for i in range(5):
+    visual_tools.publish_collision_mesh(
+        mesh_pose,
+        f"Mesh_{i}",
+        f"file://{get_package_share_directory('moveit_visual_tools')}/resources/demo_mesh.stl",
+        color=visual_tools.get_random_color(),
+    )
+    mesh_pose.position.x += 0.4
+    visual_tools.trigger_planning_scene_update()
+time.sleep(1.0)
+
+logger.info("Publishing Collision Block")
+block_pose = Pose(position=Point(y=1.0))
+for i in np.linspace(0.0, 1.0, 10):
+    visual_tools.publish_collision_block(
+        block_pose,
+        f"Block_{i}",
+        color=visual_tools.get_random_color(),
+    )
+    block_pose.position.x += 0.4
+    visual_tools.trigger_planning_scene_update()
+time.sleep(1.0)
+
+logger.info("Publishing Collision Rectanglular Cuboid")
+cuboid_min_size = 0.05
+cuboid_max_size = 0.2
+cuboid_point1 = Point(y=2.0)
+for i in np.linspace(0.0, 1.0, 10):
+    cuboid_point2 = deepcopy(cuboid_point1)
+    cuboid_point2.x += i * cuboid_max_size + cuboid_min_size
+    cuboid_point2.y += (i * cuboid_max_size + cuboid_min_size) / 2.0
+    cuboid_point2.z += i * cuboid_max_size + cuboid_min_size
+    visual_tools.publish_collision_cuboid(
+        cuboid_point1, cuboid_point2, f"Cuboid_{i}", visual_tools.get_random_color()
+    )
+    cuboid_point1.x += 0.4
+    visual_tools.trigger_planning_scene_update()
+time.sleep(1.0)
+
+logger.info("Publishing Collision Cylinder")
+cylinder_min_size = 0.01
+cylinder_max_size = 0.3
+cylinder_pose = Pose(position=Point(y=3.0))
+for i in np.linspace(0.0, 1.0, 10):
+    visual_tools.publish_collision_cylinder(
+        cylinder_pose,
+        height=i * cylinder_max_size + cylinder_min_size,
+        radius=i * cylinder_max_size + cylinder_min_size,
+        object_name=f"Cylinder_{i}",
+        color=visual_tools.get_random_color(),
+    )
+    cylinder_pose.position.x += 0.1 + i
+    visual_tools.trigger_planning_scene_update()
+time.sleep(1.0)

python/moveit_visual_tools/__init__.py

@@ -0,0 +1 @@
+from moveit_visual_tools.pymoveit_visual_tools import MoveItVisualTools

python/src/moveit_visual_tools.cpp

@@ -0,0 +1,89 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+#include <pybind11/stl.h>
+#include <rviz_visual_tools/rviz_visual_tools.hpp>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <rviz_visual_tools/binding_utils.hpp>
+
+namespace py = pybind11;
+using py::literals::operator""_a;
+
+namespace moveit_visual_tools
+{
+PYBIND11_MODULE(pymoveit_visual_tools, m)
+{
+  py::module::import("rviz_visual_tools.pyrviz_visual_tools");
+  py::module::import("moveit.core.robot_state");
+  py::module::import("moveit.core.robot_model");
+  py::module::import("moveit.core.planning_scene");
+
+  py::class_<MoveItVisualTools, rviz_visual_tools::RvizVisualTools>(m, "MoveItVisualTools")
+      .def(py::init(
+          [](const rviz_visual_tools::RvizVisualToolsNode::SharedPtr& node) { return MoveItVisualTools(node); }))
+      .def(py::init([](const rviz_visual_tools::RvizVisualToolsNode::SharedPtr& node, const std::string& base_frame,
+                       const std::string& marker_topic) { return MoveItVisualTools(node, base_frame, marker_topic); }),
+           "node"_a, "base_frame"_a, "marker_topic"_a = rviz_visual_tools::RVIZ_MARKER_TOPIC)
+      .def("set_robot_state_topic", &MoveItVisualTools::setRobotStateTopic)
+      .def("set_planning_scene_topic", &MoveItVisualTools::setPlanningSceneTopic)
+      .def("load_planning_scene_monitor", &MoveItVisualTools::loadPlanningSceneMonitor)
+      .def("process_collision_object_msg", &MoveItVisualTools::processCollisionObjectMsg, "msg"_a,
+           "color"_a = rviz_visual_tools::Colors::GREEN)
+      .def("process_attached_collision_object_msg", &MoveItVisualTools::processAttachedCollisionObjectMsg)
+      .def("move_collision_object",
+           py::overload_cast<const geometry_msgs::msg::Pose&, const std::string&, const rviz_visual_tools::Colors&>(
+               &MoveItVisualTools::moveCollisionObject))
+      .def("trigger_planning_scene_update", &MoveItVisualTools::triggerPlanningSceneUpdate)
+      .def("load_shared_robot_state", &MoveItVisualTools::loadSharedRobotState)
+      .def("get_shared_robot_state", &MoveItVisualTools::getSharedRobotState)
+      .def("get_root_robot_state", &MoveItVisualTools::getSharedRobotState)
+      .def("get_robot_model", &MoveItVisualTools::getRobotModel)
+      .def("load_ee_marker", &MoveItVisualTools::loadEEMarker)
+      .def("load_trajectory_publisher", &MoveItVisualTools::loadTrajectoryPub)
+      .def("load_robot_state_publisher", &MoveItVisualTools::loadRobotStatePub, "topic"_a, "blocking"_a = true)
+      .def("set_manual_scene_updating", &MoveItVisualTools::setManualSceneUpdating, "enable_manual"_a = true)
+      .def("publish_ee_markers",
+           py::overload_cast<const geometry_msgs::msg::Pose&, const moveit::core::JointModelGroup*,
+                             const std::vector<double>&, const rviz_visual_tools::Colors&, const std::string&>(
+               &MoveItVisualTools::publishEEMarkers),
+           "pose"_a, "ee_jmq"_a, "ee_joint_pos"_a = std::vector<double>(),
+           "color"_a = rviz_visual_tools::Colors::DEFAULT, "ns"_a = "end_effector")
+      .def("publish_ik_solutions", &MoveItVisualTools::publishIKSolutions, "ik_solutions"_a, "arm_jmg"_a,
+           "display_time"_a = 0.4)
+      .def("remove_all_collision_objects", &MoveItVisualTools::removeAllCollisionObjects)
+      .def("cleanup_collision_object", &MoveItVisualTools::cleanupCO)
+      .def("cleanup_attached_collision_object", &MoveItVisualTools::cleanupACO)
+      .def("attach_collision_object", &MoveItVisualTools::attachCO)
+      .def("publish_collision_floor", &MoveItVisualTools::publishCollisionFloor, "z"_a = 0.0, "plane_name"_a = "Floor",
+           "color"_a = rviz_visual_tools::Colors::GREEN)
+      .def("publish_collision_block", &MoveItVisualTools::publishCollisionBlock, "pose"_a, "name"_a = "block",
+           "size"_a = 0.1, "color"_a = rviz_visual_tools::Colors::GREEN)
+      .def("publish_collision_cuboid",
+           py::overload_cast<const geometry_msgs::msg::Point&, const geometry_msgs::msg::Point&, const std::string&,
+                             const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionCuboid))
+      .def("publish_collision_cuboid",
+           py::overload_cast<const geometry_msgs::msg::Pose&, double, double, double, const std::string&,
+                             const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionCuboid))
+      .def("publish_collision_cylinder",
+           py::overload_cast<const geometry_msgs::msg::Pose&, const std::string&, double, double,
+                             const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionCylinder),
+           "object_pose"_a, "object_name"_a, "radius"_a, "height"_a, "color"_a = rviz_visual_tools::Colors::GREEN)
+      .def("publish_collision_mesh",
+           py::overload_cast<const geometry_msgs::msg::Pose&, const std::string&, const std::string&,
+                             const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionMesh),
+           "object_pose"_a, "object_name"_a, "mesh_path"_a, "color"_a = rviz_visual_tools::Colors::GREEN)
+      .def("publish_collision_wall",
+           py::overload_cast<double, double, double, double, double, double, const std::string&,
+                             const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionWall),
+           "x"_a, "y"_a, "z"_a, "angle"_a = 0.0, "width"_a = 2.0, "height"_a = 1.5, "name"_a = "wall",
+           "color"_a = rviz_visual_tools::Colors::GREEN)
+      .def("publish_workspace_parameters", &MoveItVisualTools::publishWorkspaceParameters)
+      .def("publish_robot_state",
+           py::overload_cast<const moveit::core::RobotState&, const rviz_visual_tools::Colors&,
+                             const std::vector<std::string>&>(&MoveItVisualTools::publishRobotState),
+           "robot_state"_a, "color"_a = rviz_visual_tools::Colors::DEFAULT,
+           "highlight_links"_a = std::vector<std::string>())
+      .def("hide_robot", &MoveItVisualTools::hideRobot)
+      .def("show_joint_limits", &MoveItVisualTools::showJointLimits)
+      .def("get_planning_scene_monitor", &MoveItVisualTools::getPlanningSceneMonitor);
+}
+}  // namespace moveit_visual_tools