def pub_empty_uv_info(self, tile_id, obj_desire):
uvuv = uv()
tile_uv = tileuv()
tile_uv.tile_id = tile_id
tile_uv.obj_desire = obj_desire
tile_uv.cen_uv.uvinfo = [0, 0]
tile_uv.f1th_uv.uvinfo = [0, 0]
tile_uv.s2th_uv.uvinfo = [0, 0]
tile_uv.t3th_uv.uvinfo = [0, 0]
tile_uv.f4th_uv.uvinfo = [0, 0]
self.tile_pub.publish(tile_uv)
def draw_triangle_numbers(self, contours, rgb, obj_desire):
uvuv = uv()
tile_uv = tileuv()
alltile_uvlist, newrgb = self.Draw_triangle(contours, rgb, obj_desire)
newtileuv = self.Sort_tile_mass_num(alltile_uvlist)
for i in newtileuv:
# print "i",i
cv2.putText(newrgb, str(i[0][0]), (i[1][0], i[1][1]),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
tile_uv.tile_id = i[0][0]
tile_uv.obj_desire = obj_desire
tile_uv.cen_uv.uvinfo = [i[1][0], i[1][1]]
tile_uv.f1th_uv.uvinfo = i[2][0]
tile_uv.s2th_uv.uvinfo = i[2][1]
tile_uv.t3th_uv.uvinfo = i[2][2]
tile_uv.f4th_uv.uvinfo = i[2][3]
self.tile_pub.publish(tile_uv)
return newrgb.copy()
def main():
#uvlist=[123.0,112.0]
uvlist = []
camf = 624.0429 * 1e-03
#316,251
# uvcentral_place=[316,251]
uvcentral_place = [
335, 172
] #320,201#180323,174#325,163#355,diyige canshuda,you yi
uvcentral = [
369, 123
] #sucking central109-125OK,370,201#368,95#120#362,123#133da xiang xia,
First_joint_angular = []
calibinfo = [
0.109982645426, 0.114476746567, -0.0415924235801, 0.249492772999,
0.523487628443, 0.239612281752, 0.778652691205
]
urdfname = "/data/ros/ur_ws_yue/src/tilling_robot/urdf/ur5.urdf"
cailename = "/data/ros/ur_ws_yue/src/tilling_robot/yaml/cam_500_logitech.yaml"
nodename = "tilling_vision_control"
tile_width = 0.01 #m
ace_ibvs = 50
vel_ibvs = 0.1
ace = 1.4
vel = 1.05
urt = 0
detat = 0.05
ratet = 30
z_distance = 0
lamda = 3.666666
# lamda = 1000.066666
z = 0.45
ur_reader = Urposition()
code_flag_sub = Codeflags()
ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)
u_error_pub = rospy.Publisher("/object_feature_u_error",
Float64,
queue_size=10)
v_error_pub = rospy.Publisher("/object_feature_v_error",
Float64,
queue_size=10)
sub5 = rospy.Subscriber("/pick_place_tile_vision/open_ur_rotation_id",
UInt8, code_flag_sub.callback_open_ur_rotation_id)
"""
Those two flag use to make sucker sucking tile
"""
object_flag = 0
open_vison_flag = 0
desire_flag = 0
turn_on_vision_flag = 0
"""
nodename,urdfname,delta,kappa,lamda,califilename,camf)
"""
F0 = TilingVisionControl(nodename, urdfname, detat, lamda, cailename, camf)
ur_pub, sucker_center_obj_pub, sucker_center_desire_pub = F0.Init_node()
rate = rospy.Rate(ratet)
First_joint_angular = ur_reader.ave_ur_pose
Object_joint_angular_vision_state = []
Object_joint_angular_sucking_state = [
-58.85, -90.39, -88.02, -93.75, 86.74, 69.84
] #66.02
Object_joint_angular_sucking_state_new = F0.change_angle_to_pi(
Object_joint_angular_sucking_state)
Desire_joint_angular_vision_state = []
Desire_joint_angular_place_state = [
-175.78, -82.87, -112.83, 18.08, 86.98, 43.05
] #43.5
Desire_joint_angular_place_state_new = F0.change_angle_to_pi(
Desire_joint_angular_place_state)
print "First_joint_angular", First_joint_angular
# tile_2=[-217.59,-105.73,-89.85,22.94,88.92,45.29]
# tile_2_new=F0.change_angle_to_pi(tile_2)
# tile_3=[-217.92,-104.96,-82.53,16.9,88.92,45.27]
# tile_3_new=F0.change_angle_to_pi(tile_3)
count_for_tile = 0
open_suking_flag = 0
open_roation_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 0
open_suking_flag_desire = 0
open_move_to_object_flag = 0
open_align_line_flag = 0
close_align_line_flag = 0
open_vision_desire_flag = 0
open_vision_object_flag = 0
tile_nums = 0 #为了记录吸第几块砖
msg_uv_o = uv()
msg_uv_d = uv()
os.system(
"rostopic pub /pick_place_tile_vision/object_detect_id std_msgs/UInt8 '1' --once"
)
while not rospy.is_shutdown():
# print "F0.tile_0_buf",F0.tile_0_buf[-1]
# print "F0.tile_1_buf",F0.tile_1_buf[-1]
# print "F0.tile_2_buf",F0.tile_2_buf[-1]
# print "F0.tile_3_buf",F0.tile_3_buf[-1]
# time.sleep(1)
desire_tile_path_cacu = []
try:
"""
1,通过object_ibvs_id控制视觉伺服
"""
"""
Go to object position,just need opreating tile 1,
Now UV also is [316,251]
"""
msg_uv_o.uvinfo = uvcentral
msg_uv_d.uvinfo = uvcentral_place
sucker_center_obj_pub.publish(msg_uv_o)
sucker_center_desire_pub.publish(msg_uv_d)
q_now = ur_reader.ave_ur_pose
print "q_now", q_now
if len(q_now) != 0:
if len(F0.tile_0_buf) != 0 and open_vison_flag == 0:
print "First,Go to object tile position--------------"
uvm = [uvcentral]
xp = F0.tile_0_buf[-1][1] #目标位置
q_pub_now = F0.ibvs_run_ur5(uvm, z, q_now, calibinfo, xp)
MoveUrString = F0.Move_ur(q_pub_now, ace_ibvs, vel_ibvs,
urt)
ur_pub.publish(MoveUrString)
time.sleep(0.05)
feature_error_zero_flag = F0.check_vison_error_is_zero(
F0.tile_0_buf[-1][1], uvcentral, 5)
"""
if error is zero ,we close vision servo
"""
if feature_error_zero_flag:
open_vison_flag = 1
open_suking_flag = 1
print "F0.tile_0_buf", F0.tile_0_buf[-1]
"""
第二步，关闭视觉伺服，打开轨迹规划，并吸瓷砖
"""
if open_vison_flag == 1 and open_suking_flag == 1:
"""
second,go to sucking tile
"""
print "-------------num1,go to sucking tile------------"
z_distance = 0.13
q_before_sucking = q_now
q_new_pub = F0.move_sucker_to_tile(z_distance, tile_width,
q_now)
MoveUrString = F0.Move_ur(q_new_pub, ace, vel, urt)
ur_pub.publish(MoveUrString)
time.sleep(1.5)
print """num2,Open air pump for sucking"""
if open_suking_flag == 1:
F0.Open_sucking_close_Ardunio()
print """num3,go back the same point"""
# print "-------Third,go back the same point---------"
MoveUrString = F0.Move_ur(q_before_sucking, ace, vel,
urt)
ur_pub.publish(MoveUrString)
# time.sleep(1.5)
os.system(
"rostopic pub /pick_place_tile_vision/object_detect_id std_msgs/UInt8 '0' --once"
)
open_move_to_desire_flag = 1
"""
第三步，移动到目标位置
"""
if open_move_to_desire_flag == 1:
print """
Third,move ur to desire position.
"""
# temp=F0.Rotaion_tool_90(Desire_joint_angular_place_state,90)
MoveUrString = F0.Move_ur(Desire_joint_angular_place_state,
ace, vel, urt)
ur_pub.publish(MoveUrString)
open_vison_flag = 1
open_suking_flag = 0
open_move_to_desire_flag = 0
# open_vison_flag_desire=1
open_align_line_flag = 1
F0.tile_0_buf = []
# time.sleep(3)
os.system(
"rostopic pub /pick_place_tile_vision/desire_detect_id std_msgs/UInt8 '1' --once"
)
time.sleep(0.1)
"""
close all flags
"""
"""
打开瓷砖对齐程序
"""
if open_align_line_flag == 1:
try:
os.system(
"rostopic pub /pick_place_tile_vision/open_ur_rotation_id std_msgs/UInt8 '1' --once"
)
if len(code_flag_sub.open_ur_rotation_id_buf) != 0:
if code_flag_sub.open_ur_rotation_id_buf[-1] == 0:
open_align_line_flag = 0
open_vison_flag_desire = 1
print "now tile is ok,you can go to next step-------"
except KeyboardInterrupt:
sys.exit()
if len(F0.tile_0_buf) != 0 and open_vison_flag_desire == 1:
print """
第四步，打开目标空间的视觉伺服程序
"""
# if len(F0.tile_0_buf)!=0 and open_vison_flag_desire ==1:
uvcentral_place_2 = [316, 251]
# uvm2=[uvcentral_place_2]
uvmm = [uvcentral_place]
print "num0，打开目标空间的视觉伺服程序"
print "-------目标空间贴第" + str(tile_nums) + "块砖-----"
xpp = F0.tile_0_buf[-1][1]
# uv0=xpp
# desire_tile_path_cacu=F0.Caculate_desire_uv_for_place(2)
# print "xpp=F0.tile_0_buf[-1][1]",F0.tile_0_buf[-1][1]
# print "desire_tile_path_cacu[tile_nums]",desire_tile_path_cacu[tile_nums]
# print "desire_tile_path_cacu----->>>>>", desire_tile_path_cacu
# time.sleep(1)
if tile_nums < 3:
q_pub_now_d = F0.ibvs_run_ur5(uvmm, z, q_now,
calibinfo, xpp)
MoveUrString_1 = F0.Move_ur(q_pub_now_d, ace_ibvs,
vel_ibvs, urt)
ur_pub.publish(MoveUrString_1)
feature_error_zero_flag_d = F0.check_vison_error_is_zero(
xpp, uvcentral_place, 11)
"""
if error is zero ,we close vision servo
"""
if feature_error_zero_flag_d:
# open_suking_flag_desire = 1
open_vison_flag_desire = 0
close_align_line_flag = 1
tile_nums += 1
time.sleep(0.5)
desire_tile_path_cacu = []
print "---------you are now in desire space-----------"
if open_vison_flag_desire == 0 and close_align_line_flag == 1:
print """
第五步，关闭视觉伺服，打开轨迹规划，并释放瓷砖
"""
print """
num0,go to sucking tile
"""
q_before_sucking_d = q_now
print "tile_nums------>>>>>>>", tile_nums
x_distance = 0.65
# if (tile_nums-1)<3:
q_new_pub_d = F0.move_ur_to_desire(x_distance,
q_before_sucking_d)
# q_after_sucking_d = q_new_pub_d
F0.os_move_ur(q_new_pub_d, ace, vel, urt)
# MoveUrString = F0.Move_ur(q_new_pub_d, ace, vel, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(3)
open_suking_flag_desire = 1
close_align_line_flag = 0
if open_suking_flag_desire == 1:
print """
num1,close air pump for sucking
"""
F0.Open_sucking_close_Ardunio()
open_suking_flag_desire = 0
print """
num2,go back the same point
"""
MoveUrString = F0.Move_ur(q_before_sucking_d, ace, vel,
urt)
ur_pub.publish(MoveUrString)
time.sleep(1.5)
open_move_to_object_flag = 1
if open_move_to_object_flag == 1:
print "#########Desire,last step,move ur to object position-------"
# x_distance = 0.35
# q_new_pub=F0.move_ur_to_desire(x_distance,q_now)
MoveUrString_2 = F0.Move_ur(
Object_joint_angular_sucking_state_new, ace, vel, urt)
ur_pub.publish(MoveUrString_2)
open_vison_flag = 0
open_suking_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 0
open_move_to_object_flag = 0
F0.tile_0_buf = []
q_now = []
# time.sleep(1)
os.system(
"rostopic pub /pick_place_tile_vision/desire_detect_id std_msgs/UInt8 '0' --once"
)
os.system(
"rostopic pub /pick_place_tile_vision/object_detect_id std_msgs/UInt8 '1' --once"
)
"""
close all flags
"""
if tile_nums >= 3:
tile_nums = 0
else:
print "UR5 is Not Ok,Please check"
rate.sleep()
except KeyboardInterrupt:
sys.exit()
def process_rgb_desire_image(self, rgb_image):
color1 = [0, 0, 255]
color2 = [255, 0, 0]
color3 = [0, 255, 0]
thickness = 5
"""
region selecting
row 行
"""
bottom_left_cols1 = 0.33
bottom_left_rows1 = 0.45
top_left_cols1 = 0.33
top_left_rows1 = 0.08
bottom_right_cols1 = 0.75
bottom_right_rows1 = 0.45
top_right_cols1 = 0.75
top_right_rows1 = 0.08
sucker_line_uv = sucker_tile_line()
uvuv = uv()
MORPH = 7
CANNY = 250
##################
rgb = rgb_image
if rgb_image is not None:
"""'
Select Blue Desire position
tile_id=1,fixed point id
obj_desire="o" object
"""
YHLS = self.select_yellow(rgb)
# print "YHLS",YHLS
Y_gray = self.convert_gray_scale(YHLS)
Y_smooth = self.apply_smoothing(Y_gray, 11)
Y_edges = self.detect_edges(Y_smooth)
New_edges = Y_edges.copy()
Y_kernel = cv2.getStructuringElement(cv2.MORPH_RECT,
(MORPH, MORPH))
Y_closed = cv2.morphologyEx(Y_edges.copy(), cv2.MORPH_CLOSE,
Y_kernel)
_, Y_contours, Y_h = cv2.findContours(Y_closed.copy(),
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# print "Y_h",Y_h,Y_contours
if len(Y_contours) != 0:
rgb = self.draw_triangle_numbers(Y_contours, rgb, 'o')
else:
print "There is no tile0,you need put one blue tile"
self.pub_empty_uv_info(0, 'o')
cv2.circle(rgb, (316, 251), 10, (20, 100, 220), -2)
cv2.putText(rgb, 'center', (316, 251),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 1)
if len(self.obj_buf) != 0:
cv2.circle(
rgb, (int(self.obj_buf[-1][0]), int(self.obj_buf[-1][1])),
10, (100, 100, 220), -2)
cv2.putText(
rgb, 'oc',
(int(self.obj_buf[-1][0]), int(self.obj_buf[-1][1])),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 1)
if len(self.desire_buf) != 0:
cv2.circle(
rgb,
(int(self.desire_buf[-1][0]), int(self.desire_buf[-1][1])),
10, (200, 200, 220), -2)
cv2.putText(
rgb, 'dc',
(int(self.desire_buf[-1][0]), int(self.desire_buf[-1][1])),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 1)
"""
bottom_left_cols1=0.53
bottom_left_rows1=0.70
top_left_cols1=0.53
top_left_rows1=0.28
bottom_right_cols1=0.45
bottom_right_rows1=0.70
top_right_cols1=0.45
top_right_rows1=0.28
"""
# region_up = self.select_region(Y_edges, 0.44, 0.80, 0.44, 0.15, 0.70, 0.80, 0.70, 0.15)
region_up = self.select_region(Y_edges, bottom_left_cols1,
bottom_left_rows1, top_left_cols1,
top_left_rows1, bottom_right_cols1,
bottom_right_rows1, top_right_cols1,
top_right_rows1)
region_up_line = self.hough_lines(region_up)
left_line_info, slop_intercept = self.tile_sucker_lines(
rgb, region_up_line)
if left_line_info != None:
cv2.line(rgb, left_line_info[0], left_line_info[1], color1,
thickness)
sucker_line_uv.sucker_tile_uv0.uvinfo = [
left_line_info[0][0], left_line_info[0][1]
]
sucker_line_uv.sucker_tile_uv1.uvinfo = [
left_line_info[1][0], left_line_info[1][1]
]
sucker_line_uv.sucker_tile_slope = slop_intercept[0]
sucker_line_uv.sucker_tile_intercept = slop_intercept[1]
self.sucker_line_pub.publish(sucker_line_uv)
print "slop_intercept--------------", slop_intercept
else:
pass
"""
line detect
"""
cv2.namedWindow('region_up', cv2.WINDOW_NORMAL)
cv2.imshow('region_up', region_up)
# cv2.namedWindow( 'sucker_line', cv2.WINDOW_NORMAL )
# cv2.imshow( 'sucker_line', region_up_line )
# """
# HLS SPACE
# """
# HLSDOUBLE=self.convert_hls(rgb)
# cv2.namedWindow( 'HLSDOUBLE_Space', cv2.WINDOW_NORMAL )
# cv2.imshow( 'HLSDOUBLE_Space', HLSDOUBLE )
#
cv2.namedWindow('Black_HLS_Space', cv2.WINDOW_NORMAL)
cv2.imshow('Black_HLS_Space', YHLS)
cv2.namedWindow('Black_tile_edges', cv2.WINDOW_NORMAL)
cv2.imshow('Black_tile_edges', Y_edges)
cv2.namedWindow('tile_pixel_frame', cv2.WINDOW_NORMAL)
cv2.imshow('tile_pixel_frame', rgb)
cv2.waitKey(8)
# # 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rgb_image, "bgr8"))
except CvBridgeError as e:
print e
def Draw_triangle(self, contours, rgb, obj_desire):
##################
DELAY = 0.02
USE_CAM = 1
IS_FOUND = 0
count = 0 #count feature tile numbers
cnt = 0
central_list = []
uvuv = uv()
tile_uv = tileuv()
##################
_width = 480.0
_height = 640.0
_margin = 0.0
corners = np.array([
[[_margin, _margin]],
[[_margin, _height + _margin]],
[[_width + _margin, _height + _margin]],
[[_width + _margin, _margin]],
])
pts_dst = np.array(corners, np.float32)
latest_central = (0, 0)
resultuv = []
for cont in contours:
"""
#1,num,2,centeral point 3,for angular point uv ,4,clockwise direction
#caculating Area for tile selected just one tile
"""
# print "cont----------", cont
# 获取轮廓长度
arc_len = cv2.arcLength(cont, True)
# 多边形拟合
approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True)
# print "cv2.contourArea(cont)",cv2.contourArea(cont)
# print "approx",len(np.array(approx).reshape(-1,2))
if cv2.contourArea(cont) > 5000 and cv2.contourArea(cont) < 20000:
# if cv2.contourArea(cont) > 3000:
if (len(approx) == 4):
# print "a"
IS_FOUND = 1
M = cv2.moments(cont)
# 获取图像质心坐标
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
now_central = (cX, cY)
if self.Judge_isnot_same_tile(latest_central,
now_central) != 1:
count += 1
# cv2.putText(rgb, str(count), (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
# print "CX,CY", [cX, cY]
central_list.append([cX, cY])
pts_src = np.array(approx, np.float32)
# print "pts_src", pts_src
cv2.circle(rgb, (cX, cY), 5, (0, 0, 0), -1)
# print approx.tolist()
angular_point = []
print "approx", approx.tolist()
new_approx = self.Sort_tile_feature(approx.tolist())
new_approx = sorted(new_approx,
reverse=True,
key=lambda k: [k[1], k[0]])
# new_approx=approx
print "new_approx", new_approx
for i in range(len(new_approx)):
if i == 0:
cv2.circle(rgb,
(new_approx[i][0], new_approx[i][1]), 5,
(20, 60, 220), -1)
angular_point.append(
[new_approx[i][0], new_approx[i][1]])
cv2.putText(rgb, str(i),
(new_approx[i][0], new_approx[i][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 1)
else:
cv2.circle(rgb,
(new_approx[i][0], new_approx[i][1]), 5,
(0, 255, 0), -1)
angular_point.append(
[new_approx[i][0], new_approx[i][1]])
cv2.putText(rgb, str(i),
(new_approx[i][0], new_approx[i][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 1)
resultuv.append([[count], [cX, cY], angular_point])
# draw trangle in image
h, status = cv2.findHomography(pts_src, pts_dst)
out = cv2.warpPerspective(rgb, h,
(int(_width + _margin * 2),
int(_height + _margin * 2)))
cv2.drawContours(rgb, [approx], -1, (0, 255, 255), 3)
# cv2.drawContours(rgb, [approx], -1, (20*count, 255, 0), -1, cv2.LINE_AA)
print "all info for tile------", resultuv
print "angular_point", angular_point
cv2.line(rgb,
((angular_point[0][0] + angular_point[2][0]) / 2,
(angular_point[0][1] + angular_point[2][1]) / 2),
(cX, cY), (255, 0, 0), 3)
cv2.line(rgb,
((angular_point[1][0] + angular_point[3][0]) / 2,
(angular_point[1][1] + angular_point[3][1]) / 2),
(cX, cY), (255, 0, 0), 3)
# print "Now tile id",count
# if count == 1:
# tile_uv.tile_id = count
# tile_uv.obj_desire = obj_desire
# tile_uv.cen_uv.uvinfo = [cX, cY]
# tile_uv.f1th_uv.uvinfo = angular_point[0]
# tile_uv.s2th_uv.uvinfo = angular_point[1]
# tile_uv.t3th_uv.uvinfo = angular_point[2]
# tile_uv.f4th_uv.uvinfo = angular_point[3]
# self.tile_pub.publish(tile_uv)
latest_central = now_central
else:
pass
# count += 1
# cnt += 11
# return rgb.copy()
return resultuv, rgb.copy()
def main():
#uvlist=[123.0,112.0]
uvlist = []
camf = 624.0429 * 1e-03
#316,251
camera_center = [316, 251]
# uvcentral_place=[316,251]
uvcentral_place = [323, 174] #320,201#180
uvcentral = [339, 120] #sucking central109-125OK,370,201#368,95#120
First_joint_angular = []
calibinfo = [
0.109982645426, 0.114476746567, -0.0415924235801, 0.249492772999,
0.523487628443, 0.239612281752, 0.778652691205
]
urdfname = "/data/ros/ur_ws_yue/src/tilling_robot/urdf/ur5.urdf"
cailename = "/data/ros/ur_ws_yue/src/tilling_robot/yaml/cam_500_logitech.yaml"
nodename = "tilling_vision_control"
tile_width = 0.01 #m
# ace=50
# vel=0.1
ace = 1.4
vel = 1.05
urt = 0
detat = 0.05
ratet = 15
z_distance = 0
lamda = 3.666666
# lamda = 7.066666
level = 2
z = 0.45
ur_reader = Urposition()
ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)
u_error_pub = rospy.Publisher("/object_feature_u_error",
Float64,
queue_size=10)
v_error_pub = rospy.Publisher("/object_feature_v_error",
Float64,
queue_size=10)
ar_reader = arReader()
ar_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers,
ar_reader.callback)
"""
Those two flag use to make sucker sucking tile
"""
object_flag = 0
open_vison_flag = 0
desire_flag = 0
turn_on_vision_flag = 0
"""
nodename,urdfname,delta,kappa,lamda,califilename,camf)
"""
F0 = TilingVisionControl(nodename, urdfname, detat, lamda, cailename, camf)
ur_pub, sucker_center_obj_pub, sucker_center_desire_pub = F0.Init_node()
rate = rospy.Rate(ratet)
First_joint_angular = ur_reader.ave_ur_pose
Object_joint_angular_vision_state = []
Object_joint_angular_sucking_state = [
-58.85, -90.39, -88.02, -91.27, 90.11, 69.44
] #66.02
Object_joint_angular_sucking_state_new = F0.change_angle_to_pi(
Object_joint_angular_sucking_state)
Desire_joint_angular_vision_state = []
Desire_joint_angular_place_state = [
-175.78, -86.57, -120.76, 29.08, 86.95, 43.05
]
Desire_joint_angular_place_state_new = F0.change_angle_to_pi(
Desire_joint_angular_place_state)
print "First_joint_angular", First_joint_angular
# tile_2=[-217.59,-105.73,-89.85,22.94,88.92,45.29]
# tile_2_new=F0.change_angle_to_pi(tile_2)
# tile_3=[-217.92,-104.96,-82.53,16.9,88.92,45.27]
# tile_3_new=F0.change_angle_to_pi(tile_3)
open_to_desire_pose_flag = 0 #用于打开关闭ur移动到贴砖位置
count_for_tile = 0
open_suking_flag = 0
open_roation_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 0
open_suking_flag_desire = 0
open_move_to_object_flag = 0
tile_nums = 0 #为了记录吸第几块砖
msg_uv_o = uv()
msg_uv_d = uv()
pos_dict_temp = []
open_store_ar_pose = 0
count_time_line = 0
while not rospy.is_shutdown():
desire_tile_path_cacu = []
uvlist = []
try:
# #q_now,ar_pose,info
# q_now = ur_reader.ave_ur_pose
# print "q_now",q_now
# if open_store_ar_pose==0:
# pos_dict = ar_reader.ave_pos_dict
# print "pos_dict", pos_dict
# if len(pos_dict)!=0:
# # all_pub_q=F0.caculate_feature_pos(q_now,pos_dict,calibinfo)
# pos_dict_temp=pos_dict
# all_pub_q=F0.caculate_pub_joint_for_ur_use_path(2,q_now,pos_dict_temp,calibinfo)
# print all_pub_q
# open_store_ar_pose=1
# if len(pos_dict_temp)!=0:
# # F0.get_ar_frame_in_base1(q_now,pos_dict,calibinfo)
# # k,p=F0.get_jacabian_from_joint(urdfname,q_now)
# # print "urdf",p
# # F0.caculating_path(2,q_now,pos_dict,calibinfo)
#
# # for i in xrange(len(all_pub_q)):
# MoveUrString=F0.Move_ur(all_pub_q[0], ace, vel, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(5)
# i+=1
# if i==4:
# i=0
# """
# 1,通过object_ibvs_id控制视觉伺服
# """
# """
# First,Go to object position,just need opreating tile 1,
# Now UV also is [316,251]
# """
# msg_uv_o.uvinfo=uvcentral
# msg_uv_d.uvinfo=uvcentral_place
# sucker_center_obj_pub.publish(msg_uv_o)
# sucker_center_desire_pub.publish(msg_uv_d)
q_now = ur_reader.ave_ur_pose
print "q_now", q_now
if len(q_now) != 0:
# if len(F0.tile_0_buf)!=0 and open_vison_flag ==0:
# print "Go to object position--------------"
# uvm = [uvcentral]
# xp=F0.tile_0_buf[-1][1]#目标位置
# q_pub_now=F0.ibvs_run_ur5(uvm,z,q_now,calibinfo,xp)
# MoveUrString=F0.Move_ur(q_pub_now, ace, vel, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(0.05)
# feature_error_zero_flag=F0.check_vison_error_is_zero(F0.tile_0_buf[-1][1],uvcentral,5)
# """
# if error is zero ,we close vision servo
# """
# if feature_error_zero_flag:
# open_vison_flag =1
# open_suking_flag=1
# print "F0.tile_0_buf",F0.tile_0_buf[-1]
# """
# 第二步，关闭视觉伺服，打开轨迹规划，并吸瓷砖
# """
# if open_vison_flag ==1 and open_suking_flag==1:
# """
# First,go to sucking tile
# """
# print "-------------First,go to sucking tile------------"
# z_distance=0.13
# q_before_sucking=q_now
# q_new_pub=F0.move_sucker_to_tile(z_distance,tile_width,q_now)
# MoveUrString=F0.Move_ur(q_new_pub, ace, 0.2, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(1.5)
# """
# Second,Open air pump for sucking
# """
# if open_suking_flag==1:
# F0.Open_sucking_close_Ardunio()
# """
# Third,go back the same point
# """
# print "-------Third,go back the same point---------"
# MoveUrString=F0.Move_ur(q_before_sucking, ace, 0.2, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(1)
# open_move_to_desire_flag=1
# if open_move_to_desire_flag==1:
# """
# Fourth,move ur to desire position.
# """
# print "------Fourth,move ur to desire position--------"
# MoveUrString=F0.Move_ur(Desire_joint_angular_place_state_new, ace, 0.3, urt)
# ur_pub.publish(MoveUrString)
# open_vison_flag = 1
# open_suking_flag = 0
# open_move_to_desire_flag=0
# open_vison_flag_desire=1
# F0.tile_0_buf=[]
# time.sleep(6)
#
# """
# close all flags
# """
#
# """
# 第三步，打开目标空间的视觉伺服程序
# """
if open_vison_flag_desire == 0:
pos_dict = ar_reader.ave_pos_dict
uvmm = [camera_center]
print "目标空间第一步，打开目标空间的视觉伺服程序"
# xpp=F0.tile_0_buf[-1][1]
uvlist.append(F0.get_uv_from_ar(pos_dict[0][:3])[:2])
# print uvlist
uv0 = uvlist[0]
q_pub_now_d = F0.ibvs_run_ur5(uvmm, z, q_now, calibinfo,
uv0)
MoveUrString_1 = F0.Move_ur(q_pub_now_d, ace, vel, urt)
ur_pub.publish(MoveUrString_1)
feature_error_zero_flag_d = F0.check_vison_error_is_zero(
uv0, camera_center, 3)
"""
if error is zero ,we close vision servo
"""
if feature_error_zero_flag_d:
print """
目标空间第二步，计算贴砖路径
"""
# open_suking_flag_desire = 1
open_vison_flag_desire = 1
# tile_nums+=1
time.sleep(2.0)
print "q_now before all_pub_q"
all_pub_q = F0.caculate_pub_joint_for_ur_use_path(
level, q_now, pos_dict, calibinfo)
# print "---------you are now in desire space-----------"
"""
第四步，关闭视觉伺服，打开轨迹规划，并释放瓷砖
"""
print "open_vison_flag_desire ==1 and open_suking_flag_desire==1", open_vison_flag_desire, open_suking_flag_desire
if open_vison_flag_desire == 1:
# MoveUrString=F0.Move_ur(all_pub_q[0], ace, vel, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(5)
"""
First,go to place sucking tile position
"""
print "第四步，关闭视觉伺服，打开轨迹规划，并释放瓷砖"
count_time_line += 1
if open_to_desire_pose_flag == 0:
MoveUrString = F0.Move_ur(all_pub_q[tile_nums], ace,
0.2, urt)
ur_pub.publish(MoveUrString)
time.sleep(4)
open_to_desire_pose_flag = 1
x_distance = 0.60
if open_to_desire_pose_flag == 1 and count_time_line == 2:
q_before_sucking_d = all_pub_q[tile_nums]
q_new_pub_d = F0.move_ur_to_desire(
x_distance, all_pub_q[tile_nums])
MoveUrString = F0.Move_ur(q_new_pub_d, 0.2, vel, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(5)/
open_suking_flag_desire = 1
count_time_line = 0
tile_nums += 1
"""
Second,close air pump for sucking
"""
if open_suking_flag_desire == 1:
F0.Open_sucking_close_Ardunio()
open_suking_flag_desire = 0
"""
Third,go back the same point
"""
MoveUrString = F0.Move_ur(q_before_sucking_d, ace, 0.2,
urt)
# ur_pub.publish(MoveUrString)
# time.sleep(1)
open_move_to_object_flag = 1
if open_move_to_object_flag == 1:
"""
Fourth,move ur to desire position.
"""
print "#########Desire,--Fourth,move ur to object position-------"
MoveUrString_2 = F0.Move_ur(
Object_joint_angular_sucking_state_new, ace, 0.3,
urt)
# ur_pub.publish(MoveUrString_2)
open_vison_flag = 0
open_suking_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 0
open_move_to_object_flag = 0
open_suking_flag_desire = 0
time.sleep(7)
#
# # if (tile_nums-1)<3:
#
# q_new_pub_d = F0.move_ur_to_desire(x_distance, q_before_sucking_d)
# q_after_sucking_d = q_new_pub_d
# MoveUrString = F0.Move_ur(q_new_pub_d, 0.2, vel, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(6)
# # if (tile_nums-1)==2:
# # # x_distance = 0.29
# # # y_distcane= -0.355
# # new_q_temp=F0.Rotaion_tool_90(q_after_sucking_d,-90)
# # q_new_pub_d = F0.move_ur_to_desire(x_distance, new_q_temp)
# # q_after_sucking_d = q_new_pub_d
# # MoveUrString = F0.Move_ur(q_new_pub_d, 0.2, vel, urt)
# # ur_pub.publish(MoveUrString)
# # time.sleep(6)
#
# # q_before_sucking_d = []
# """
# Second,close air pump for sucking
# """
# if open_suking_flag_desire==1:
# F0.Open_sucking_close_Ardunio()
# open_suking_flag_desire=0
# """
# Third,go back the same point
# """
# MoveUrString=F0.Move_ur(q_before_sucking_d, ace, 0.2, urt)
# ur_pub.publish(MoveUrString)
# time.sleep(1)
# open_move_to_object_flag=1
# if open_move_to_object_flag==1:
# """
# Fourth,move ur to desire position.
# """
# print "#########Desire,--Fourth,move ur to object position-------"
# # x_distance = 0.35
# # q_new_pub=F0.move_ur_to_desire(x_distance,q_now)
# MoveUrString_2=F0.Move_ur(Object_joint_angular_sucking_state_new, ace, 0.3, urt)
# ur_pub.publish(MoveUrString_2)
# open_vison_flag = 0
# open_suking_flag = 0
# open_move_to_desire_flag=0
# open_vison_flag_desire=0
# open_move_to_object_flag=0
# F0.tile_0_buf=[]
# q_now=[]
# time.sleep(7)
#
# """
# close all flags
# """
# if tile_nums >= 3:
# tile_nums = 0
if tile_nums == 4:
tile_nums = 0
# else:
# print "UR5 is Not Ok,Please check"
rate.sleep()
except KeyboardInterrupt:
# sys.exit()
pass
def main():
#uvlist=[123.0,112.0]
uvlist = []
camf = 624.0429 * 1e-03
#316,251
# uvcentral_place=[316,251]
uvcentral_place = [320, 201]
uvcentral = [370, 105] #sucking central109-125
First_joint_angular = []
calibinfo = [
0.109982645426, 0.114476746567, -0.0415924235801, 0.249492772999,
0.523487628443, 0.239612281752, 0.778652691205
]
urdfname = "/data/ros/ur_ws_yue/src/tilling_robot/urdf/ur5.urdf"
cailename = "/data/ros/ur_ws_yue/src/tilling_robot/yaml/cam_500_logitech.yaml"
nodename = "tilling_vision_control"
tile_width = 0.01 #m
ace = 50
vel = 0.1
urt = 0
detat = 0.05
ratet = 30
z_distance = 0
lamda = 3.666666
# lamda = 7.066666
z = 0.45
ur_reader = Urposition()
ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)
u_error_pub = rospy.Publisher("/object_feature_u_error",
Float64,
queue_size=10)
v_error_pub = rospy.Publisher("/object_feature_v_error",
Float64,
queue_size=10)
"""
Those two flag use to make sucker sucking tile
"""
object_flag = 0
open_vison_flag = 0
desire_flag = 0
turn_on_vision_flag = 0
"""
nodename,urdfname,delta,kappa,lamda,califilename,camf)
"""
F0 = TilingVisionControl(nodename, urdfname, detat, lamda, cailename, camf)
ur_pub, sucker_center_obj_pub, sucker_center_desire_pub = F0.Init_node()
rate = rospy.Rate(ratet)
First_joint_angular = ur_reader.ave_ur_pose
Object_joint_angular_vision_state = []
Object_joint_angular_sucking_state = [
-110.18, -93.16, -92.82, -84.93, 86.68, 66.02
] #66.02
Object_joint_angular_sucking_state_new = F0.change_angle_to_pi(
Object_joint_angular_sucking_state)
Desire_joint_angular_vision_state = []
Desire_joint_angular_place_state = [
-219.61, -82.61, -128.57, 31.76, 86.31, 42.58
]
Desire_joint_angular_place_state_new = F0.change_angle_to_pi(
Desire_joint_angular_place_state)
print "First_joint_angular", First_joint_angular
count_for_tile = 0
open_suking_flag = 0
open_roation_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 0
open_suking_flag_desire = 0
open_move_to_object_flag = 0
tile_nums = 0 #为了记录吸第几块砖
msg_uv_o = uv()
msg_uv_d = uv()
while not rospy.is_shutdown():
desire_tile_path_cacu = []
try:
"""
1,通过object_ibvs_id控制视觉伺服
"""
"""
First,Go to object position,just need opreating tile 1,
Now UV also is [316,251]
"""
msg_uv_o.uvinfo = uvcentral
msg_uv_d.uvinfo = uvcentral_place
sucker_center_obj_pub.publish(msg_uv_o)
sucker_center_desire_pub.publish(msg_uv_d)
q_now = ur_reader.ave_ur_pose
print "q_now", q_now
if len(q_now) != 0:
if len(F0.tile_0_buf) != 0 and open_vison_flag == 0:
print "Go to object position--------------"
uvm = [uvcentral]
xp = F0.tile_0_buf[-1][1] #目标位置
q_pub_now = F0.ibvs_run_ur5(uvm, z, q_now, calibinfo, xp)
MoveUrString = F0.Move_ur(q_pub_now, ace, vel, urt)
ur_pub.publish(MoveUrString)
time.sleep(0.05)
feature_error_zero_flag = F0.check_vison_error_is_zero(
F0.tile_0_buf[-1][1], uvcentral, 5)
"""
if error is zero ,we close vision servo
"""
if feature_error_zero_flag:
open_vison_flag = 1
open_suking_flag = 1
print "F0.tile_0_buf", F0.tile_0_buf[-1]
"""
第二步，关闭视觉伺服，打开轨迹规划，并吸瓷砖
"""
if open_vison_flag == 1 and open_suking_flag == 1:
"""
First,go to sucking tile
"""
print "-------------First,go to sucking tile------------"
z_distance = 0.13
q_before_sucking = q_now
q_new_pub = F0.move_sucker_to_tile(z_distance, tile_width,
q_now)
MoveUrString = F0.Move_ur(q_new_pub, ace, 0.2, urt)
ur_pub.publish(MoveUrString)
time.sleep(1.5)
"""
Second,Open air pump for sucking
"""
if open_suking_flag == 1:
F0.Open_sucking_close_Ardunio()
"""
Third,go back the same point
"""
print "-------Third,go back the same point---------"
MoveUrString = F0.Move_ur(q_before_sucking, ace, 0.2,
urt)
ur_pub.publish(MoveUrString)
time.sleep(1)
open_move_to_desire_flag = 1
if open_move_to_desire_flag == 1:
"""
Fourth,move ur to desire position.
"""
print "------Fourth,move ur to desire position--------"
# x_distance = 0.35
# q_new_pub=F0.move_ur_to_desire(x_distance,q_now)
MoveUrString = F0.Move_ur(
Desire_joint_angular_place_state_new, ace, 0.3, urt)
ur_pub.publish(MoveUrString)
open_vison_flag = 1
open_suking_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 1
F0.tile_0_buf = []
time.sleep(4)
"""
close all flags
"""
"""
第三步，打开目标空间的视觉伺服程序
"""
if len(F0.tile_0_buf) != 0 and open_vison_flag_desire == 1:
# if len(F0.tile_0_buf)!=0 and open_vison_flag_desire ==1:
uvm = [uvcentral_place]
print "第三步，打开目标空间的视觉伺服程序"
print "-------目标空间贴第" + str(tile_nums) + "块砖-----"
xpp = F0.tile_0_buf[-1][1]
uv0 = xpp
desire_tile_path_cacu = F0.Caculate_desire_uv_for_place(
uv0, 2)
print "xpp=F0.tile_0_buf[-1][1]", F0.tile_0_buf[-1][1]
print "desire_tile_path_cacu[tile_nums]", desire_tile_path_cacu[
tile_nums]
# time.sleep(1)
q_pub_now_d = F0.ibvs_run_ur5(
uvm, z, q_now, calibinfo,
desire_tile_path_cacu[tile_nums])
MoveUrString_1 = F0.Move_ur(q_pub_now_d, ace, vel, urt)
ur_pub.publish(MoveUrString_1)
feature_error_zero_flag_d = F0.check_vison_error_is_zero(
desire_tile_path_cacu[tile_nums], uvcentral_place, 3)
"""
if error is zero ,we close vision servo
"""
if feature_error_zero_flag_d:
open_suking_flag_desire = 1
open_vison_flag_desire = 1
tile_nums += 1
time.sleep(0.5)
desire_tile_path_cacu = []
print "---------you are now in desire space-----------"
"""
第四步，关闭视觉伺服，打开轨迹规划，并吸瓷砖
"""
if open_vison_flag_desire == 1 and open_suking_flag_desire == 1:
"""
First,go to sucking tile
"""
print "第四步，关闭视觉伺服，打开轨迹规划，并释放瓷砖"
q_before_sucking_d = q_now
print "tile_nums------>>>>>>>", tile_nums
if (tile_nums - 1) == 0: #1
x_distance = 0.46
y_distcane = -0.375
elif (tile_nums - 1) == 1: #2
x_distance = 0.38
y_distcane = -0.42
elif (tile_nums - 1) == 2:
x_distance = 0.29
y_distcane = -0.355
q_new_pub_d = F0.move_ur_to_desire_vertical(
x_distance, y_distcane, q_before_sucking_d)
q_after_sucking_d = q_new_pub_d
MoveUrString = F0.Move_ur(q_new_pub_d, ace, vel, urt)
ur_pub.publish(MoveUrString)
time.sleep(6)
# q_before_sucking_d = []
"""
Second,close air pump for sucking
"""
if open_suking_flag_desire == 1:
F0.Open_sucking_close_Ardunio()
open_suking_flag_desire = 0
"""
Third,go back the same point
"""
MoveUrString = F0.Move_ur(q_before_sucking_d, ace, 0.2,
urt)
ur_pub.publish(MoveUrString)
time.sleep(1)
open_move_to_object_flag = 1
if open_move_to_object_flag == 1:
"""
Fourth,move ur to desire position.
"""
print "#########Desire,--Fourth,move ur to object position-------"
# x_distance = 0.35
# q_new_pub=F0.move_ur_to_desire(x_distance,q_now)
MoveUrString_2 = F0.Move_ur(
Object_joint_angular_sucking_state_new, ace, 0.3, urt)
ur_pub.publish(MoveUrString_2)
open_vison_flag = 0
open_suking_flag = 0
open_move_to_desire_flag = 0
open_vison_flag_desire = 0
open_move_to_object_flag = 0
F0.tile_0_buf = []
q_now = []
time.sleep(7)
"""
close all flags
"""
if tile_nums >= 3:
tile_nums = 0
else:
print "UR5 is Not Ok,Please check"
rate.sleep()
except KeyboardInterrupt:
sys.exit()
