通过标定结果识别ArUco码位姿抓取
#! /usr/bin/env python3
# coding:utf-8
import rospy
from dobot_bringup.srv import *
import time
import rospy
import transforms3d as tfs
from geometry_msgs.msg import PoseStamped
import math
import time,datetime
from tabulate import tabulate
import numpy as np
from scipy.spatial.transform import Rotation as R
real_camera_pose = None
def camera_callback(pose):
global real_camera_pose
real_camera_pose = pose.pose
def _msg_to_opencv(transform_msg):
if isinstance(transform_msg, dict):
cmt = transform_msg["position"]
tr = np.array((cmt['x'], cmt['y'], cmt['z']))
cmq = transform_msg["orientation"]
rot = tfs.quaternions.quat2mat(
(cmq['w'], cmq['x'], cmq['y'], cmq['z']))
return rot
else:
cmt = transform_msg.position
tr = [cmt.x, cmt.y, cmt.z]
cmq = transform_msg.orientation
rot = [cmq.w, cmq.x, cmq.y, cmq.z]
return rot,tr
class ROS_demo:
def __init__(self):
# 创建节点
try:
rospy.init_node('demo')
self.client_a()
# 确保 server 是存在的，等待服务开启, 阻塞代码
time.sleep(0.5)
rospy.wait_for_service('/dobot_bringup/srv/EnableRobot')
rospy.wait_for_service('/dobot_bringup/srv/MovJ')
print("服务开启")
except:
print("开启服务错误")
def client_a(self):
# 服务名称地址
self.EnableRobot_l = rospy.ServiceProxy('/dobot_bringup/srv/EnableRobot', EnableRobot)
self.MovJ_l = rospy.ServiceProxy('/dobot_bringup/srv/MovJ', MovJ)
self.Sync_l = rospy.ServiceProxy('/dobot_bringup/srv/Sync', Sync)
self.SpeedFactor_l = rospy.ServiceProxy('/dobot_bringup/srv/SpeedFactor', SpeedFactor)
self.MovL_l = rospy.ServiceProxy('/dobot_bringup/srv/MovL', MovL)
self.DO_l = rospy.ServiceProxy('/dobot_bringup/srv/DO', DO)
self.JointMovJ_l = rospy.ServiceProxy('/dobot_bringup/srv/JointMovJ', JointMovJ)
self.SpeedFactor_l = rospy.ServiceProxy('/dobot_bringup/srv/SpeedFactor', SpeedFactor)
#self.Claw_init_l = rospy.ServiceProxy('/dobot_bringup/srv/Claw_init', Claw_init)
def initialization(self): # 初始化：速度、坐标系、负载、工具偏心等
response = self.EnableRobot_l.call()
cc = SpeedFactorRequest()
cc.ratio = 30
response = self.SpeedFactor_l.call(cc)
def Sync(self): # 同步
response = self.Sync_l.call()
print(response)
def point(self, Move, X_j1, Y_j2, Z_j3, RX_j4, RY_j5, RZ_j6): # 运动指令
if Move == "MovJ":
P1 = MovJRequest()
P1.x = X_j1
P1.y = Y_j2
P1.z = Z_j3
P1.a = RX_j4
P1.b = RY_j5
P1.c = RZ_j6
response = self.MovJ_l.call(P1)
print(response)
elif Move == "MovL":
P1 = MovLRequest()
P1.x = X_j1
P1.y = Y_j2
P1.z = Z_j3
P1.a = RX_j4
P1.b = RY_j5
P1.c = RZ_j6
response = self.MovL_l.call(P1)
print(response)
elif Move == "JointMovJ":
P1 = MovLRequest()
P1.j1 = X_j1
P1.j2 = Y_j2
P1.j3 = Z_j3
P1.j4 = RX_j4
P1.j5 = RY_j5
P1.j6 = RZ_j6
response = self.JointMovJ.call(P1)
print(response)
else:
print("无该指令")
def DO(self, index, status): # IO 控制夹爪/气泵
DO_V = DORequest()
DO_V.index = index
DO_V.status = status
response = self.DO_l.call(DO_V)
#def Claw_init(self): # 485通讯夹爪控制
# response = self.Claw_init_l.call()
# print "夹爪反馈" + str(response)
if __name__ == '__main__':
talker = ROS_demo()
time.sleep(0.5)
talker.initialization()
time.sleep(1)
#rospy.init_node("vision", anonymous=False)
rospy.Subscriber("/aruco_single/pose", PoseStamped, camera_callback,queue_size=10)
talker.point("MovJ", 166.841, -313.59, 280.895, -177.655, 25.137, -112.531)
time.sleep(1)
while not rospy.is_shutdown():
time.sleep(1)
# rospy.loginfo(str(samples))
time.sleep(0.8)
if real_camera_pose == None:
rospy.loginfo('等待相机姿态话题数据到位 ...')
else:
command = str(input("指令: 输入1运行、e退出: "))
if command == "1":
PW, PR = _msg_to_opencv(real_camera_pose)
print(PW)
print(PR)
Rv_left2right = [3.05871 ,0.0249906 ,-2.65032]
t_left2right = [-0.0957464 , -0.410773,0.514155 ]
x = PW[1]
y = PW[2]
z = PW[3]
w = PW[0]
r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
r = r / math.pi * 180
p = math.asin(2 * (w * y - z * x))
p = p / math.pi * 180
y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
y = y / math.pi * 180
# r = r +7
#p = p -5
# y = y + 5
#if r < -180:
# r = 180-(180+(r+7))
#if p < -180:
# p = 180-(180-(p+10))
#if y > 180:
# y = ((y +5)-180) - 180
print("\n源坐标系到目标坐标系旋转顺序为X,Y,Z,左手系．")
PP = PP = [round((t_left2right[0]-PR[0])*1000,3),round((t_left2right[1]-PR[1])*1000,3),round((t_left2right[2]-PR[2])*1000,3),round(r,3),round(p,3),round(y,3)]
print(PP)
while True:
try:
talker.point("MovJ", 166.841, -313.59, 280.895, -177.655, 25.137, -112.531)
talker.point("MovJ", PP[0],PP[1], PP[2]+20,PP[3], PP[4],PP[5])
# talker.point("MovL", 166.841,-313.59,217.895,-177.655,25.137,-112.531)
# talker.DO(1,1)
# talker.point("MovJ", 166.841,-313.59,280.895,-177.655,25.137,-112.531)
# talker.point("MovJ", 166.841,-313.59,280.895,-177.655,25.137,-112.531)
# talker.point("MovL", 166.841,-313.59,217.895,-177.655,25.137,-112.531)
# talker.DO(1,0)
talker.point("MovJ", PP[0],PP[1], PP[2],PP[3], PP[4],PP[5])
talker.point("MovJ", PP[0],PP[1], PP[2]+20,PP[3], PP[4],PP[5])
talker.point("MovJ", 166.841, -313.59, 280.895, -177.655, 25.137, -112.531)
talker.Sync()
break
except:
print("运行失败")
break
elif command == 'e':
break
Comments NOTHING