20221021 auto

src/3rd_app/move2grasp/README.md

@@ -10,5 +10,13 @@ roslaunch move2grasp move2grasp.launch
 
 
 
+# Ver.full
+
+建议参考代码而不是直接使用
+
+需要使用直接覆盖到原位置，然后使用方式与标准代码一致
+
+**！！！覆盖前先备份好原有的代码**
+
 
 

src/3rd_app/move2grasp/launch/move2grasp.launch

@@ -44,14 +44,14 @@
   	</group>
 
 
-        <node pkg="move2grasp" type="move.py" name="move" />
+        <node pkg="move2grasp" type="move.py" name="move" output="screen"/>
 	<group if="$(eval arg('camera_type_tel')=='d435')">
 		<node pkg="move2grasp" type="grasp_d435.py" name="grasp" output="screen">
 			<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
 		</node>
 	</group>
 	<group if="$(eval arg('camera_type_tel')=='astrapro')">
-		<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" />
+		<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
 	</group>
 
 </launch>

src/3rd_app/move2grasp/launch/teleop2grasp.launch

@@ -5,9 +5,9 @@
   	<arg name="slam_methods" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
   	<arg name="configuration_basename" default="spark_lds_2d.lua"/>
   	<arg name="open_rviz" default="true"/>
-	<arg name="lidar_type_tel" default="3iroboticslidar2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
+	<arg name="lidar_type_tel" default="ydlidar_g2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
 	<!-- 摄像机类型 -->
-	<arg name="camera_type_tel" default="astrapro" doc="camera type [astrapro, astra, d435...]"/>
+	<arg name="camera_type_tel" default="d435" doc="camera type [astrapro, astra, d435...]"/>
 
 
 	<!--spark底盘驱动，机器人描述,底盘,相机-->
@@ -49,7 +49,7 @@
 
 	<node pkg="move2grasp" type="teleop.py" name="teleop" launch-prefix="xterm -e" />
 	<group if="$(eval arg('camera_type_tel')=='d435')">
-		<node pkg="move2grasp" type="grasp_d435.py" name="grasp" output="screen">
+		<node pkg="move2grasp" type="grasp_best.py" name="grasp" output="screen">
 			<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
 		</node>
 	</group>

src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz

@@ -6,8 +6,8 @@ Panels:
       Expanded:
         - /TF1/Frames1
         - /PointStamped1
-      Splitter Ratio: 0.594406008720398
-    Tree Height: 341
+      Splitter Ratio: 0.594406009
+    Tree Height: 112
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -16,18 +16,17 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
+    Splitter Ratio: 0.588679016
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
+    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: LaserScan
-Preferences:
-  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -39,7 +38,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.029999999329447746
+        Line Width: 0.0299999993
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -122,8 +121,6 @@ Visualization Manager:
           Value: false
         ir_bumper_right_link:
           Value: false
-        laser_frame:
-          Value: true
         left_wheel_link:
           Value: false
         lidar_link:
@@ -138,7 +135,6 @@ Visualization Manager:
           Value: false
         spark_stack:
           Value: true
-      Marker Alpha: 1
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -158,14 +154,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_depth_frame:
                     camera_color_frame:
-                        camera_color_optical_frame:
                       {}
+                    camera_depth_frame:
                       camera_depth_optical_frame:
                         {}
                       camera_left_ir_frame:
@@ -207,7 +203,6 @@ Visualization Manager:
                 left_wheel_link:
                   {}
                 lidar_link:
-                  laser_frame:
                   {}
                 right_wheel_link:
                   {}
@@ -232,13 +227,15 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 11799
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: LaserScan
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.029999999329447746
+      Size (m): 0.0299999993
       Style: Flat Squares
       Topic: /scan
       Unreliable: false
@@ -257,7 +254,7 @@ Visualization Manager:
       Transport Hint: compressed
       Unreliable: false
       Value: true
-    - Alpha: 0.699999988079071
+    - Alpha: 0.699999988
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
@@ -390,10 +387,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        laser_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         left_wheel_link:
           Alpha: 1
           Show Axes: false
@@ -403,7 +396,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         right_wheel_link:
           Alpha: 1
           Show Axes: false
@@ -428,8 +420,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 1.3373792171478271
-        Min Value: 0.17965421080589294
+        Max Value: 1.33737922
+        Min Value: 0.179654211
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -440,13 +432,15 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
+      Max Intensity: 4096
       Min Color: 0; 0; 0
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.009999999776482582
+      Size (m): 0.00999999978
       Style: Flat Squares
       Topic: /camera/depth/points
       Unreliable: false
@@ -459,8 +453,7 @@ Visualization Manager:
       Enabled: true
       History Length: 1
       Name: PointStamped
-      Queue Size: 10
-      Radius: 0.20000000298023224
+      Radius: 0.200000003
       Topic: /clicked_point
       Unreliable: false
       Value: true
@@ -479,10 +472,7 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -492,35 +482,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 11.24269962310791
+      Distance: 11.2426996
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
+        Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Field of View: 0.7853981852531433
       Focal Point:
-        X: 9.536739753457368e-07
-        Y: 4.768369876728684e-07
-        Z: -4.768369876728684e-07
+        X: 9.53673975e-07
+        Y: 4.76836988e-07
+        Z: -4.76836988e-07
       Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
+      Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4803990423679352
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.480399042
       Target Frame: map
-      Yaw: 1.8854000568389893
+      Value: ThirdPersonFollower (rviz)
+      Yaw: 1.88540006
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 536
+  Height: 576
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004180000003efc0100000002fb0000000800540069006d0065010000000000000418000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a80000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b6fc020000000afb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000012d000000b10000001600fffffffb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000001b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -529,6 +519,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1048
-  X: -164
-  Y: 56
+  Width: 959
+  X: 65
+  Y: 24

src/3rd_app/move2grasp/scripts/grasp_best.py

@@ -180,7 +180,7 @@ class GraspObject():
 
     def grasp_cp(self, msg):
         # 判断收到的抓取或放的信息
