from pathlib import Path from threading import Thread import time import rclpy import rclpy.duration import rclpy.time from unilabos_msgs.action import SendCmd from rclpy.action import ActionServer, ActionClient from rclpy.action.server import ServerGoalHandle from rclpy.callback_groups import ReentrantCallbackGroup from moveit_msgs.msg import JointConstraint, Constraints from copy import deepcopy from pymoveit2 import MoveIt2, GripperInterface import numpy as np from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode # from tf_transformations import quaternion_from_euler from tf2_ros import Buffer, TransformListener import json class MoveitInterface(BaseROS2DeviceNode): def __init__(self, device_id, moveit_type, joint_poses, resource_tracker, rotation = None, device_config = None): super().__init__( driver_instance=self, device_id=device_id, status_types={}, action_value_mappings={}, hardware_interface={}, print_publish=False, resource_tracker=resource_tracker, ) self.callback_group = ReentrantCallbackGroup() data_config = json.load(open(f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json", encoding="utf-8")) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.arm_move_flag = False self.move_option = ['pick', 'place', 'side_pick', 'side_place'] self.joint_poses = joint_poses self.cartesian_flag = False self.mesh_group = ['reactor','sample','beaker'] self.moveit2 = {} self.resource_action = None self.resource_client = None self.resource_action_ok = False for move_group, config in data_config.items(): base_link_name = f"{device_id}_{config['base_link_name']}" end_effector_name = f"{device_id}_{config['end_effector_name']}" joint_names = [f"{device_id}_{name}" for name in config['joint_names']] self.moveit2[f"{move_group}"] = MoveIt2( node=self, joint_names=joint_names, base_link_name=base_link_name, end_effector_name=end_effector_name, group_name=f"{device_id}_{move_group}", callback_group=self.callback_group, use_move_group_action= True, ignore_new_calls_while_executing = True ) self.moveit2[f"{move_group}"].allowed_planning_time = 3.0 self.moveit_pose_server = ActionServer( self, SendCmd, f'set_position', execute_callback=self.callback, callback_group=self.callback_group, result_timeout=5000 ) self.moveit_pick_server = ActionServer( self, SendCmd, f'pick_and_place', execute_callback=self.pick_and_place_callback, callback_group=self.callback_group, result_timeout=5000 ) self.moveit_joint_server = ActionServer( self, SendCmd, f'set_status', execute_callback=self.set_status_callback, callback_group=self.callback_group, result_timeout=5000 ) self.create_timer(1, self.wait_for_resource_action,callback_group=ReentrantCallbackGroup()) def wait_for_resource_action(self): if not self.resource_action_ok: while self.resource_action is None: self.resource_action = self.check_tf_update_actions() time.sleep(1) self.resource_client = ActionClient(self, SendCmd, self.resource_action) self.resource_action_ok = True while not self.resource_client.wait_for_server(timeout_sec=5.0): self.get_logger().info('等待 TfUpdate 服务器...') def check_tf_update_actions(self): topics = self.get_topic_names_and_types() for topic_item in topics: topic_name, topic_types = topic_item if 'action_msgs/msg/GoalStatusArray' in topic_types: # 删除 /_action/status 部分 base_name = topic_name.replace('/_action/status', '') # 检查最后一个部分是否为 tf_update parts = base_name.split('/') if parts and parts[-1] == 'tf_update': return base_name return None def callback(self,goal_handle: ServerGoalHandle): """使用moveit 移动到指定点 Args: command: A JSON-formatted string that includes quaternion, speed, position position (list) : 点的位置 [x,y,z] quaternion (list) : 点的姿态(四元数) [x,y,z,w] move_group (string) : The move group moveit will plan speed (float) : The speed of the movement, speed > 0 retry (int) : Retry times when moveit plan fails Returns: None """ result = SendCmd.Result() try: cmd_str = str(goal_handle.request.command).replace('\'','\"') cmd_dict = json.loads(cmd_str) self.moveit_task(**cmd_dict) goal_handle.succeed() result.success=True except Exception as e: print(e) goal_handle.abort() result.success=False return result def moveit_task(self,move_group,position,quaternion,speed=1,retry=10,cartesian=False, target_link = None ,offsets =[0,0,0]): speed_ = float(max(0.1, min(speed, 1))) self.moveit2[move_group].max_velocity = speed_ self.moveit2[move_group].max_acceleration = speed_ re_ = False pose_result =[x + y for x, y in zip(position, offsets)] # print(pose_result) while retry > -1 and not re_ : self.moveit2[move_group].move_to_pose( target_link=target_link, position=pose_result, quat_xyzw=quaternion, cartesian=cartesian, # cartesian_fraction_threshold=0.0, cartesian_max_step=0.01, weight_position=1.0 ) re_ = self.moveit2[move_group].wait_until_executed() retry += -1 return re_ def moveit_joint_task(self, move_group, joint_positions, joint_names = None,speed=1,retry=10): re_ = False joint_positions_ = [float(x) for x in joint_positions] speed_ = float(max(0.1, min(speed, 1))) self.moveit2[move_group].max_velocity = speed_ self.moveit2[move_group].max_acceleration = speed_ while retry > -1 and not re_ : self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_,joint_names=joint_names) re_ = self.moveit2[move_group].wait_until_executed() retry += -1 print(self.moveit2[move_group].compute_fk(joint_positions)) return re_ def resource_manager(self,resource, parent_link): goal_msg = SendCmd.Goal() str_dict = {} str_dict[resource] = parent_link goal_msg.command = json.dumps(str_dict) self.resource_client.send_goal(goal_msg) return True def pick_and_place_callback(self,goal_handle: ServerGoalHandle): """ Using MoveIt to make the robotic arm pick or place materials to a target point. Args: command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height *option (string) : Action type: pick/place/side_pick/side_place *move_group (string): The move group moveit will plan *status(string) : Target pose resource(string) : The target resource x_distance (float) : The distance to the target in x direction(meters) y_distance (float) : The distance to the target in y direction(meters) lift_height (float) : The height at which the material should be lifted(meters) retry (float) : Retry times when moveit plan fails speed (float) : The speed of the movement, speed > 0 Returns: None """ result = SendCmd.Result() try: cmd_str = str(goal_handle.request.command).replace('\'','\"') cmd_dict = json.loads(cmd_str) if cmd_dict["option"] in self.move_option: option_index = self.move_option.index(cmd_dict["option"]) place_flag = option_index % 2 config = {} function_list = [] status = cmd_dict["status"] joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status] config.update({k: cmd_dict[k] for k in ["speed", "retry", 'move_group'] if k in cmd_dict}) # 夹取 if not place_flag: if 'target' in cmd_dict.keys(): function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"])) else: function_list.append(lambda: self.resource_manager(cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name)) else: function_list.append(lambda: self.resource_manager(cmd_dict["resource"], 'world')) constraints = [] if "constraints" in cmd_dict.keys(): for i in range(len(cmd_dict["constraints"])): v = float(cmd_dict["constraints"][i]) if v > 0: constraints.append(JointConstraint( joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i], position=joint_positions_[i], tolerance_above=v, tolerance_below=v, weight=1.0 )) if "lift_height" in cmd_dict.keys(): retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_) pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z] quaternion = [retval.pose.orientation.x, retval.pose.orientation.y, retval.pose.orientation.z, retval.pose.orientation.w] function_list = [lambda: self.moveit_task(position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z], quaternion=quaternion,**config, cartesian=self.cartesian_flag)] + function_list pose[2] += float(cmd_dict["lift_height"]) function_list.append(lambda: self.moveit_task(position=pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)) end_pose = pose if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys(): if "x_distance" in cmd_dict.keys(): deep_pose = deepcopy(pose) deep_pose[0] += float(cmd_dict["x_distance"]) elif "y_distance" in cmd_dict.keys(): deep_pose = deepcopy(pose) deep_pose[1] += float(cmd_dict["y_distance"]) function_list = [lambda: self.moveit_task(position=pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)] + function_list function_list.append(lambda: self.moveit_task(position=deep_pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)) end_pose = deep_pose retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik( position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)) position_ = [retval_ik.position[retval_ik.name.index(i)] for i in self.moveit2[cmd_dict["move_group"]].joint_names] function_list = [lambda: self.moveit_joint_task( joint_positions=position_, joint_names=self.moveit2[cmd_dict["move_group"]].joint_names, **config, )] + function_list else: function_list = [lambda: self.moveit_joint_task(**config,joint_positions=joint_positions_)] + function_list for i in range(len(function_list)): if i == 0: self.cartesian_flag = False else: self.cartesian_flag = True re = function_list[i]() if not re: print(i,re) goal_handle.abort() result.success=False return result goal_handle.succeed() result.success=True except Exception as e: print(e) goal_handle.abort() self.cartesian_flag = False result.success=False return result def set_status_callback(self,goal_handle: ServerGoalHandle): """ Goto home position Args: command: A JSON-formatted string that includes speed *status (string) : The joint status moveit will plan *move_group (string): The move group moveit will plan separate (list) : The joint index to be separated lift_height (float) : The height at which the material should be lifted(meters) x_distance (float) : The distance to the target in x direction(meters) y_distance (float) : The distance to the target in y direction(meters) speed (float) : The speed of the movement, speed > 0 retry (float) : Retry times when moveit plan fails Returns: None """ result = SendCmd.Result() try: cmd_str = str(goal_handle.request.command).replace('\'','\"') cmd_dict = json.loads(cmd_str) config = {} config["move_group"] = cmd_dict["move_group"] if "speed" in cmd_dict.keys(): config["speed"] = cmd_dict["speed"] if "retry" in cmd_dict.keys(): config["retry"] = cmd_dict["retry"] if "separate" in cmd_dict.keys(): separate = cmd_dict["separate"] else: separate = None status = cmd_dict["status"] joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status] if separate is not None : separate_j = [joint_positions_[i] for i in separate] separate_n = [self.joint_names[i] for i in separate] re = self.moveit_joint_task(**config, joint_positions=separate_j,joint_names=separate_n) if not re: goal_handle.abort() result.success=False return result re = self.moveit_joint_task(**config,joint_positions=joint_positions_) if not re: goal_handle.abort() result.success=False return result goal_handle.succeed() result.success=True except Exception as e: print(e) goal_handle.abort() result.success=False return result