src/3rd_app/move2grasp/README.md

@@ -10,13 +10,5 @@ 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" output="screen"/>
+        <node pkg="move2grasp" type="move.py" name="move" />
 	<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.py" name="grasp" />
+		<node pkg="move2grasp" type="grasp_pro_new.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="ydlidar_g2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
+	<arg name="lidar_type_tel" default="3iroboticslidar2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
 	<!-- 摄像机类型 -->
-	<arg name="camera_type_tel" default="d435" doc="camera type [astrapro, astra, d435...]"/>
+	<arg name="camera_type_tel" default="astrapro" 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_best.py" name="grasp" output="screen">
+		<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>

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

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

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

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

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

@@ -1,51 +1,46 @@
 #!/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 sensor_msgs.msg import Image,LaserScan
+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 *
-from matplotlib import pyplot as plt
 
 
 class GraspObject():
     '''
     监听主控，用于物品抓取功能
     '''
-    image_last_time = None
 
     def __init__(self):
         '''
         初始化
         '''
-        self.xc = 0
-        self.yc = 0
-        self.xc_prev = 0
-        self.yc_prev = 0
-        self.found_count = 0
-        self.is_have_object = False
+
+        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.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.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)
@@ -59,358 +54,260 @@ class GraspObject():
             'grasp_status', String, queue_size=1)
         # 发布TWist消息控制机器人底盘
         self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
-        self.arm_to_home()
-
-    def hsv_fliter(self, img):
-        # change rgb to hsv
-        cv_img2 = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
-
-        # 蓝色物体颜色检测范围
-        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)
-
-        # 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
-
-    # 使用CV检测物体       
-    def image_cb(self, data):
-        xc = -1
-        yc = -1
-        # change to opencv
-        try:
-            cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
-        except CvBridgeError as e:
-            print('error')
-
-        cv_image5 = self.hsv_fliter(cv_image1)
-
-        contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
-
-
-        # if find contours, pick the biggest box
-        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)
-                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)
-
-                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 self.found_count >= 30:
-                self.found_count = 0
-                self.is_found_object = True
-                self.xc = xc
-                self.yc = yc
-            else:
-                # if box is not moving
-                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:
-                    self.found_count = 0
-        self.xc_prev = xc
-        self.yc_prev = yc
-        
-        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)
-
-        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 grasp(self, xc, yc):
-        r1 = rospy.Rate(0.095)
-        r2 = rospy.Rate(10)
-        r3 = rospy.Rate(1)
-        pos = position()
-
-        # 物体所在坐标+标定误差
-        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
-        pos.z = 0
-        rospy.loginfo("to check point")
-        self.pub1.publish(pos)
-        r1.sleep()
-
-        # 利用雷达检查是否有方块
-        if self.check_grasp_state():
-            rospy.loginfo("Grasp !!!")
-            pos.x = 250  #160
-            pos.y = 0
-            pos.z = 150
-            self.pub1.publish(pos)
-            r3.sleep()
-            return True
-        else:
-            rospy.loginfo("Grasp Fail... to home")
-            self.pub2.publish(0)
-            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)
 
+    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) 
 
-     # 检查是否成功抓取，成功返回True，反之返回False
-    def check_grasp_state(self):
+    # 执行抓取
+    def grasp(self):
+        print("start to grasp\n")
+        global xc, yc, found_count
+        # stop function
 
-        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
+        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()
 
-        if check:
-            # leave camera area
-            rospy.loginfo("leave camera area")
-            pos.x = 100
-            pos.y = -200
-            pos.z = 150  #-80
+        # 提起物体
+        pos.x = 250  #160
+        pos.y = 0
+        pos.z = 170  #55
         self.pub1.publish(pos)
         r1.sleep()
-
-            # go forward
-            pos.x = 230
-            pos.y = 0
-            data = rospy.wait_for_message("/camera/color/image_raw", Image)
+    # 使用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")
+            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')
 
-            cv_image5 = self.hsv_fliter(cv_image1)
+        # change rgb to hsv
+        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
 
-            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()
+        # 蓝色物体颜色检测范围
+        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)
+
+        # 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
+            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
+                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:
-            pos.x = 230
+                    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
+                    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
+        
+        # cv2.imshow("source", cv_image1)
+        # cv2.waitKey(1)
+
+        # self.image_last_time = rospy.get_rostime()
+        
+        
+    # 释放物体
+    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 = 70
+        if height == 0:
+            pos.z = -40
+            self.pub1.publish(pos)
+        elif height ==1:
+            pos.z = -40
+            self.pub1.publish(pos)
+        elif height == 2:
+            pos.z = 80
+            self.pub1.publish(pos)
+        elif height ==3:
+            print('nmsl')
+            self.pub2.publish(0)
+        time.sleep(2)
+        # stop pump
+        self.pub2.publish(0)
+        time.sleep(1)
+        # r1.sleep()
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
         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
-
+        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()
+    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

@@ -0,0 +1,263 @@
+#!/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,36 +1,28 @@
 #!/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  
-import tf
-import tf.transformations as tf_transformations
+from tf.transformations import quaternion_from_euler  
 from visualization_msgs.msg import Marker  
 from math import radians, pi  
