353 lines
17 KiB
Python
353 lines
17 KiB
Python
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) |