diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz index 9d7aa67..63353f1 100755 --- a/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz +++ b/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz @@ -7,7 +7,7 @@ Panels: - /TF1/Frames1 - /PointStamped1 Splitter Ratio: 0.594406008720398 - Tree Height: 210 + Tree Height: 341 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -23,7 +23,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan @@ -159,14 +158,14 @@ Visualization Manager: camera_bottom_screw_frame: camera_link: camera_aligned_depth_to_color_frame: - camera_color_optical_frame: - {} + {} camera_aligned_depth_to_infra1_frame: camera_infra1_optical_frame: {} - camera_color_frame: - {} camera_depth_frame: + camera_color_frame: + camera_color_optical_frame: + {} camera_depth_optical_frame: {} camera_left_ir_frame: @@ -521,7 +520,7 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b80000003efc0100000002fb0000000800540069006d00650100000000000003b8000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002480000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004180000003efc0100000002fb0000000800540069006d0065010000000000000418000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a80000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -530,6 +529,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 952 - X: 72 - Y: 27 + Width: 1048 + X: -164 + Y: 56 diff --git a/src/3rd_app/move2grasp/scripts/grasp_d435.py b/src/3rd_app/move2grasp/scripts/grasp_d435.py index 3dcabbb..7205442 100755 --- a/src/3rd_app/move2grasp/scripts/grasp_d435.py +++ b/src/3rd_app/move2grasp/scripts/grasp_d435.py @@ -32,12 +32,13 @@ class GraspObject(): 初始化 ''' - global xc, yc, xc_prev, yc_prev, found_count + global xc, yc, xc_prev, yc_prev, found_count, height xc = 0 yc = 0 xc_prev = xc yc_prev = yc found_count = 0 + height = 0 self.is_found_object = False # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1) # 订阅机械臂抓取指令 @@ -94,6 +95,23 @@ class GraspObject(): self.grasp_status_pub.publish(status) if msg.data=='0': # 放下物体 + height = 1 + self.is_found_object = False + self.release_object() + status=String() + status.data='0' + self.grasp_status_pub.publish(status) + if msg.data=='2': + # 放下物体 + height = 2 + self.is_found_object = False + self.release_object() + status=String() + status.data='0' + self.grasp_status_pub.publish(status) + if msg.data=='3': + # 放下物体 + height = 3 self.is_found_object = False self.release_object() status=String() @@ -127,19 +145,19 @@ class GraspObject(): r2 = rospy.Rate(10) pos = position() # 物体所在坐标+标定误差 - pos.x = a[0] * yc + a[1] - pos.y = b[0] * xc + b[1] + pos.x = round(a[0] * yc + a[1],2) + pos.y = round(b[0] * xc + b[1],2) pos.z = 20 - # pos.z = 20 - print("z = 20\n") + # # pos.z = 20 + # print("z = 20\n") self.pub1.publish(pos) r2.sleep() # go down -100 - pos.z = -70 + pos.z = -60 self.pub1.publish(pos) print("z = -83\n") r2.sleep() - + print((pos.x,pos.y)) # 开始吸取物体 self.pub2.publish(1) r2.sleep() @@ -163,7 +181,7 @@ class GraspObject(): cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV) # 蓝色物体颜色检测范围 - LowerBlue = np.array([100, 100, 140]) + LowerBlue = np.array([90, 95, 120]) UpperBlue = np.array([140, 255, 255]) # LowerBlue = np.array([4, 103, 162]) # UpperBlue = np.array([85, 136, 181]) @@ -185,7 +203,7 @@ class GraspObject(): # cv2.imshow("win1", cv_image1) # cv2.imshow("win2", cv_image5) # cv2.waitKey(1) - contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + contours, hier = cv2.findContours(cv_image4, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # if find contours, pick the biggest box if len(contours) > 0: @@ -229,14 +247,19 @@ class GraspObject(): # go forward pos.x = 200 pos.y = 0 - pos.z = -40 #-80 + if height == 0: + pos.z = -40 + elif height ==1: + pos.z = -40 + elif height == 2: + pos.z = 55 + elif height ==3: + pos.z = 150 #-80 self.pub1.publish(pos) r1.sleep() - # stop pump self.pub2.publish(0) - # r2.sleep() - #r1.sleep() + r1.sleep(0.1) pos.x = 120 pos.y = 0 pos.z = 35 diff --git a/src/3rd_app/move2grasp/scripts/teleop.py b/src/3rd_app/move2grasp/scripts/teleop.py index b58b9fc..fc5643a 100755 --- a/src/3rd_app/move2grasp/scripts/teleop.py +++ b/src/3rd_app/move2grasp/scripts/teleop.py @@ -68,7 +68,7 @@ def keyboardLoop(): finally : termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) # ch代表获取的键盘按键 - if ch == 'g': + if ch == 'e': if can_grasp: msg=String() msg.data='1' @@ -76,7 +76,7 @@ def keyboardLoop(): can_grasp=False speed = 0 turn = 0 - elif ch == 'h': + elif ch == '1': if can_release: msg=String() msg.data='0' @@ -84,6 +84,23 @@ def keyboardLoop(): can_release=False speed = 0 turn = 0 + elif ch == '2': + if can_release: + msg=String() + msg.data='2' + grasp_pub.publish(msg) + can_release=False + speed = 0 + turn = 0 + elif ch == '3': + if can_release: + msg=String() + msg.data='3' + grasp_pub.publish(msg) + can_release=False + speed = 0 + turn = 0 + elif ch == 'z': if can_release: msg=String()