-import numpy as np
-
-
 class Move2Grasp():  
-    STATE_PENDING = 0
-    STATE_GOING = 1
-    STATE_GRASP = 0
-    STATE_HOMING = 0
-    STATE_DROP = 0
+    successCount = 0
     def __init__(self):  
         rospy.init_node('move2grasp', anonymous=False)  
  
-        rospy.loginfo("1211113242ewrqewr325235")
         rospy.on_shutdown(self.shutdown)
-        # 订阅RVIZ上的点击事件
+        #订阅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)
@@ -39,8 +31,7 @@ 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...")  
  
@@ -49,25 +40,11 @@ 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  
@@ -85,83 +62,46 @@ class Move2Grasp():
             # Set the goal pose 
             # 设置目标点  
             #pose = Pose(Point(0.0,0.0,0.0), 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
+            pose = Pose(Point(msg.point.x,msg.point.y,msg.point.z), Quaternion(0.0, 0.0, 0.0, 1.0))
+            goal.target_pose.pose=pose  
  
             # Start the robot moving toward the goal  
             # 机器人移动  
-            status = self.move(goal)
+            status=self.move(goal)
             # 如果到达指定地点,就发送抓取指令
-            if status == True:
-                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")
+            if status==True:
+                print('goal reached and start grasp')
+                msg=String()
+                msg.data='1'
+                self.grasp_pub.publish(msg)
 
     def grasp_status_cp(self, msg):
-
-        # 抓取失败,任务重新开始
-        if msg.data == '0':
-            rospy.logerr("Grasp failed!!! Waiting next clicked_point...")
-            self.task_running = self.STATE_PENDING
-
             # 物体抓取成功,让机器人回起始点
-        elif msg.data == '1':
-            self.task_running = self.STATE_HOMING
+            if msg.data=='1': 
                 goal = MoveBaseGoal()
+                self.successCount+=1
                 goal.target_pose.header.frame_id = 'map' 
                 goal.target_pose.header.stamp = rospy.Time.now() 
-            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 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))
+                goal.target_pose.pose=pose  
+                status=self.move(goal)
+                #status=self.move(goal)
                 # 到达起始点,放下物体
-            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)
-
+                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)
 
 
     def move(self, goal):  
@@ -171,8 +111,7 @@ 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  
             # 如果一分钟之内没有到达，放弃目标  
@@ -183,23 +122,21 @@ 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,10 +33,11 @@ def keyboardLoop():
  
     #速度变量
     # 慢速
-    walk_vel_ = rospy.get_param('walk_vel', 0.2)
+    slow_walk_vel_ = rospy.get_param('walk_vel',0.05)
+    walk_vel_ = rospy.get_param('walk_vel', 0.1)
     # 快速
-    run_vel_ = rospy.get_param('run_vel', 1.0)
-    yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
+    run_vel_ = rospy.get_param('run_vel', 0.5)
+    yaw_rate_ = rospy.get_param('yaw_rate', 0.9)
     yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
     # walk_vel_前后速度
     max_tv = walk_vel_
@@ -44,16 +45,14 @@ 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]抓取物体")
-    print ("按[H]放下物体到第一L")
-    print ("按[J]放下物体到第二L")
-    print ("按[K]放下物体到第三L")
+    print ("按[G]抓取物体，按[H]放下物体")
     print ("按[Q]退出" )
+    print ("B紧急停止")
  
     #读取按键循环
     while not rospy.is_shutdown():
@@ -69,59 +68,70 @@ def keyboardLoop():
         finally :
             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
         # ch代表获取的键盘按键
-        if ch == 'g':
-            # if can_grasp:
-            msg=String()
-            msg.data='0'
-            grasp_pub.publish(msg)
-            # can_grasp=False
-            speed = 0
-            turn = 0
-        elif ch == 'h':
-            # if can_release:
+        if ch == 'e':
+            if can_grasp:
                 msg=String()
                 msg.data='1'
                 grasp_pub.publish(msg)
-            # can_release=False
+                can_grasp=False
                 speed = 0
                 turn = 0
-        elif ch == 'j':
-            # if can_release:
+        elif ch == '1':
+            if can_release:
+                msg=String()
+                msg.data='0'
+                grasp_pub.publish(msg)
+                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
+                can_release=False
                 speed = 0
-        elif ch == 'k':
-            # if can_release:
+                turn = 0
+        elif ch == '3':
+            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
+            speed = -1.2
             turn = 0
         elif ch == 'a':
             max_rv = yaw_rate_
             speed = 0
-            turn = 1
+            turn = 0.5
         elif ch == 'd':
             max_rv = yaw_rate_
             speed = 0
-            turn = -1
+            turn = -0.5
         elif ch == 'W':
             max_tv = run_vel_
-            speed = 1
+            speed = 1.5
             turn = 0
         elif ch == 'S':
             max_tv = run_vel_
-            speed = -1
+            speed = -1.5
             turn = 0
         elif ch == 'A':
             max_rv = yaw_rate_run_
@@ -131,6 +141,26 @@ 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: