Skip to content

Commit e4d36ee

Browse files
JafarAbdiJafarAbdi
committed
Add python bindings
1 parent 8578433 commit e4d36ee

File tree

7 files changed

+275
-1
lines changed

7 files changed

+275
-1
lines changed

‎CMakeLists.txt

+20
Original file line numberDiff line numberDiff line change
@@ -99,4 +99,24 @@ if(BUILD_TESTING)
9999
ament_lint_auto_find_test_dependencies()
100100
endif()
101101

102+
####################
103+
## Python binding ##
104+
####################
105+
106+
find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
107+
find_package(pybind11_vendor REQUIRED)
108+
find_package(pybind11 REQUIRED)
109+
110+
ament_python_install_package(moveit_visual_tools PACKAGE_DIR python/moveit_visual_tools)
111+
112+
pybind11_add_module(pymoveit_visual_tools python/src/moveit_visual_tools.cpp)
113+
target_link_libraries(pymoveit_visual_tools PUBLIC moveit_visual_tools)
114+
ament_target_dependencies(pymoveit_visual_tools PUBLIC pybind11 rviz_visual_tools)
115+
install(TARGETS pymoveit_visual_tools
116+
LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/moveit_visual_tools)
117+
install(PROGRAMS
118+
python/examples/moveit_visual_tools_demo.py
119+
DESTINATION lib/${PROJECT_NAME}
120+
)
121+
102122
ament_package()

‎README.md

+4
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,10 @@ To run some demos displaying robot states and collision objects:
5353

5454
ros2 launch moveit_visual_tools demo_rviz.launch.py
5555

56+
To run the python binding demo:
57+
58+
ros2 launch moveit_visual_tools demo_rviz.launch.py launch_python_demo:=true
59+
5660
## Code API
5761

5862
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

+28-1
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,9 @@
3131
from launch import LaunchDescription
3232
from launch_ros.actions import Node
3333
from ament_index_python.packages import get_package_share_directory
34+
from launch.conditions import IfCondition, UnlessCondition
35+
from launch.substitutions import LaunchConfiguration
36+
from launch.actions import DeclareLaunchArgument
3437

3538

