20220904更新1

2022-09-04 21:55:35 +08:00
parent 5def62264b
commit 6fbae252f9
7 changed files with 172 additions and 111 deletions
--- a/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz
+++ b/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: 210
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
@@ -16,7 +16,7 @@ Panels:
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
@@ -27,6 +27,8 @@ Panels:
    Name: Time
    SyncMode: 0
    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
  toolButtonStyle: 2
 Visualization Manager:
@@ -38,7 +40,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 +123,8 @@ Visualization Manager:
          Value: false
        ir_bumper_right_link:
          Value: false
+        laser_frame:
+          Value: true
        left_wheel_link:
          Value: false
        lidar_link:
@@ -135,6 +139,7 @@ Visualization Manager:
          Value: false
        spark_stack:
          Value: true
+      Marker Alpha: 1
      Marker Scale: 1
      Name: TF
      Show Arrows: true
@@ -203,7 +208,8 @@ Visualization Manager:
                left_wheel_link:
                  {}
                lidar_link:
-                  {}
+                  laser_frame:
+                    {}
                right_wheel_link:
                  {}
                room_base_link:
@@ -227,15 +233,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 +258,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 +391,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 +404,7 @@ Visualization Manager:
          Alpha: 1
          Show Axes: false
          Show Trail: false
+          Value: true
        right_wheel_link:
          Alpha: 1
          Show Axes: false
@@ -420,8 +429,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 +441,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 +460,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 +480,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 +493,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: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b80000003efc0100000002fb0000000800540069006d00650100000000000003b8000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002480000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
@@ -519,6 +530,6 @@ Window Geometry:
    collapsed: false
  Views:
    collapsed: false
-  Width: 959
-  X: 65
-  Y: 24
+  Width: 952
+  X: 72
+  Y: 27
--- a/src/3rd_app/move2grasp/scripts/grasp_d435.py
+++ b/src/3rd_app/move2grasp/scripts/grasp_d435.py
@@ -135,7 +135,7 @@ class GraspObject():
        self.pub1.publish(pos)
        r2.sleep()
        # go down -100
-        pos.z = -50
+        pos.z = -70
        self.pub1.publish(pos)
        print("z = -83\n")
        r2.sleep()
@@ -163,8 +163,10 @@ class GraspObject():
        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)

        # 蓝色物体颜色检测范围
-        LowerBlue = np.array([95, 90, 80])
-        UpperBlue = np.array([130, 255, 255])
+        LowerBlue = np.array([95, 90, 100])
+        UpperBlue = np.array([130, 210, 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)

@@ -233,7 +235,7 @@ class GraspObject():

        # stop pump
        self.pub2.publish(0)
-        r2.sleep()
+        # r2.sleep()
        #r1.sleep()
        pos.x = 120
        pos.y = 0
--- a/src/3rd_app/move2grasp/scripts/teleop.py
+++ b/src/3rd_app/move2grasp/scripts/teleop.py
@@ -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_
@@ -51,6 +52,7 @@ def keyboardLoop():
    print ("使用[WASD]控制机器人")
    print ("按[G]抓取物体，按[H]放下物体")
    print ("按[Q]退出" )
+    print ("B紧急停止")
 
    #读取按键循环
    while not rospy.is_shutdown():
@@ -82,22 +84,30 @@ def keyboardLoop():
                can_release=False
                speed = 0
                turn = 0
+        elif ch == 'z':
+            if can_release:
+                msg=String()
+                msg.data='2'
+                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.15
        elif ch == 'd':
            max_rv = yaw_rate_
            speed = 0
-            turn = -1
+            turn = -0.15
        elif ch == 'W':
            max_tv = run_vel_
            speed = 1
@@ -114,6 +124,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.075
+        elif ch == 'l':
+            max_rv = yaw_rate_
+            speed = 0
+            turn = -0.075
+        elif ch == 'b' or ch == 'B':
+            max_rv = yaw_rate_
+            speed = 0
+            turn = 0
        elif ch == 'q':
            exit()
        else: