import numpy as np from einops import rearrange from collections import deque import rospy from std_msgs.msg import Header from geometry_msgs.msg import Twist from sensor_msgs.msg import JointState, Image from nav_msgs.msg import Odometry from cv_bridge import CvBridge import threading class RosOperator: def __init__(self, args): self.robot_base_deque = None self.puppet_arm_right_deque = None self.puppet_arm_left_deque = None self.img_front_deque = None self.img_right_deque = None self.img_left_deque = None self.img_front_depth_deque = None self.img_right_depth_deque = None self.img_left_depth_deque = None self.bridge = None self.puppet_arm_left_publisher = None self.puppet_arm_right_publisher = None self.robot_base_publisher = None self.puppet_arm_publish_thread = None self.puppet_arm_publish_lock = None self.args = args self.ctrl_state = False self.ctrl_state_lock = threading.Lock() self.init() self.init_ros() def init(self): self.bridge = CvBridge() self.img_left_deque = deque() self.img_right_deque = deque() self.img_front_deque = deque() self.img_left_depth_deque = deque() self.img_right_depth_deque = deque() self.img_front_depth_deque = deque() self.puppet_arm_left_deque = deque() self.puppet_arm_right_deque = deque() self.robot_base_deque = deque() self.puppet_arm_publish_lock = threading.Lock() self.puppet_arm_publish_lock.acquire() def puppet_arm_publish(self, left, right): # 默认速度和力矩值 last_velocity = [-0.010990142822265625, -0.010990142822265625, -0.03296661376953125, 0.010990142822265625, -0.010990142822265625, -0.010990142822265625, -0.010990142822265625, -0.010990142822265625, -0.010990142822265625, -0.03296661376953125, -0.010990142822265625, -0.010990142822265625, -0.03296661376953125, -0.03296661376953125] last_effort = [-0.021978378295898438, 0.2417583465576172, 0.320878982543945, 0.1527481079101562, -0.013187408447265625, -0.013187408447265625, 0.0, -0.03076934814453125, -0.3296699523925781, 0.43956756591797, 0.5208797454833984, -0.11868095397949219, 0.03956031799316406, 0.0] # 修正位置 left[-1] *= 12 right[-1] *= 12 # 始终为正数，小于0的裁剪为0 left[-1] = max(left[-1], 0) right[-1] = max(right[-1], 0) joint_state_msg = JointState() joint_state_msg.header = Header() joint_state_msg.header.stamp = rospy.Time.now() # 设置时间戳 joint_state_msg.name = ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] # 设置关节名称 joint_state_msg.position = left joint_state_msg.velocity = last_velocity[:7] joint_state_msg.effort = last_effort[:7] self.puppet_arm_left_publisher.publish(joint_state_msg) joint_state_msg.position = right joint_state_msg.velocity = last_velocity[7:] joint_state_msg.effort = last_effort[7:] self.puppet_arm_right_publisher.publish(joint_state_msg) def robot_base_publish(self, vel): vel_msg = Twist() vel_msg.linear.x = vel[0] vel_msg.linear.y = 0 vel_msg.linear.z = 0 vel_msg.angular.x = 0 vel_msg.angular.y = 0 vel_msg.angular.z = vel[1] self.robot_base_publisher.publish(vel_msg) def puppet_arm_publish_continuous(self, left, right): rate = rospy.Rate(self.args.publish_rate) left_arm = None right_arm = None while True and not rospy.is_shutdown(): if len(self.puppet_arm_left_deque) != 0: left_arm = list(self.puppet_arm_left_deque[-1].position) if len(self.puppet_arm_right_deque) != 0: right_arm = list(self.puppet_arm_right_deque[-1].position) if left_arm is None or right_arm is None: rate.sleep() continue else: break left_symbol = [1 if left[i] - left_arm[i] > 0 else -1 for i in range(len(left))] right_symbol = [1 if right[i] - right_arm[i] > 0 else -1 for i in range(len(right))] flag = True step = 0 while flag and not rospy.is_shutdown(): if self.puppet_arm_publish_lock.acquire(False): return left_diff = [abs(left[i] - left_arm[i]) for i in range(len(left))] right_diff = [abs(right[i] - right_arm[i]) for i in range(len(right))] flag = False for i in range(len(left)): if left_diff[i] < self.args.arm_steps_length[i]: left_arm[i] = left[i] else: left_arm[i] += left_symbol[i] * self.args.arm_steps_length[i] flag = True for i in range(len(right)): if right_diff[i] < self.args.arm_steps_length[i]: right_arm[i] = right[i] else: right_arm[i] += right_symbol[i] * self.args.arm_steps_length[i] flag = True joint_state_msg = JointState() joint_state_msg.header = Header() joint_state_msg.header.stamp = rospy.Time.now() # 设置时间戳 joint_state_msg.name = ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] # 设置关节名称 joint_state_msg.position = left_arm self.puppet_arm_left_publisher.publish(joint_state_msg) joint_state_msg.position = right_arm self.puppet_arm_right_publisher.publish(joint_state_msg) step += 1 print("puppet_arm_publish_continuous:", step) rate.sleep() def puppet_arm_publish_linear(self, left, right): num_step = 100 rate = rospy.Rate(200) left_arm = None right_arm = None while True and not rospy.is_shutdown(): if len(self.puppet_arm_left_deque) != 0: left_arm = list(self.puppet_arm_left_deque[-1].position) if len(self.puppet_arm_right_deque) != 0: right_arm = list(self.puppet_arm_right_deque[-1].position) if left_arm is None or right_arm is None: rate.sleep() continue else: break traj_left_list = np.linspace(left_arm, left, num_step) traj_right_list = np.linspace(right_arm, right, num_step) for i in range(len(traj_left_list)): traj_left = traj_left_list[i] traj_right = traj_right_list[i] traj_left[-1] = left[-1] traj_right[-1] = right[-1] joint_state_msg = JointState() joint_state_msg.header = Header() joint_state_msg.header.stamp = rospy.Time.now() # 设置时间戳 joint_state_msg.name = ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] # 设置关节名称 joint_state_msg.position = traj_left self.puppet_arm_left_publisher.publish(joint_state_msg) joint_state_msg.position = traj_right self.puppet_arm_right_publisher.publish(joint_state_msg) rate.sleep() def puppet_arm_publish_continuous_thread(self, left, right): if self.puppet_arm_publish_thread is not None: self.puppet_arm_publish_lock.release() self.puppet_arm_publish_thread.join() self.puppet_arm_publish_lock.acquire(False) self.puppet_arm_publish_thread = None self.puppet_arm_publish_thread = threading.Thread(target=self.puppet_arm_publish_continuous, args=(left, right)) self.puppet_arm_publish_thread.start() def get_frame(self): if len(self.img_left_deque) == 0 or len(self.img_right_deque) == 0 or len(self.img_front_deque) == 0 or \ (self.args.use_depth_image and (len(self.img_left_depth_deque) == 0 or len(self.img_right_depth_deque) == 0 or len(self.img_front_depth_deque) == 0)): return False if self.args.use_depth_image: frame_time = min([self.img_left_deque[-1].header.stamp.to_sec(), self.img_right_deque[-1].header.stamp.to_sec(), self.img_front_deque[-1].header.stamp.to_sec(), self.img_left_depth_deque[-1].header.stamp.to_sec(), self.img_right_depth_deque[-1].header.stamp.to_sec(), self.img_front_depth_deque[-1].header.stamp.to_sec()]) else: frame_time = min([self.img_left_deque[-1].header.stamp.to_sec(), self.img_right_deque[-1].header.stamp.to_sec(), self.img_front_deque[-1].header.stamp.to_sec()]) if len(self.img_left_deque) == 0 or self.img_left_deque[-1].header.stamp.to_sec() < frame_time: return False if len(self.img_right_deque) == 0 or self.img_right_deque[-1].header.stamp.to_sec() < frame_time: return False if len(self.img_front_deque) == 0 or self.img_front_deque[-1].header.stamp.to_sec() < frame_time: return False if len(self.puppet_arm_left_deque) == 0 or self.puppet_arm_left_deque[-1].header.stamp.to_sec() < frame_time: return False if len(self.puppet_arm_right_deque) == 0 or self.puppet_arm_right_deque[-1].header.stamp.to_sec() < frame_time: return False if self.args.use_depth_image and (len(self.img_left_depth_deque) == 0 or self.img_left_depth_deque[-1].header.stamp.to_sec() < frame_time): return False if self.args.use_depth_image and (len(self.img_right_depth_deque) == 0 or self.img_right_depth_deque[-1].header.stamp.to_sec() < frame_time): return False if self.args.use_depth_image and (len(self.img_front_depth_deque) == 0 or self.img_front_depth_deque[-1].header.stamp.to_sec() < frame_time): return False if self.args.use_robot_base and (len(self.robot_base_deque) == 0 or self.robot_base_deque[-1].header.stamp.to_sec() < frame_time): return False while self.img_left_deque[0].header.stamp.to_sec() < frame_time: self.img_left_deque.popleft() img_left = self.bridge.imgmsg_to_cv2(self.img_left_deque.popleft(), 'passthrough') while self.img_right_deque[0].header.stamp.to_sec() < frame_time: self.img_right_deque.popleft() img_right = self.bridge.imgmsg_to_cv2(self.img_right_deque.popleft(), 'passthrough') while self.img_front_deque[0].header.stamp.to_sec() < frame_time: self.img_front_deque.popleft() img_front = self.bridge.imgmsg_to_cv2(self.img_front_deque.popleft(), 'passthrough') while self.puppet_arm_left_deque[0].header.stamp.to_sec() < frame_time: self.puppet_arm_left_deque.popleft() puppet_arm_left = self.puppet_arm_left_deque.popleft() while self.puppet_arm_right_deque[0].header.stamp.to_sec() < frame_time: self.puppet_arm_right_deque.popleft() puppet_arm_right = self.puppet_arm_right_deque.popleft() img_left_depth = None if self.args.use_depth_image: while self.img_left_depth_deque[0].header.stamp.to_sec() < frame_time: self.img_left_depth_deque.popleft() img_left_depth = self.bridge.imgmsg_to_cv2(self.img_left_depth_deque.popleft(), 'passthrough') img_right_depth = None if self.args.use_depth_image: while self.img_right_depth_deque[0].header.stamp.to_sec() < frame_time: self.img_right_depth_deque.popleft() img_right_depth = self.bridge.imgmsg_to_cv2(self.img_right_depth_deque.popleft(), 'passthrough') img_front_depth = None if self.args.use_depth_image: while self.img_front_depth_deque[0].header.stamp.to_sec() < frame_time: self.img_front_depth_deque.popleft() img_front_depth = self.bridge.imgmsg_to_cv2(self.img_front_depth_deque.popleft(), 'passthrough') robot_base = None if self.args.use_robot_base: while self.robot_base_deque[0].header.stamp.to_sec() < frame_time: self.robot_base_deque.popleft() robot_base = self.robot_base_deque.popleft() return (img_front, img_left, img_right, img_front_depth, img_left_depth, img_right_depth, puppet_arm_left, puppet_arm_right, robot_base) def img_left_callback(self, msg): if len(self.img_left_deque) >= 2000: self.img_left_deque.popleft() self.img_left_deque.append(msg) def img_right_callback(self, msg): if len(self.img_right_deque) >= 2000: self.img_right_deque.popleft() self.img_right_deque.append(msg) def img_front_callback(self, msg): if len(self.img_front_deque) >= 2000: self.img_front_deque.popleft() self.img_front_deque.append(msg) def img_left_depth_callback(self, msg): if len(self.img_left_depth_deque) >= 2000: self.img_left_depth_deque.popleft() self.img_left_depth_deque.append(msg) def img_right_depth_callback(self, msg): if len(self.img_right_depth_deque) >= 2000: self.img_right_depth_deque.popleft() self.img_right_depth_deque.append(msg) def img_front_depth_callback(self, msg): if len(self.img_front_depth_deque) >= 2000: self.img_front_depth_deque.popleft() self.img_front_depth_deque.append(msg) def puppet_arm_left_callback(self, msg): if len(self.puppet_arm_left_deque) >= 2000: self.puppet_arm_left_deque.popleft() self.puppet_arm_left_deque.append(msg) def puppet_arm_right_callback(self, msg): if len(self.puppet_arm_right_deque) >= 2000: self.puppet_arm_right_deque.popleft() self.puppet_arm_right_deque.append(msg) def robot_base_callback(self, msg): if len(self.robot_base_deque) >= 2000: self.robot_base_deque.popleft() self.robot_base_deque.append(msg) def ctrl_callback(self, msg): self.ctrl_state_lock.acquire() self.ctrl_state = msg.data self.ctrl_state_lock.release() def get_ctrl_state(self): self.ctrl_state_lock.acquire() state = self.ctrl_state self.ctrl_state_lock.release() return state def init_ros(self): rospy.init_node('joint_state_publisher', anonymous=True) rospy.Subscriber(self.args.img_left_topic, Image, self.img_left_callback, queue_size=1000, tcp_nodelay=True) rospy.Subscriber(self.args.img_right_topic, Image, self.img_right_callback, queue_size=1000, tcp_nodelay=True) rospy.Subscriber(self.args.img_front_topic, Image, self.img_front_callback, queue_size=1000, tcp_nodelay=True) if self.args.use_depth_image: rospy.Subscriber(self.args.img_left_depth_topic, Image, self.img_left_depth_callback, queue_size=1000, tcp_nodelay=True) rospy.Subscriber(self.args.img_right_depth_topic, Image, self.img_right_depth_callback, queue_size=1000, tcp_nodelay=True) rospy.Subscriber(self.args.img_front_depth_topic, Image, self.img_front_depth_callback, queue_size=1000, tcp_nodelay=True) rospy.Subscriber(self.args.puppet_arm_left_topic, JointState, self.puppet_arm_left_callback, queue_size=1000, tcp_nodelay=True) rospy.Subscriber(self.args.puppet_arm_right_topic, JointState, self.puppet_arm_right_callback, queue_size=1000, tcp_nodelay=True) # rospy.Subscriber(self.args.robot_base_topic, Odometry, self.robot_base_callback, queue_size=1000, tcp_nodelay=True) # self.puppet_arm_left_publisher = rospy.Publisher(self.args.puppet_arm_left_cmd_topic, JointState, queue_size=10) # self.puppet_arm_right_publisher = rospy.Publisher(self.args.puppet_arm_right_cmd_topic, JointState, queue_size=10) # self.robot_base_publisher = rospy.Publisher(self.args.robot_base_cmd_topic, Twist, queue_size=10) if __name__ == '__main__': import argparse parser = argparse.ArgumentParser() args = parser.parse_args() args.img_left_topic = '/camera_l/color/image_raw' args.img_right_topic = '/camera_r/color/image_raw' args.img_front_topic = '/camera_f/color/image_raw' args.puppet_arm_left_cmd_topic = '/master/joint_left' args.puppet_arm_right_cmd_topic = '/master/joint_right' args.puppet_arm_left_topic = '/puppet/joint_left' args.puppet_arm_right_topic = '/puppet/joint_right' args.publish_rate = 30 args.use_robot_base = False args.use_actions_interpolation = False args.use_depth_image = False a = RosOperator(args) print(a)