3639
def load_file(package_name, file_path):
@@ -61,6 +64,18 @@ def generate_launch_description():
6164
package="moveit_visual_tools",
6265
executable="moveit_visual_tools_demo",
6366
output="screen",
67+
condition=UnlessCondition(LaunchConfiguration("launch_python_demo")),
68+
parameters=[
69+
robot_description,
70+
robot_description_semantic,
71+
],
72+
)
73+
74+
moveit_visual_tools_demo_py = Node(
75+
package="moveit_visual_tools",
76+
executable="moveit_visual_tools_demo.py",
77+
output="screen",
78+
condition=IfCondition(LaunchConfiguration("launch_python_demo")),
6479
parameters=[
6580
robot_description,
6681
robot_description_semantic,
@@ -93,4 +108,16 @@ def generate_launch_description():
93108
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"],
94109
)
95110

96-
return LaunchDescription([rviz_node, static_tf, moveit_visual_tools_demo])
111+
return LaunchDescription(
112+
[
113+
DeclareLaunchArgument(
114+
"launch_python_demo",
115+
default_value="False",
116+
description="Launch the Python demo",
117+
),
118+
rviz_node,
119+
static_tf,
120+
moveit_visual_tools_demo,
121+
moveit_visual_tools_demo_py,
122+
]
123+
)

‎package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,8 @@
2929
<depend>trajectory_msgs</depend>
3030
<depend>visualization_msgs</depend>
3131

32+
<build_depend>pybind11_vendor</build_depend>
33+
3234
<test_depend>ament_lint_auto</test_depend>
3335
<test_depend>ament_lint_common</test_depend>
3436

‎python/examples/moveit_visual_tools_demo.py

+131
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
1+
#!/usr/bin/env python3
2+
3+
import moveit_visual_tools as mvt
4+
import rviz_visual_tools as rvt
5+
import sys
6+
import time
7+
from rclpy import logging
8+
import numpy as np
9+
from ament_index_python.packages import get_package_share_directory
10+
from copy import deepcopy
11+
12+
logger = logging.get_logger("moveit_visual_tools_demo")
13+
14+
from geometry_msgs.msg import Pose, Point, Quaternion
15+
16+
rvt.init()
17+
18+
PLANNING_GROUP_NAME = "arm"
19+
20+
node = rvt.RvizVisualToolsNode("moveit_visual_tools_demo")
21+
node.start_spin_thread()
22+
visual_tools = mvt.MoveItVisualTools(node, "world", "/moveit_visual_tools")
23+
visual_tools.load_planning_scene_monitor()
24+
visual_tools.load_marker_publisher(True)
25+
visual_tools.load_robot_state_publisher("display_robot_state")
26+
visual_tools.set_manual_scene_updating()
27+
28+
robot_state = visual_tools.get_shared_robot_state()
29+
robot_model = visual_tools.get_robot_model()
30+
jmg = robot_model.get_joint_model_group(PLANNING_GROUP_NAME)
31+
32+
visual_tools.delete_all_markers()
33+
visual_tools.remove_all_collision_objects()
34+
visual_tools.trigger_planning_scene_update()
35+
36+
visual_tools.publish_text(
37+
Pose(position=Point(x=0.0, y=0.0, z=4.0)),
38+
"MoveIt-Visual-Tools",
39+
scale=rvt.Scales.XXLARGE,
40+
)
41+
visual_tools.trigger()
42+
43+
logger.info("Showing 5 random robot states")
44+
for _ in range(5):
45+
robot_state.set_to_random_positions(jmg)
46+
visual_tools.publish_robot_state(robot_state)
47+
time.sleep(1.0)
48+
49+
logger.info("Showing 5 random robot states in different colors")
50+
for _ in range(5):
51+
robot_state.set_to_random_positions(jmg)
52+
visual_tools.publish_robot_state(robot_state, color=visual_tools.get_random_color())
53+
time.sleep(1.0)
54+
55+
logger.info("Hiding robot state")
56+
visual_tools.hide_robot()
57+
time.sleep(1.0)
58+
59+
logger.info("Showing robot state")
60+
visual_tools.publish_robot_state(robot_state, color=rvt.Colors.DEFAULT)
61+
time.sleep(1.0)
62+
63+
logger.info("Publishing Collision Floor")
64+
visual_tools.publish_collision_floor(-0.5, "Floor", color=rvt.Colors.GREY)
65+
visual_tools.trigger_planning_scene_update()
66+
time.sleep(1.0)
67+
68+
logger.info("Publishing Collision Wall")
69+
visual_tools.publish_collision_wall(
70+
-4.0, -1.0, 0.0, np.pi * 1.1, 6.0, 4.0, "Wall", color=rvt.Colors.GREY
71+
)
72+
visual_tools.trigger_planning_scene_update()
73+
time.sleep(1.0)
74+
75+
76+
logger.info("Publishing Collision Mesh")
77+
mesh_pose = Pose()
78+
for i in range(5):
79+
visual_tools.publish_collision_mesh(
80+
mesh_pose,
81+
f"Mesh_{i}",
82+
f"file://{get_package_share_directory('moveit_visual_tools')}/resources/demo_mesh.stl",
83+
color=visual_tools.get_random_color(),
84+
)
85+
mesh_pose.position.x += 0.4
86+
visual_tools.trigger_planning_scene_update()
87+
time.sleep(1.0)
88+
89+
logger.info("Publishing Collision Block")
90+
block_pose = Pose(position=Point(y=1.0))
91+
for i in np.linspace(0.0, 1.0, 10):
92+
visual_tools.publish_collision_block(
93+
block_pose,
94+
f"Block_{i}",
95+
color=visual_tools.get_random_color(),
96+
)
97+
block_pose.position.x += 0.4
98+
visual_tools.trigger_planning_scene_update()
99+
time.sleep(1.0)
100+
101+
logger.info("Publishing Collision Rectanglular Cuboid")
102+
cuboid_min_size = 0.05
103+
cuboid_max_size = 0.2
104+
cuboid_point1 = Point(y=2.0)
105+
for i in np.linspace(0.0, 1.0, 10):
106+
cuboid_point2 = deepcopy(cuboid_point1)
107+
cuboid_point2.x += i * cuboid_max_size + cuboid_min_size
108+
cuboid_point2.y += (i * cuboid_max_size + cuboid_min_size) / 2.0
109+
cuboid_point2.z += i * cuboid_max_size + cuboid_min_size
110+
visual_tools.publish_collision_cuboid(
111+
cuboid_point1, cuboid_point2, f"Cuboid_{i}", visual_tools.get_random_color()
112+
)
113+
cuboid_point1.x += 0.4
114+
visual_tools.trigger_planning_scene_update()
115+
time.sleep(1.0)
116+
117+
logger.info("Publishing Collision Cylinder")
118+
cylinder_min_size = 0.01
119+
cylinder_max_size = 0.3
120+
cylinder_pose = Pose(position=Point(y=3.0))
121+
for i in np.linspace(0.0, 1.0, 10):
122+
visual_tools.publish_collision_cylinder(
123+
cylinder_pose,
124+
height=i * cylinder_max_size + cylinder_min_size,
125+
radius=i * cylinder_max_size + cylinder_min_size,
126+
object_name=f"Cylinder_{i}",
127+
color=visual_tools.get_random_color(),
128+
)
129+
cylinder_pose.position.x += 0.1 + i
130+
visual_tools.trigger_planning_scene_update()
131+
time.sleep(1.0)

‎python/moveit_visual_tools/__init__.py

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
from moveit_visual_tools.pymoveit_visual_tools import MoveItVisualTools

‎python/src/moveit_visual_tools.cpp

+89
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
#include <pybind11/pybind11.h>
2+
#include <pybind11/eigen.h>
3+
#include <pybind11/stl.h>
4+
#include <rviz_visual_tools/rviz_visual_tools.hpp>
5+
#include <moveit_visual_tools/moveit_visual_tools.h>
6+
#include <rviz_visual_tools/binding_utils.hpp>
7+
8+
namespace py = pybind11;
9+
using py::literals::operator""_a;
10+
11+
namespace moveit_visual_tools
12+
{
13+
PYBIND11_MODULE(pymoveit_visual_tools, m)
14+
{
15+
py::module::import("rviz_visual_tools.pyrviz_visual_tools");
16+
py::module::import("moveit.core.robot_state");
17+
py::module::import("moveit.core.robot_model");
18+
py::module::import("moveit.core.planning_scene");
19+
20+
py::class_<MoveItVisualTools, rviz_visual_tools::RvizVisualTools>(m, "MoveItVisualTools")
21+
.def(py::init(
22+
[](const rviz_visual_tools::RvizVisualToolsNode::SharedPtr& node) { return MoveItVisualTools(node); }))
23+
.def(py::init([](const rviz_visual_tools::RvizVisualToolsNode::SharedPtr& node, const std::string& base_frame,
24+
const std::string& marker_topic) { return MoveItVisualTools(node, base_frame, marker_topic); }),
25+
"node"_a, "base_frame"_a, "marker_topic"_a = rviz_visual_tools::RVIZ_MARKER_TOPIC)
26+
.def("set_robot_state_topic", &MoveItVisualTools::setRobotStateTopic)
27+
.def("set_planning_scene_topic", &MoveItVisualTools::setPlanningSceneTopic)
28+
.def("load_planning_scene_monitor", &MoveItVisualTools::loadPlanningSceneMonitor)
29+
.def("process_collision_object_msg", &MoveItVisualTools::processCollisionObjectMsg, "msg"_a,
30+
"color"_a = rviz_visual_tools::Colors::GREEN)
31+
.def("process_attached_collision_object_msg", &MoveItVisualTools::processAttachedCollisionObjectMsg)
32+
.def("move_collision_object",
33+
py::overload_cast<const geometry_msgs::msg::Pose&, const std::string&, const rviz_visual_tools::Colors&>(
34+
&MoveItVisualTools::moveCollisionObject))
35+
.def("trigger_planning_scene_update", &MoveItVisualTools::triggerPlanningSceneUpdate)
36+
.def("load_shared_robot_state", &MoveItVisualTools::loadSharedRobotState)
37+
.def("get_shared_robot_state", &MoveItVisualTools::getSharedRobotState)
38+
.def("get_root_robot_state", &MoveItVisualTools::getSharedRobotState)
39+
.def("get_robot_model", &MoveItVisualTools::getRobotModel)
40+
.def("load_ee_marker", &MoveItVisualTools::loadEEMarker)
41+
.def("load_trajectory_publisher", &MoveItVisualTools::loadTrajectoryPub)
42+
.def("load_robot_state_publisher", &MoveItVisualTools::loadRobotStatePub, "topic"_a, "blocking"_a = true)
43+
.def("set_manual_scene_updating", &MoveItVisualTools::setManualSceneUpdating, "enable_manual"_a = true)
44+
.def("publish_ee_markers",
45+
py::overload_cast<const geometry_msgs::msg::Pose&, const moveit::core::JointModelGroup*,
46+
const std::vector<double>&, const rviz_visual_tools::Colors&, const std::string&>(
47+
&MoveItVisualTools::publishEEMarkers),
48+
"pose"_a, "ee_jmq"_a, "ee_joint_pos"_a = std::vector<double>(),
49+
"color"_a = rviz_visual_tools::Colors::DEFAULT, "ns"_a = "end_effector")
50+
.def("publish_ik_solutions", &MoveItVisualTools::publishIKSolutions, "ik_solutions"_a, "arm_jmg"_a,
51+
"display_time"_a = 0.4)
52+
.def("remove_all_collision_objects", &MoveItVisualTools::removeAllCollisionObjects)
53+
.def("cleanup_collision_object", &MoveItVisualTools::cleanupCO)
54+
.def("cleanup_attached_collision_object", &MoveItVisualTools::cleanupACO)
55+
.def("attach_collision_object", &MoveItVisualTools::attachCO)
56+
.def("publish_collision_floor", &MoveItVisualTools::publishCollisionFloor, "z"_a = 0.0, "plane_name"_a = "Floor",
57+
"color"_a = rviz_visual_tools::Colors::GREEN)
58+
.def("publish_collision_block", &MoveItVisualTools::publishCollisionBlock, "pose"_a, "name"_a = "block",
59+
"size"_a = 0.1, "color"_a = rviz_visual_tools::Colors::GREEN)
60+
.def("publish_collision_cuboid",
61+
py::overload_cast<const geometry_msgs::msg::Point&, const geometry_msgs::msg::Point&, const std::string&,
62+
const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionCuboid))
63+
.def("publish_collision_cuboid",
64+
py::overload_cast<const geometry_msgs::msg::Pose&, double, double, double, const std::string&,
65+
const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionCuboid))
66+
.def("publish_collision_cylinder",
67+
py::overload_cast<const geometry_msgs::msg::Pose&, const std::string&, double, double,
68+
const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionCylinder),
69+
"object_pose"_a, "object_name"_a, "radius"_a, "height"_a, "color"_a = rviz_visual_tools::Colors::GREEN)
70+
.def("publish_collision_mesh",
71+
py::overload_cast<const geometry_msgs::msg::Pose&, const std::string&, const std::string&,
72+
const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionMesh),
73+
"object_pose"_a, "object_name"_a, "mesh_path"_a, "color"_a = rviz_visual_tools::Colors::GREEN)
74+
.def("publish_collision_wall",
75+
py::overload_cast<double, double, double, double, double, double, const std::string&,
76+
const rviz_visual_tools::Colors&>(&MoveItVisualTools::publishCollisionWall),
77+
"x"_a, "y"_a, "z"_a, "angle"_a = 0.0, "width"_a = 2.0, "height"_a = 1.5, "name"_a = "wall",
78+
"color"_a = rviz_visual_tools::Colors::GREEN)
79+
.def("publish_workspace_parameters", &MoveItVisualTools::publishWorkspaceParameters)
80+
.def("publish_robot_state",
81+
py::overload_cast<const moveit::core::RobotState&, const rviz_visual_tools::Colors&,
82+
const std::vector<std::string>&>(&MoveItVisualTools::publishRobotState),
83+
"robot_state"_a, "color"_a = rviz_visual_tools::Colors::DEFAULT,
84+
"highlight_links"_a = std::vector<std::string>())
85+
.def("hide_robot", &MoveItVisualTools::hideRobot)
86+
.def("show_joint_limits", &MoveItVisualTools::showJointLimits)
87+
.def("get_planning_scene_monitor", &MoveItVisualTools::getPlanningSceneMonitor);
88+
}
89+
} // namespace moveit_visual_tools

0 commit comments

Comments
 (0)