-
Notifications
You must be signed in to change notification settings - Fork 58
/
Copy pathmoveit2.py
25 lines (21 loc) · 938 Bytes
/
moveit2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
from moveit2 import MoveIt2Interface
from rclpy.executors import MultiThreadedExecutor
from threading import Thread
import rclpy
class MoveIt2(MoveIt2Interface):
def __init__(self, robot_model: str, separate_gripper_controller: bool = True, use_sim_time: bool = True, node_name: str = 'ign_moveit2_py'):
try:
rclpy.init()
except:
if not rclpy.ok():
import sys
sys.exit("ROS 2 could not be initialised")
super().__init__(robot_model=robot_model,
separate_gripper_controller=separate_gripper_controller,
use_sim_time=use_sim_time,
node_name=node_name)
self._moveit2_executor = MultiThreadedExecutor(1)
self._moveit2_executor.add_node(self)
thread = Thread(target=self._moveit2_executor.spin, args=())
thread.daemon = True
thread.start()