-        if msg.data == '1':
+        if msg.data == '0':
             print("收到抓取信号")
             times=0
             steps=0
@@ -233,7 +233,7 @@ class GraspObject():
                 # 抓取不成功，将允许重新抓取
                 self.can_grasp = True
  
-        elif msg.data=='0':
+        elif msg.data=='1':
             # 放下物体
             self.is_found_object = False
             self.release_object(1)

src/3rd_app/move2grasp/scripts/grasp_d435.py

@@ -1,46 +1,51 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 
+from copy import deepcopy
 import os
-import time
-import random
-import ctypes
-import roslib
 import rospy
-import smach
-import smach_ros
-import threading
-import string
 import math
+import cmath
 import cv2
 import numpy as np
 from geometry_msgs.msg import Twist
 from std_msgs.msg import String
-from geometry_msgs.msg import Pose, Point, Quaternion
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import Image,LaserScan
 from cv_bridge import CvBridge, CvBridgeError
 from spark_carry_object.msg import *
+from matplotlib import pyplot as plt
 
 
 class GraspObject():
     '''
     监听主控，用于物品抓取功能
     '''
+    image_last_time = None
 
     def __init__(self):
         '''
         初始化
         '''
-
-        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.xc = 0
+        self.yc = 0
+        self.xc_prev = 0
+        self.yc_prev = 0
+        self.found_count = 0
+        self.is_have_object = False
         self.is_found_object = False
-        # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+        self.object_union = []
+        self.last_object_union = []
+
+        filename = os.environ['HOME'] + "/thefile.txt"
+        with open(filename, 'r') as f:
+            s = f.read()
+        arr = s.split()
+        self.x_kb = [float(arr[0]), float(arr[1])]
+        self.y_kb = [float(arr[2]), float(arr[3])]
+        rospy.logwarn('X axia k and b value: ' + str(self.x_kb))
+        rospy.logwarn('X axia k and b value: ' + str(self.y_kb))
+
+        # self.sub = rospy.Subscriber("/camera/color/image_raw", Image, self.image_cb, queue_size=1)
         # 订阅机械臂抓取指令
         self.sub2 = rospy.Subscriber(
             '/grasp', String, self.grasp_cp, queue_size=1)
@@ -54,260 +59,358 @@ class GraspObject():
             'grasp_status', String, queue_size=1)
         # 发布TWist消息控制机器人底盘
         self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
-        pos = position()
-        pos.x = 120
-        pos.y = 0
-        pos.z = 35
-        self.pub1.publish(pos)
+        self.arm_to_home()
 
-    def grasp_cp(self, msg):
-        global height
-        if msg.data == '1':
-            # 订阅摄像头话题,对图像信息进行处理
-            self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
-            self.is_found_object = False
-            rate = rospy.Rate(10)
-            times=0
-            steps=0
-            while not self.is_found_object:
-                rate.sleep()
-                times+=1
-                # 转一圈没有发现可抓取物体,退出抓取
-                if steps>=5:
-                    self.sub.unregister()
-                    print("stop grasp\n")
-                    status=String()
-                    status.data='-1'
-                    self.grasp_status_pub.publish(status)
-                    return
-                # 旋转一定角度扫描是否有可供抓取的物体
-                if times>=30:
-                    times=0
-                    steps+=1
-                    self.turn_body()
-                    print("not found\n")
-            print("unregisting sub\n")
-            self.sub.unregister()
-            print("unregisted sub\n")
-            # 抓取检测到的物体    
-            self.grasp()
-            status=String()
-            status.data='1'
-            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()
-            status.data='0'
-            self.grasp_status_pub.publish(status) 
+    def hsv_fliter(self, img):
+        # change rgb to hsv
+        cv_img2 = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
 
-    # 执行抓取
-    def grasp(self):
-        print("start to grasp\n")
-        global xc, yc, found_count
-        # stop function
+        # 蓝色物体颜色检测范围
+        LowerBlue = np.array([95, 90, 80])
+        UpperBlue = np.array([130, 255, 255])
+        mask = cv2.inRange(cv_img2, LowerBlue, UpperBlue)
+        cv_img3 = cv2.bitwise_and(cv_img2, cv_img2, mask=mask)
 
