diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e08aa6..10d82cf 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/README.md b/README.md index 555954d..39d391a 100644 --- a/README.md +++ b/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) diff --git a/launch/demo_rviz.launch.py b/launch/demo_rviz.launch.py index 5c08233..4ec8be0 100644 --- a/launch/demo_rviz.launch.py +++ b/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, + ] + ) diff --git a/package.xml b/package.xml index 72d54fd..2057040 100644 --- a/package.xml +++ b/package.xml @@ -29,6 +29,8 @@ trajectory_msgs visualization_msgs + pybind11_vendor + ament_lint_auto ament_lint_common diff --git a/python/examples/moveit_visual_tools_demo.py b/python/examples/moveit_visual_tools_demo.py new file mode 100755 index 0000000..7815d62 --- /dev/null +++ b/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) diff --git a/python/moveit_visual_tools/__init__.py b/python/moveit_visual_tools/__init__.py new file mode 100644 index 0000000..8f737c4 --- /dev/null +++ b/python/moveit_visual_tools/__init__.py @@ -0,0 +1 @@ +from moveit_visual_tools.pymoveit_visual_tools import MoveItVisualTools diff --git a/python/src/moveit_visual_tools.cpp b/python/src/moveit_visual_tools.cpp new file mode 100644 index 0000000..44cde2a --- /dev/null +++ b/python/src/moveit_visual_tools.cpp @@ -0,0 +1,89 @@ +#include +#include +#include +#include +#include +#include + +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_(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( + &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 rviz_visual_tools::Colors&, const std::string&>( + &MoveItVisualTools::publishEEMarkers), + "pose"_a, "ee_jmq"_a, "ee_joint_pos"_a = std::vector(), + "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(&MoveItVisualTools::publishCollisionCuboid)) + .def("publish_collision_cuboid", + py::overload_cast(&MoveItVisualTools::publishCollisionCuboid)) + .def("publish_collision_cylinder", + py::overload_cast(&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(&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(&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&>(&MoveItVisualTools::publishRobotState), + "robot_state"_a, "color"_a = rviz_visual_tools::Colors::DEFAULT, + "highlight_links"_a = std::vector()) + .def("hide_robot", &MoveItVisualTools::hideRobot) + .def("show_joint_limits", &MoveItVisualTools::showJointLimits) + .def("get_planning_scene_monitor", &MoveItVisualTools::getPlanningSceneMonitor); +} +} // namespace moveit_visual_tools