-        filename = os.environ['HOME'] + "/thefile.txt"
-        file_pix = open(filename, 'r')
-        s = file_pix.read()
-        file_pix.close()
-        print(s)
-        arr=s.split()
-        a1=arr[0]
-        a2=arr[1]
-        a3=arr[2]
-        a4=arr[3]
-        a = [0]*2
-        b = [0]*2
-        a[0]=float(a1)
-        a[1]=float(a2)
-        b[0]=float(a3)
-        b[1]=float(a4)
-        print('k and b value:',a[0],a[1],b[0],b[1])
-        r1 = rospy.Rate(0.095)
-        r2 = rospy.Rate(10)
-        pos = position()
-        # 物体所在坐标+标定误差
-        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")
-        self.pub1.publish(pos)
-        r2.sleep()
-        # go down -100
-        pos.z = -60
-        self.pub1.publish(pos)
-        print("z = -83\n")
-        r2.sleep()
-        print((pos.x,pos.y))
-        # 开始吸取物体
-        self.pub2.publish(1)
-        print('nmmsl')
-        r2.sleep()
+        # gray process
+        cv_img4 = cv_img3[:, :, 0]
+
+        # smooth and clean noise
+        blurred = cv2.blur(cv_img4, (9, 9))
+        (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        cv_img_ret = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
+        cv_img_ret = cv2.erode(cv_img_ret, None, iterations=4)
+        cv_img_ret = cv2.dilate(cv_img_ret, None, iterations=4)
+        return cv_img_ret
 
-        # 提起物体
-        pos.x = 250  #160
-        pos.y = 0
-        pos.z = 170  #55
-        self.pub1.publish(pos)
-        r1.sleep()
     # 使用CV检测物体       
     def image_cb(self, data):
-        global xc, yc, xc_prev, yc_prev, found_count
+        xc = -1
+        yc = -1
         # change to opencv
         try:
             cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
-            ht = len(cv_image1)
-            wit= len(cv_image1[0])
-            cv_image1[0:ht//3,0:wit]=[0,0,0]
         except CvBridgeError as e:
             print('error')
 
-        # change rgb to hsv
-        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
+        cv_image5 = self.hsv_fliter(cv_image1)
 
-        # 蓝色物体颜色检测范围
-        LowerBlue = np.array([95, 90, 130])
-        UpperBlue = np.array([130, 255, 255])
-        # LowerBlue = np.array([4, 103, 162])
-        # UpperBlue = np.array([85, 136, 181])
-        mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
-        cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
+        contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
 
-        # gray process
-        cv_image4 = cv_image3[:, :, 0]
-
-        # smooth and clean noise
-        blurred = cv2.blur(cv_image4, (9, 9))
-        (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
-        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
-        cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
-        cv_image5 = cv2.erode(cv_image5, None, iterations=4)
-        cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
-
-        # detect contour
-        # cv2.imshow("win1", cv_image1)
-        # cv2.imshow("win2", cv_image5)
-        # cv2.waitKey(1)
-        
-        contours, hier = cv2.findContours(cv_image4, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
 
         # if find contours, pick the biggest box
-        if len(contours) > 0:
-            size = []
-            size_max = 0
+        if len(contours) == 0:
+            self.is_have_object = False
+            self.is_found_object = False
+            self.found_count = 0
+        else:
+            self.is_have_object = True
+            index = -1
+            p_min = 10000
+            self.object_union.clear()
             for i, c in enumerate(contours):
                 rect = cv2.minAreaRect(c)
                 box = cv2.boxPoints(rect)
                 box = np.int0(box)
-                # if abs(y_mid) > 250 or abs(x_mid)>270:
-                #     continue
+                cv2.drawContours(cv_image1, [box], 0, (0, 255, 0), 2)
+
+                x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
+                y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
+
                 w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
                 h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2)
 
-                if w>h:
-                    tmp = h
-                    h = w
-                    w = tmp
-                if  w !=0 and h/w >=1.3:
-                    Ph0MidX = (box[0][0]+box[3][0])/2
-                    Ph0MidY = (box[0][1]+box[1][1])/2
-                    Ph1MidX = (box[1][0]+box[2][0])/2
-                    Ph1MidY = (box[1][1]+box[2][1])/2
-                    x_mid = (Ph0MidX+Ph1MidX+box[0][0]+box[1][0])/4
-                    y_mid = (Ph0MidY+Ph1MidY+box[0][1]+box[1][1])/4
-                    size.append(w * h/2)
-                else:
-                    x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
-                    y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
-                    size.append(w * h)
-                # cv2.circle(pic, (int(x_mid),int(y_mid)), 3, (0, 0, 255), 3)
-                if size[i] > size_max:
-                    size_max = size[i]
-                    index = i
+                size = w * h
+                # if size[i] > size_max:
+                #     size_max = size[i]
+                #     index = i
+                #     self.xc = x_mid
+                #     self.yc = y_mid
+
+                p, theta = cmath.polar(complex(x_mid - 320, 480 - y_mid))
+                cn = cmath.rect(p, theta)
+                              
+                if p > 350:
+                    continue
+
+                self.object_union.append((p, theta, w, h, size, x_mid, y_mid))
+                
+                if p < p_min:
+                    index = index + 1
+                    p_min = p
                     xc = x_mid
                     yc = y_mid
+            self.object_union.sort(key=lambda x:x[0]) # 按抓取长度小到大排序
+                    
             # if box is not moving for 20 times
             # print found_count
-            if found_count >= 30:
+            if self.found_count >= 30:
+                self.found_count = 0
                 self.is_found_object = True
-                cmd_vel = Twist()
-                self.cmd_vel_pub.publish(cmd_vel)
+                self.xc = xc
+                self.yc = yc
             else:
                 # if box is not moving
-                if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
-                    found_count = found_count + 1
+                if index == -1:
+                    # rospy.logwarn("No object eligible for grasp")
+                    self.found_count = 0
+                    self.is_found_object = False
+                elif abs(xc - self.xc_prev) <= 8 and abs(yc - self.yc_prev) <= 8:
+                    # cn = cmath.rect(self.object_union['p'][index], self.object_union['theta'][index])
+                    # cv2.line(cv_image1, (320, 480), (int(320 + cn.real), int(480 - cn.imag)), (255, 0, 0), 2)
+                    self.found_count = self.found_count + 1
                 else:
-                    found_count = 0
-        else:
-            found_count = 0
-        xc_prev = xc
-        yc_prev = yc
+                    self.found_count = 0
+        self.xc_prev = xc
+        self.yc_prev = yc
         
-        # cv2.imshow("source", cv_image1)
+        cv2.imshow("source", cv_image1)
+        # cv2.imshow("gray", cv_image5)
+        cv2.waitKey(1)
+
+        self.image_last_time = rospy.get_rostime()
+
+    def grasp_cp(self, msg):
+        if msg.data == '1':
+            # 订阅摄像头话题,对图像信息进行处理
+            self.sub = rospy.Subscriber("/camera/color/image_raw", Image, self.image_cb, queue_size=1)
+            self.is_found_object = False
+            times=0
+            state=0
+            rospy.sleep(1) # 等待1秒在当前画面寻找物体
+            # 转一圈没有发现可抓取物体,退出抓取
+            rospy.loginfo("-----search object")
+            vel = Twist()
+            vel.angular.z = 0.2
+            rate = rospy.Rate(10)
+            if not self.is_have_object:
+                while not self.is_have_object:
+                    rate.sleep()
+                    times = times + 1
+                    if (times > math.pi * 2 / vel.angular.z * 10):
+                        rospy.logerr("can not found object!!! Waiting next clicked_point...")
+                        self.grasp_status_pub.publish(String('0'))
+                        state = -1
+                        break
+                    self.cmd_vel_pub.publish(vel)
+
+            if (state != -1):
+                times = 0
+                rospy.loginfo("-----locate object")
+                rospy.sleep(0.5)
+                self.is_found_object = False
+                while not self.is_found_object:
+                    rate.sleep()
+                    times = times + 1
+                    if (times > 60):
+                        rospy.logerr("object founded but can not location it!!! Waiting next clicked_point...")
+                        self.grasp_status_pub.publish(String('0'))
+                        state = -1
+                        break
+
+            if (state != -1):
+                # 抓取检测到的物体    
+                rospy.loginfo("-----start to grasp")
+                self.last_object_union = deepcopy(self.object_union)
+                grasp_retry_num = len(self.last_object_union)
+                for i in range(grasp_retry_num):
+                    result = self.grasp(self.last_object_union[i][5], self.last_object_union[i][6])
+                    if result:
+                        self.grasp_status_pub.publish(String('1')) 
+                        break
+                    elif i >= 2 or i == grasp_retry_num - 1:
+                        self.grasp_status_pub.publish(String('0')) 
+                        break
+            else:
+                self.grasp_status_pub.publish(String('0'))
+            self.sub.unregister()
+            rospy.loginfo("unregisted sub")
+
+            # 几率性会报非 ROS 的错误，待解决
+            # cv2.destroyAllWindows()
             # cv2.waitKey(1)
 
-        # self.image_last_time = rospy.get_rostime()
+        if msg.data=='0':
+            # 放下物体
+            rospy.loginfo("-----start to release")
+            self.release_object()
+            rospy.loginfo("last_object_union_lenght: %d" % (len(self.last_object_union)))
+            rospy.loginfo("last_object_union: " + str(self.last_object_union))
+            if len(self.last_object_union) > 1:
+                self.grasp_status_pub.publish(String('3'))
+            else:
+                self.grasp_status_pub.publish(String('2'))
 
 
-    # 释放物体
-    def release_object(self):
-        r1 = rospy.Rate(0.15)  # 5s
-        r2 = rospy.Rate(1)     # 1s
+        
+
+    # 执行抓取
+    def grasp(self, xc, yc):
+        r1 = rospy.Rate(0.095)
+        r2 = rospy.Rate(10)
+        r3 = rospy.Rate(1)
         pos = position()
-        # go forward
-        pos.x = 200
+
+        # 物体所在坐标+标定误差
+        pos.x = self.x_kb[0] * yc + self.x_kb[1]
+        pos.y = self.y_kb[0] * xc + self.y_kb[1]
+        pos.z = 20
+
+	    # pos.z = 20
+        rospy.loginfo("to z = 20")
+        self.pub1.publish(pos)
+        r2.sleep()
+
+        # go down -100
+        pos.z = -50
+        self.pub1.publish(pos)
+        rospy.loginfo("to z = -83")
+        r2.sleep()
+
+        # 开始吸取物体
+        rospy.loginfo("opening pump...")
+        self.pub2.publish(1)
+        r2.sleep()
+
+        # 提起物体到检查位置
+        pos.x = 250  #160
         pos.y = 0
-        if height == 0:
-            pos.z = -40
+        pos.z = 0
+        rospy.loginfo("to check point")
         self.pub1.publish(pos)
-        elif height ==1:
-            pos.z = -40
+        r1.sleep()
+
+        # 利用雷达检查是否有方块
+        if self.check_grasp_state():
+            rospy.loginfo("Grasp !!!")
+            pos.x = 250  #160
+            pos.y = 0
+            pos.z = 150
             self.pub1.publish(pos)
-        elif height == 2:
-            pos.z = 80
-            self.pub1.publish(pos)
-        elif height ==3:
-            print('nmsl')
+            r3.sleep()
+            return True
+        else:
+            rospy.loginfo("Grasp Fail... to home")
             self.pub2.publish(0)
-        time.sleep(2)
-        # stop pump
-        self.pub2.publish(0)
-        time.sleep(1)
-        # r1.sleep()
+            self.arm_to_home()
+            return False
+
+
+    def arm_to_home(self):
+        pos = position()
         pos.x = 120
         pos.y = 0
         pos.z = 35
         self.pub1.publish(pos)
+    
+
+     # 检查是否成功抓取，成功返回True，反之返回False
+    def check_grasp_state(self):
+        
+        self.success_grasp_num = 0
+        scan_num = 0
+        # 读十次雷达数据，10hz
+        while scan_num < 60:
+
+            scan_msg = rospy.wait_for_message("/scan",LaserScan,timeout=None)
+
+            # 计算激光束数量,num值为1817
+            num = int((scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment)
+
+            # 输入想要检测的角度与范围，机器人正后方为起始点
+            expect_angle = 180
+            ranges = 20
+            test_direction = int(expect_angle/360*num)
+
+            detect_num = 0 # 已经检测的次数
+            sum = 0 # 数据总和
+
+            # 计算检测区域的距离?
+            for i in range(test_direction-ranges,test_direction+ranges):
+                # 排除检测距离值有出现0值的情况
+                if scan_msg.ranges[i] != 0:
+                    sum = scan_msg.ranges[i]+sum
+                    detect_num +=1
+                    # print("scan_msg.ranges[",i,"]=",scan_msg.ranges[i])
+
+            # 避免检测距离值出现均导致计算报错
+            if detect_num == 0:
+                continue
+            
+            # 取距离抓取成功值约0.26—0.32,单位为米
+            averlaser = sum/detect_num
+            print("averlaser=",averlaser)
+
+            # 判断是否成功抓取
+            if averlaser < 0.35:
+                self.success_grasp_num += 1
+                # 判断成功抓取次数超过3次，返回true
+                if self.success_grasp_num >= 3:
+                    self.success_grasp_num = 0
+                    return True
+            print("scan_num=",scan_num)
+            scan_num += 1
+        return False
+        
+        
+
+    # 释放物体
+    def release_object(self, check=True):
+        r1 = rospy.Rate(0.15)  # 5s
+        r2 = rospy.Rate(1)     # 1s
+        pos = position()
+
+        if check:
+            # leave camera area
+            rospy.loginfo("leave camera area")
+            pos.x = 100
+            pos.y = -200
+            pos.z = 150  #-80
+            self.pub1.publish(pos)
             r1.sleep()
-        return 'succeeded'
+
+            # go forward
+            pos.x = 230
+            pos.y = 0
+            data = rospy.wait_for_message("/camera/color/image_raw", Image)
+            try:
+                cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
+            except CvBridgeError as e:
+                print('error')
+
+            cv_image5 = self.hsv_fliter(cv_image1)
+
+            pos.z = 70  #-80
+            self.pub1.publish(pos)
+            r1.sleep()
+            if np.mean(cv_image5[190:310, 290:390]) < 20: # 人工测量值
+                pos.z = -40 # L1
+                self.pub1.publish(pos)
+                r1.sleep()
+        else:
+            pos.x = 230
+            pos.y = 0
+            pos.z = 70
+            self.pub1.publish(pos)
+            r1.sleep()
+                
+        # stop pump
+        rospy.loginfo("stop pump")
+        self.pub2.publish(0)
+        r2.sleep()
+
+        rospy.loginfo("to home")
+        self.arm_to_home()
+        return True
+
     # 转动机器人到一定角度       
-    def turn_body(self):
-        cmd_vel = Twist()
-        cmd_vel.angular.z = 0.25
-        rate = rospy.Rate(10)
-        for i in range(40):            
-            self.cmd_vel_pub.publish(cmd_vel)            
-            rate.sleep()
+    # def turn_body(self):
+    #     cmd_vel = Twist()
+    #     cmd_vel.angular.z = 0.25
+    #     rate = rospy.Rate(10)
+    #     for i in range(40):            
+    #         self.cmd_vel_pub.publish(cmd_vel)            
+    #         rate.sleep()
        
 
         

src/3rd_app/move2grasp/scripts/grasp_pro_new.py

@@ -1,263 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-
-import os
-import time
-import random
-import ctypes
-import roslib
-import rospy
-import smach
-import smach_ros
-import threading
-import string
-import math
-import cv2
-import numpy as np
-from geometry_msgs.msg import Twist
-from std_msgs.msg import String
-from geometry_msgs.msg import Pose, Point, Quaternion
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge, CvBridgeError
-from spark_carry_object.msg import *
-
-
-class GraspObject():
-    '''
-    监听主控，用于物品抓取功能
-    '''
-
-    def __init__(self):
-        '''
-        初始化
-        '''
-
-        global xc, yc, xc_prev, yc_prev, found_count
-        xc = 0
-        yc = 0
-        xc_prev = xc
-        yc_prev = yc
-        found_count = 0
-        self.is_found_object = False
-        # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
-        # 订阅机械臂抓取指令
-        self.sub2 = rospy.Subscriber(
-            '/grasp', String, self.grasp_cp, queue_size=1)
-        # 发布机械臂位姿
-        self.pub1 = rospy.Publisher(
-            'position_write_topic', position, queue_size=10)
-        # 发布机械臂吸盘
-        self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
-        # 发布机械臂状态
-        self.grasp_status_pub = rospy.Publisher(
-            'grasp_status', String, queue_size=1)
-        # 发布TWist消息控制机器人底盘
-        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
-        pos = position()
-        pos.x = 120
-        pos.y = 0
-        pos.z = 35
-        self.pub1.publish(pos)
-
-    def grasp_cp(self, msg):
-        if msg.data == '1':
-            # 订阅摄像头话题,对图像信息进行处理
-            self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
-            self.is_found_object = False
-            rate = rospy.Rate(10)
-            times=0
-            steps=0
-            while not self.is_found_object:
-                rate.sleep()
-                times+=1
-                # 转一圈没有发现可抓取物体,退出抓取
-                if steps>=5:
-                    self.sub.unregister()
-                    print("stop grasp\n")
-                    status=String()
-                    status.data='-1'
-                    self.grasp_status_pub.publish(status)
-                    return
-                # 旋转一定角度扫描是否有可供抓取的物体
-                if times>=30:
-                    times=0
-                    steps+=1
-                    self.turn_body()
-                    print("not found\n")
-            print("unregisting sub\n")
-            self.sub.unregister()
-            print("unregisted sub\n")
-            # 抓取检测到的物体    
-            self.grasp()
-            status=String()
-            status.data='1'
-            self.grasp_status_pub.publish(status) 
-        if msg.data=='0':
-            # 放下物体
-            self.is_found_object = False
-            self.release_object()
-            status=String()
-            status.data='0'
-            self.grasp_status_pub.publish(status) 
-
-    # 执行抓取
-    def grasp(self):
-        print("start to grasp\n")
-        global xc, yc, found_count
-        # stop function
-
-        filename = os.environ['HOME'] + "/thefile.txt"
-        file_pix = open(filename, 'r')
-        s = file_pix.read()
-        file_pix.close()
-        print(s)
-        arr=s.split()
-        a1=arr[0]
-        a2=arr[1]
-        a3=arr[2]
-        a4=arr[3]
-        a = [0]*2
-        b = [0]*2
-        a[0]=float(a1)
-        a[1]=float(a2)
-        b[0]=float(a3)
-        b[1]=float(a4)
-        print('k and b value:',a[0],a[1],b[0],b[1])
-        r1 = rospy.Rate(0.095)
-        r2 = rospy.Rate(10)
-        pos = position()
-        # 物体所在坐标+标定误差
-        pos.x = a[0] * yc  + a[1]
-        pos.y = b[0] * xc + b[1]
-        pos.z = 20
-	    # pos.z = 20
-        print("z = 20\n")
-        self.pub1.publish(pos)
-        r2.sleep()
-        # go down -100
-        pos.z = -50
-        self.pub1.publish(pos)
-        print("z = -83\n")
-        r2.sleep()
-
-        # 开始吸取物体
-        self.pub2.publish(1)
-        r2.sleep()
-
-        # 提起物体
-        pos.x = 250  #160
-        pos.y = 0
-        pos.z = 150  #55
-        self.pub1.publish(pos)
-        r1.sleep()
-    # 使用CV检测物体       
-    def image_cb(self, data):
-        global xc, yc, xc_prev, yc_prev, found_count
-        # change to opencv
-        try:
-            cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
-        except CvBridgeError as e:
-            print('error')
-
-        # change rgb to hsv
-        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
-
-        # 蓝色物体颜色检测范围
-        LowerBlue = np.array([95, 70, 60])
-        UpperBlue = np.array([140, 255, 255])
-        mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
-        cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
-
-        # gray process
-        cv_image4 = cv_image3[:, :, 0]
-
-        # smooth and clean noise
-        blurred = cv2.blur(cv_image4, (9, 9))
-        (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
-        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
-        cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
-        cv_image5 = cv2.erode(cv_image5, None, iterations=4)
-        cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
-
-        # detect contour
-        # cv2.imshow("win1", cv_image3)
-        # cv2.imshow("win2", cv_image5)
-        cv2.waitKey(1)
-        contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
-
-        # if find contours, pick the biggest box
-        if len(contours) > 0:
-            size = []
-            size_max = 0
-            for i, c in enumerate(contours):
-                rect = cv2.minAreaRect(c)
-                box = cv2.boxPoints(rect)
-                box = np.int0(box)
-                x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
-                y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
-                w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
-                h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2) 
-                size.append(w * h)
-                if size[i] > size_max:
-                    size_max = size[i]
-                    index = i
-                    xc = x_mid
-                    yc = y_mid
-            # if box is not moving for 20 times
-            # print found_count
-            if found_count >= 30:
-                self.is_found_object = True
-                cmd_vel = Twist()
-                self.cmd_vel_pub.publish(cmd_vel)
-            else:
-                # if box is not moving
-                if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
-                    found_count = found_count + 1
-                else:
-                    found_count = 0
-        else:
-            found_count = 0
-        xc_prev = xc
-        yc_prev = yc
-    # 释放物体
-    def release_object(self):
-        r1 = rospy.Rate(0.15)  # 5s
-        r2 = rospy.Rate(1)     # 1s
-        pos = position()
-        # go forward
-        pos.x = 200
-        pos.y = 0
-        pos.z = -40  #-80
-        self.pub1.publish(pos)
-        r1.sleep()
-
-        # stop pump
-        self.pub2.publish(0)
-        r2.sleep()
-        #r1.sleep()
-        pos.x = 120
-        pos.y = 0
-        pos.z = 35
-        self.pub1.publish(pos)
-        r1.sleep()
-        return 'succeeded'
-    # 转动机器人到一定角度       
-    def turn_body(self):
-        cmd_vel = Twist()
-        cmd_vel.angular.z = 0.25
-        rate = rospy.Rate(10)
-        for i in range(40):            
-            self.cmd_vel_pub.publish(cmd_vel)            
-            rate.sleep()
-       
-
-        
-if __name__ == '__main__':
-    try:
-        rospy.init_node('GraspObject', anonymous=False)
-        rospy.loginfo("Init GraspObject main")   
-        GraspObject()
-        rospy.spin()
-    except rospy.ROSInterruptException:
-        rospy.loginfo("End spark GraspObject main")
-

src/3rd_app/move2grasp/scripts/move.py

@@ -1,28 +1,36 @@
 #!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
 
-from glob import glob
-from itertools import cycle
-from sre_constants import SUCCESS
 import rospy
 import actionlib
 from actionlib_msgs.msg import *
 from std_msgs.msg import String
 from geometry_msgs.msg import Pose, Point, Quaternion, Twist, PointStamped
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from tf.transformations import quaternion_from_euler  
+import tf
+import tf.transformations as tf_transformations
 from visualization_msgs.msg import Marker
 from math import radians, pi
+import numpy as np
+
+
 class Move2Grasp():
-    successCount = 0
+    STATE_PENDING = 0
+    STATE_GOING = 1
+    STATE_GRASP = 0
+    STATE_HOMING = 0
+    STATE_DROP = 0
     def __init__(self):
         rospy.init_node('move2grasp', anonymous=False)
 
+        rospy.loginfo("1211113242ewrqewr325235")
         rospy.on_shutdown(self.shutdown)
         # 订阅RVIZ上的点击事件
         rospy.Subscriber('clicked_point', PointStamped, self.cp_callback)
+        self.clicked_point_pub = rospy.Publisher('clicked_point', PointStamped, queue_size=1)
         # 订阅机械臂抓取状态
-        rospy.Subscriber('/grasp_status', String, self.grasp_status_cp, queue_size=1)
+        rospy.Subscriber('/grasp_status', String,
+                         self.grasp_status_cp, queue_size=1)
         # Publisher to manually control the robot (e.g. to stop it)
         # 发布TWist消息控制机器人
         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
@@ -31,7 +39,8 @@ class Move2Grasp():
 
         # Subscribe to the move_base action server
         # 订阅move_base服务器的消息
-        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
+        self.move_base = actionlib.SimpleActionClient(
+            "move_base", MoveBaseAction)
 
         rospy.loginfo("Waiting for move_base action server...")
 
@@ -40,11 +49,25 @@ class Move2Grasp():
         self.move_base.wait_for_server(rospy.Duration(60))
 
         rospy.loginfo("Connected to move base server")
-        rospy.loginfo("Starting navigation test")  
 
+        self.listener = tf.TransformListener()
+        rospy.sleep(1) # need to delay
+        self.base_position = self.get_currect_pose()
+        self.drop_position = np.dot(self.base_position, tf_transformations.compose_matrix(translate=[0.5, 0, 0], angles=[0, 0, pi]))
+        self.last_position = None
+        self.task_running = self.STATE_PENDING
+
+        rospy.loginfo("move node init ok...")
+
+    def get_currect_pose(self):
+        (target_trans, target_rot) = self.listener.lookupTransform("map", "base_footprint", rospy.Time(0))
+        return tf_transformations.compose_matrix(translate=target_trans, angles=tf_transformations.euler_from_quaternion(target_rot))
 
 
     def cp_callback(self, msg):
+        if (self.task_running == self.STATE_PENDING):
+            self.task_running = self.STATE_GOING
+            self.last_position = msg
             rospy.loginfo("POINT:%f,%f,%f", msg.point.x, msg.point.y, msg.point.z)
 
             # Intialize the waypoint goal
@@ -62,7 +85,9 @@ class Move2Grasp():
             # Set the goal pose
             # 设置目标点
             #pose = Pose(Point(0.0,0.0,0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
-            pose = Pose(Point(msg.point.x,msg.point.y,msg.point.z), Quaternion(0.0, 0.0, 0.0, 1.0))
+            quat = tf_transformations.quaternion_from_matrix(self.base_position)
+            pose = Pose(Point(msg.point.x, msg.point.y, msg.point.z),
+                        Quaternion(quat[0], quat[1], quat[2], quat[3]))
             goal.target_pose.pose = pose
             
             # Start the robot moving toward the goal
@@ -70,38 +95,73 @@ class Move2Grasp():
             status = self.move(goal)
             # 如果到达指定地点,就发送抓取指令
             if status == True:
-                print('goal reached and start grasp')
-                msg=String()
-                msg.data='1'
-                self.grasp_pub.publish(msg)
+                rospy.loginfo('goal reached and start grasp')
+                self.grasp_pub.publish(String('1'))
+                self.task_running = self.STATE_GRASP
+            else:
+                rospy.logerr("navigation failed!!! Waiting next clicked_point...")
+                self.task_running = self.STATE_PENDING
+        else: rospy.logwarn("task still running. ignore")
 
     def grasp_status_cp(self, msg):
+
+        # 抓取失败,任务重新开始
+        if msg.data == '0':
+            rospy.logerr("Grasp failed!!! Waiting next clicked_point...")
+            self.task_running = self.STATE_PENDING
+
         # 物体抓取成功,让机器人回起始点
-            if msg.data=='1': 
+        elif msg.data == '1':
+            self.task_running = self.STATE_HOMING
             goal = MoveBaseGoal()
-                self.successCount+=1
             goal.target_pose.header.frame_id = 'map'
             goal.target_pose.header.stamp = rospy.Time.now()
-                if self.successCount<=3:
-                    pose = Pose(Point(0.3,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
-                elif self.successCount <=6:
-                    pose = Pose(Point(0.4,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
-                elif self.successCount <=9:
-                    pose = Pose(Point(0.5,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
+            tran = tf_transformations.translation_from_matrix(self.drop_position)
+            quat = tf_transformations.quaternion_from_matrix(self.drop_position)
+            pose = Pose(Point(tran[0], tran[1], tran[2]), Quaternion(quat[0], quat[1], quat[2], quat[3]))
             goal.target_pose.pose = pose
             status = self.move(goal)
             # status=self.move(goal)
             # 到达起始点,放下物体
-                if status==True:
-                    msg=String()
-                    selfCycle = self.successCount%3
-                    if selfCycle == 1:
-                        msg.data='0'
-                    elif selfCycle == 2:
-                        msg.data='2'
-                    elif selfCycle == 0:
-                        msg.data='3'
-                    self.grasp_pub.publish(msg)
+            if status != True:
+                rospy.logerr("This is an unexpected situation because the robot")
+                rospy.logerr("cannot return to the place where it was placed.")
+                rospy.logerr("The robot will drop the object at the currect pose.")
+                rospy.logerr("Than re-click point on Rviz...")
+            else:
+                r1 = rospy.Rate(10)
+                vel = Twist()
+                vel.linear.x = 0.1
+                for i in range(10):
+                    self.cmd_vel_pub.publish(vel)
+                    r1.sleep()
+
+            
+            self.grasp_pub.publish(String('0'))
+            self.task_running = self.STATE_DROP
+
+        # 放置成功,任务结束
+        elif msg.data == '2':
+            rospy.loginfo("task finish!!! Waiting next clicked_point...")
+            self.task_running = self.STATE_PENDING
+            r1 = rospy.Rate(10)
+            vel = Twist()
+            vel.linear.x = -0.1
+            for i in range(10):
+                self.cmd_vel_pub.publish(vel)
+                r1.sleep()
+
+        # 最近一次抓取的位置还有能抓的方块, 再导航过去抓取
+        elif msg.data == '3':
+            rospy.loginfo("There is still an object to grab at the last grab position. Go to the last position")
+            r1 = rospy.Rate(10)
+            vel = Twist()
+            vel.linear.x = -0.1
+            for i in range(10):
+                self.cmd_vel_pub.publish(vel)
+                r1.sleep()
+            self.clicked_point_pub.publish(self.last_position)
+
 
 
     def move(self, goal):
@@ -111,7 +171,8 @@ class Move2Grasp():
 
         # Allow 1 minute to get there
         # 设定1分钟的时间限制
-            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))   
+        finished_within_time = self.move_base.wait_for_result(
+            rospy.Duration(60))
 
         # If we don't get there in time, abort the goal
         # 如果一分钟之内没有到达，放弃目标
@@ -122,21 +183,23 @@ class Move2Grasp():
         else:
             # We made it!
             state = self.move_base.get_state()
+            rospy.loginfo(state)
             if state == GoalStatus.SUCCEEDED:
                 rospy.loginfo("Goal succeeded!!!!!!!")
+                rospy.sleep(2) # 停稳
                 return True
 
- 
- 
     def shutdown(self):
         rospy.loginfo("Stopping the robot...")
         # Cancel any active goals
+        if (self.move_base.get_state == 1):
             self.move_base.cancel_goal()
         rospy.sleep(2)
         # Stop the robot
         self.cmd_vel_pub.publish(Twist())
         rospy.sleep(1)
 
+
 if __name__ == '__main__':
     try:
         Move2Grasp()

src/3rd_app/move2grasp/scripts/teleop.py

@@ -14,17 +14,17 @@ cmd = Twist()
 pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
 grasp_pub = rospy.Publisher('/grasp', String, queue_size=1)
 
-global can_grasp
-global can_release
+# global can_grasp
+# global can_release
 
-def grasp_status_cp(msg):
-    global can_release,can_grasp
-    # 物体抓取成功,让机器人回起始点
-    if msg.data=='1':
-        can_release=True
-    if msg.data=='0' or msg.data=='-1':
-        can_grasp=True
-grasp_status=rospy.Subscriber('/grasp_status', String, grasp_status_cp, queue_size=1)
+# def grasp_status_cp(msg):
+#     global can_release,can_grasp
+#     # 物体抓取成功,让机器人回起始点
+#     if msg.data=='1':
+#         can_release=True
+#     if msg.data=='0' or msg.data=='-1':
+#         can_grasp=True
+# grasp_status=rospy.Subscriber('/grasp_status', String, grasp_status_cp, queue_size=1)
 
 def keyboardLoop():
     rospy.init_node('teleop')
@@ -33,11 +33,10 @@ def keyboardLoop():
  
     #速度变量
     # 慢速
-    slow_walk_vel_ = rospy.get_param('walk_vel',0.05)
-    walk_vel_ = rospy.get_param('walk_vel', 0.1)
+    walk_vel_ = rospy.get_param('walk_vel', 0.2)
     # 快速
-    run_vel_ = rospy.get_param('run_vel', 0.5)
-    yaw_rate_ = rospy.get_param('yaw_rate', 0.9)
+    run_vel_ = rospy.get_param('run_vel', 1.0)
+    yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
     yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
     # walk_vel_前后速度
     max_tv = walk_vel_
@@ -45,14 +44,16 @@ def keyboardLoop():
     max_rv = yaw_rate_
     # 参数初始化
     speed=0
-    global can_release,can_grasp
-    can_grasp=True
-    can_release=False
+    # global can_release,can_grasp
+    # can_grasp=True
+    # can_release=False
     
     print ("使用[WASD]控制机器人")
-    print ("按[G]抓取物体，按[H]放下物体")
+    print ("按[G]抓取物体")
+    print ("按[H]放下物体到第一L")
+    print ("按[J]放下物体到第二L")
+    print ("按[K]放下物体到第三L")
     print ("按[Q]退出" )
-    print ("B紧急停止")
  
     #读取按键循环
     while not rospy.is_shutdown():
@@ -68,70 +69,59 @@ def keyboardLoop():
         finally :
             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
         # ch代表获取的键盘按键
-        if ch == 'e':
-            if can_grasp:
-                msg=String()
-                msg.data='1'
-                grasp_pub.publish(msg)
-                can_grasp=False
-                speed = 0
-                turn = 0
-        elif ch == '1':
-            if can_release:
+        if ch == 'g':
+            # if can_grasp:
             msg=String()
             msg.data='0'
             grasp_pub.publish(msg)
-                can_release=False
+            # can_grasp=False
             speed = 0
             turn = 0
-        elif ch == '2':
-            if can_release:
+        elif ch == 'h':
+            # if can_release:
+            msg=String()
+            msg.data='1'
+            grasp_pub.publish(msg)
+            # can_release=False
+            speed = 0
+            turn = 0
+        elif ch == 'j':
+            # if can_release:
             msg=String()
             msg.data='2'
             grasp_pub.publish(msg)
-                can_release=False
+            # can_release=False
             speed = 0
-                turn = 0
-        elif ch == '3':
-            if can_release:
+        elif ch == 'k':
+            # if can_release:
             msg=String()
             msg.data='3'
             grasp_pub.publish(msg)
-                can_release=False
+            # can_release=False
             speed = 0
-                turn = 0
-        
-        elif ch == 'z':
-            if can_release:
-                msg=String()
-                msg.data='4'
-                grasp_pub.publish(msg)
-                can_release=False
-                speed = 0
-                turn = 0
         elif ch == 'w':
             max_tv = walk_vel_
             speed = 1
             turn = 0
         elif ch == 's':
             max_tv = walk_vel_
-            speed = -1.2
+            speed = -1
             turn = 0
         elif ch == 'a':
             max_rv = yaw_rate_
             speed = 0
-            turn = 0.5
+            turn = 1
         elif ch == 'd':
             max_rv = yaw_rate_
             speed = 0
-            turn = -0.5
+            turn = -1
         elif ch == 'W':
             max_tv = run_vel_
-            speed = 1.5
+            speed = 1
             turn = 0
         elif ch == 'S':
             max_tv = run_vel_
-            speed = -1.5
+            speed = -1
             turn = 0
         elif ch == 'A':
             max_rv = yaw_rate_run_
@@ -141,26 +131,6 @@ def keyboardLoop():
             max_rv = yaw_rate_run_
             speed = 0
             turn = -1
-        elif ch == 'i':
-            max_tv = slow_walk_vel_
-            speed = 1
-            turn = 0
-        elif ch == 'k':
-            max_tv = slow_walk_vel_
-            speed = -1
-            turn = 0
-        elif ch == 'j':
-            max_rv = yaw_rate_
-            speed = 0
-            turn = 0.1
-        elif ch == 'l':
-            max_rv = yaw_rate_
-            speed = 0
-            turn = -0.1
-        elif ch == 'b' or ch == 'B':
-            max_rv = yaw_rate_
-            speed = 0
-            turn = 0
         elif ch == 'q':
             exit()
         else: