From 11f1aaae331afcf156b5e1bb3c28a17ca534da82 Mon Sep 17 00:00:00 2001 From: "litian.zhuang" Date: Fri, 21 Oct 2022 22:56:49 +0800 Subject: [PATCH] 20221021 auto --- src/3rd_app/move2grasp/CMakeLists.txt | 0 src/3rd_app/move2grasp/README.md | 8 + .../move2grasp/launch/move2grasp.launch | 4 +- .../move2grasp/launch/teleop2grasp.launch | 6 +- src/3rd_app/move2grasp/package.xml | 0 .../param/base_local_planner_params.yaml | 0 .../param/costmap_common_params.yaml | 0 .../param/dwa_local_planner_params.yaml | 0 .../param/global_costmap_params.yaml | 0 .../param/local_costmap_params.yaml | 0 src/3rd_app/move2grasp/param/move_base.xml | 0 .../param/navfn_global_planner_params.yaml | 0 .../move2grasp/rviz/spark_cartographer.rviz | 0 .../rviz/spark_frontier_exploration.rviz | 0 .../move2grasp/rviz/spark_gmapping.rviz | 0 .../rviz/spark_gmapping_astrapro.rviz | 0 .../move2grasp/rviz/spark_gmapping_d435.rviz | 80 ++- src/3rd_app/move2grasp/rviz/spark_hector.rviz | 0 src/3rd_app/move2grasp/rviz/spark_karto.rviz | 0 .../move2grasp/scripts/.idea/.gitignore | 0 .../inspectionProfiles/profiles_settings.xml | 0 .../move2grasp/scripts/.idea/modules.xml | 0 .../move2grasp/scripts/.idea/scripts.iml | 0 src/3rd_app/move2grasp/scripts/.idea/vcs.xml | 0 .../move2grasp/scripts/grasp_d435.old.py | 434 -------------- src/3rd_app/move2grasp/scripts/grasp_d435.py | 565 +++++++++++------- .../move2grasp/scripts/grasp_pro_new.py | 263 -------- src/3rd_app/move2grasp/scripts/move.py | 317 ++++++---- src/3rd_app/move2grasp/scripts/teleop.py | 140 ++--- 29 files changed, 627 insertions(+), 1190 deletions(-) mode change 100755 => 100644 src/3rd_app/move2grasp/CMakeLists.txt mode change 100755 => 100644 src/3rd_app/move2grasp/README.md mode change 100755 => 100644 src/3rd_app/move2grasp/launch/move2grasp.launch mode change 100755 => 100644 src/3rd_app/move2grasp/launch/teleop2grasp.launch mode change 100755 => 100644 src/3rd_app/move2grasp/package.xml mode change 100755 => 100644 src/3rd_app/move2grasp/param/base_local_planner_params.yaml mode change 100755 => 100644 src/3rd_app/move2grasp/param/costmap_common_params.yaml mode change 100755 => 100644 src/3rd_app/move2grasp/param/dwa_local_planner_params.yaml mode change 100755 => 100644 src/3rd_app/move2grasp/param/global_costmap_params.yaml mode change 100755 => 100644 src/3rd_app/move2grasp/param/local_costmap_params.yaml mode change 100755 => 100644 src/3rd_app/move2grasp/param/move_base.xml mode change 100755 => 100644 src/3rd_app/move2grasp/param/navfn_global_planner_params.yaml mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_cartographer.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_frontier_exploration.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_gmapping.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_hector.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/rviz/spark_karto.rviz mode change 100755 => 100644 src/3rd_app/move2grasp/scripts/.idea/.gitignore mode change 100755 => 100644 src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml mode change 100755 => 100644 src/3rd_app/move2grasp/scripts/.idea/modules.xml mode change 100755 => 100644 src/3rd_app/move2grasp/scripts/.idea/scripts.iml mode change 100755 => 100644 src/3rd_app/move2grasp/scripts/.idea/vcs.xml delete mode 100755 src/3rd_app/move2grasp/scripts/grasp_d435.old.py delete mode 100755 src/3rd_app/move2grasp/scripts/grasp_pro_new.py diff --git a/src/3rd_app/move2grasp/CMakeLists.txt b/src/3rd_app/move2grasp/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/README.md b/src/3rd_app/move2grasp/README.md old mode 100755 new mode 100644 index 547b0b2..2dd9f70 --- a/src/3rd_app/move2grasp/README.md +++ b/src/3rd_app/move2grasp/README.md @@ -10,5 +10,13 @@ roslaunch move2grasp move2grasp.launch +# Ver.full + +建议参考代码而不是直接使用 + +需要使用直接覆盖到原位置，然后使用方式与标准代码一致 + +**！！！覆盖前先备份好原有的代码** + diff --git a/src/3rd_app/move2grasp/launch/move2grasp.launch b/src/3rd_app/move2grasp/launch/move2grasp.launch old mode 100755 new mode 100644 index dcd279a..81829e2 --- a/src/3rd_app/move2grasp/launch/move2grasp.launch +++ b/src/3rd_app/move2grasp/launch/move2grasp.launch @@ -44,14 +44,14 @@ - + - + diff --git a/src/3rd_app/move2grasp/launch/teleop2grasp.launch b/src/3rd_app/move2grasp/launch/teleop2grasp.launch old mode 100755 new mode 100644 index 8673fd8..c6d423d --- a/src/3rd_app/move2grasp/launch/teleop2grasp.launch +++ b/src/3rd_app/move2grasp/launch/teleop2grasp.launch @@ -5,9 +5,9 @@ - + - + @@ -49,7 +49,7 @@ - + diff --git a/src/3rd_app/move2grasp/package.xml b/src/3rd_app/move2grasp/package.xml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/base_local_planner_params.yaml b/src/3rd_app/move2grasp/param/base_local_planner_params.yaml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/costmap_common_params.yaml b/src/3rd_app/move2grasp/param/costmap_common_params.yaml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/dwa_local_planner_params.yaml b/src/3rd_app/move2grasp/param/dwa_local_planner_params.yaml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/global_costmap_params.yaml b/src/3rd_app/move2grasp/param/global_costmap_params.yaml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/local_costmap_params.yaml b/src/3rd_app/move2grasp/param/local_costmap_params.yaml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/move_base.xml b/src/3rd_app/move2grasp/param/move_base.xml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/param/navfn_global_planner_params.yaml b/src/3rd_app/move2grasp/param/navfn_global_planner_params.yaml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/rviz/spark_cartographer.rviz b/src/3rd_app/move2grasp/rviz/spark_cartographer.rviz old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/rviz/spark_frontier_exploration.rviz b/src/3rd_app/move2grasp/rviz/spark_frontier_exploration.rviz old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping.rviz old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz old mode 100755 new mode 100644 index 63353f1..1d3ea79 --- 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.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_color_frame: + {} camera_depth_frame: - camera_color_frame: - camera_color_optical_frame: - {} camera_depth_optical_frame: {} camera_left_ir_frame: @@ -207,8 +203,7 @@ Visualization Manager: left_wheel_link: {} lidar_link: - laser_frame: - {} + {} right_wheel_link: {} room_base_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 diff --git a/src/3rd_app/move2grasp/rviz/spark_hector.rviz b/src/3rd_app/move2grasp/rviz/spark_hector.rviz old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/rviz/spark_karto.rviz b/src/3rd_app/move2grasp/rviz/spark_karto.rviz old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/scripts/.idea/.gitignore b/src/3rd_app/move2grasp/scripts/.idea/.gitignore old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml b/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/scripts/.idea/modules.xml b/src/3rd_app/move2grasp/scripts/.idea/modules.xml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/scripts/.idea/scripts.iml b/src/3rd_app/move2grasp/scripts/.idea/scripts.iml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/scripts/.idea/vcs.xml b/src/3rd_app/move2grasp/scripts/.idea/vcs.xml old mode 100755 new mode 100644 diff --git a/src/3rd_app/move2grasp/scripts/grasp_d435.old.py b/src/3rd_app/move2grasp/scripts/grasp_d435.old.py deleted file mode 100755 index 1eef4b2..0000000 --- a/src/3rd_app/move2grasp/scripts/grasp_d435.old.py +++ /dev/null @@ -1,434 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -from copy import deepcopy -import os -import rospy -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 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 - self.is_found_object = False - self.can_grasp = True - self.def_object_union = { - 'p': [], - 'theta': [], - 'w': [], - 'h': [], - 'size': [], - 'x': [], - 'y': [], - } - self.object_union = deepcopy(self.def_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/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) - self.arm_to_home() - - # 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') - - # 转化成HSV格式 - cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV) - # 蓝色物体颜色检测范围 - LowerBlue = np.array([95, 90, 80]) - UpperBlue = np.array([130, 255, 255]) - # 阈值处理，蓝色部分变为白色，其他部分变为黑色 - mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue) - # 与运算，将白色部分与原来图片的蓝色部分结合 - cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask) - # 取三维数组的第一维所有的数据 - cv_image4 = cv_image3[:, :, 0] - # 低通滤波器，9*9滤波 - blurred = cv2.blur(cv_image4, (9, 9)) - # 简单阈值函数，处理图像灰度值，大于90的都归为255，其他归0 - (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY) - kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) - 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) - - # 找出轮廓 - contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - # 将识别出的轮廓画出来，显示在原图中 - # cv2.drawContours(cv_image1, contours, 0, (0, 255, 0), 2) - # cv2.imshow("test",cv_image1) - self.object_union = deepcopy(self.def_object_union) - # 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 - 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 - - p, theta = cmath.polar(complex(x_mid - 320, 480 - y_mid)) - cn = cmath.rect(p, theta) - - if p > 350: - continue - - self.object_union['p'].append(p) - self.object_union['theta'].append(theta) - self.object_union['size'].append(size) - self.object_union['w'].append(w) - self.object_union['h'].append(h) - self.object_union['x'].append(x_mid) - self.object_union['y'].append(y_mid) - - if p < p_min: - index = index + 1 - p_min = p - xc = x_mid - yc = y_mid - - # 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) <= 4 and abs(yc - self.yc_prev) <= 4: - # print(xc, " ", yc) - 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': - print("收到抓取信号") - times=0 - steps=0 - if self.is_found_object: - if self.can_grasp: - print("现在允许抓取") - if self.grasp(): - print("已成功抓取") - return - else: - # 抓取不成功，将允许重新抓取 - self.can_grasp = True - return - else: - print("抓取执行中，不允许重复抓取") - return - - # 转一圈没有发现可抓取物体,退出抓取 - else: - vel = Twist() - vel.angular.z = 0.2 - rate = rospy.Rate(10) - - # 判断视野范围内有无方块 - 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')) - return - self.cmd_vel_pub.publish(vel) - - times = 0 - while not self.is_found_object: - rate.sleep() - times = times + 1 - if (times > 30): - rospy.logerr("object founded but can not location it!!! Waiting next clicked_point...") - # self.grasp_status_pub.publish(String('0')) - return - - # print("unregisting sub\n") - # self.sub.unregister() - - # 抓取检测到的物体 - if self.grasp(): - print("已成功抓取") - else: - # 抓取不成功，将允许重新抓取 - self.can_grasp = True - - elif msg.data=='0': - # 放下物体 - self.is_found_object = False - self.release_object(1) - elif msg.data=='2': - # 放下物体 - self.is_found_object = False - self.release_object(2) - elif msg.data=='3': - # 放下物体 - self.is_found_object = False - self.release_object(3) - - # 执行抓取 - def grasp(self): - # 设置是否允许抓取值为False，避免抓取过程中重复收到抓取的信号 - self.can_grasp = False - print("现在开始抓取\n") - # 设置睡眠时间 - r1 = rospy.Rate(0.1) - r2 = rospy.Rate(10) - - pos = position() - - # 物体所在坐标+标定误差 - # 先到目标位置上方 - pos.x = self.x_kb[0] * self.yc + self.x_kb[1] - pos.y = self.y_kb[0] * self.xc + self.y_kb[1] - pos.z = 20 - self.pub1.publish(pos) - r2.sleep() - - # 开始往下放 - pos.z = -50 - self.pub1.publish(pos) - r2.sleep() - - # 开始吸取物体，pub2 1为吸取，0为释放 - self.pub2.publish(1) - r2.sleep() - - # 提起物体到检查位置 - pos.x = 250 - pos.y = 0 - pos.z = 70 - self.pub1.publish(pos) - r2.sleep() - - # 利用雷达检查是否有方块 - if self.check_grasp_state(): - print("检测到抓取成功") - pos.x = 220 #160 - pos.y = 0 - pos.z = 160 - self.pub1.publish(pos) - r2.sleep() - return True - else: - print("检测到抓取失败") - self.arm_to_home() - return False - - - def arm_to_home(self): - pos = position() - - pos.x = 140 - pos.y = 0 - pos.z = 60 - self.pub1.publish(pos) - rospy.sleep(1) - - pos.x = 120 - pos.y = 0 - pos.z = 35 - self.pub1.publish(pos) - rospy.sleep(2) - - - # 检查是否成功抓取，成功返回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]) - - # 避免检测距离值出现均为0导致计算报错 - 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,height): - r1 = rospy.Rate(1) # 1s - pos = position() - # go forward - if height == 1: - pos.x = 220 - pos.y = 0 - pos.z = -40 #-80 - self.pub1.publish(pos) - r1.sleep() - - if height == 2: - pos.x = 220 - pos.y = 0 - pos.z = 80 #-80 - self.pub1.publish(pos) - r1.sleep() - - if height == 3: - pos.x = 220 - pos.y = 0 - pos.z = 160 #-80 - self.pub1.publish(pos) - r1.sleep() - - # stop pump - self.pub2.publish(0) - rospy.sleep(1) - - if height == 3: - pos.x = 220 - pos.y = 80 - pos.z = 160 - print("避开第三层方块") - self.pub1.publish(pos) - rospy.sleep(2) - - pos.x = 120 - pos.y = 150 - pos.z = 35 - self.pub1.publish(pos) - rospy.sleep(1) - - self.arm_to_home() - # 释放后，修改成能够成功抓取 - self.can_grasp = 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() - - - -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") - diff --git a/src/3rd_app/move2grasp/scripts/grasp_d435.py b/src/3rd_app/move2grasp/scripts/grasp_d435.py index 62c79b0..8535500 100755 --- a/src/3rd_app/move2grasp/scripts/grasp_d435.py +++ b/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) + 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 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 + 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.waitKey(1) + 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')) + - # self.image_last_time = rospy.get_rostime() - - # 释放物体 - 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) + 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 ==1: - pos.z = -40 - 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) - r1.sleep() - return 'succeeded' + + + # 检查是否成功抓取，成功返回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() + + # 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() diff --git a/src/3rd_app/move2grasp/scripts/grasp_pro_new.py b/src/3rd_app/move2grasp/scripts/grasp_pro_new.py deleted file mode 100755 index fd1db65..0000000 --- a/src/3rd_app/move2grasp/scripts/grasp_pro_new.py +++ /dev/null @@ -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") - diff --git a/src/3rd_app/move2grasp/scripts/move.py b/src/3rd_app/move2grasp/scripts/move.py index 76bde89..1ee489e 100755 --- a/src/3rd_app/move2grasp/scripts/move.py +++ b/src/3rd_app/move2grasp/scripts/move.py @@ -1,145 +1,208 @@ #!/usr/bin/env python3 # -*- coding: UTF-8 -*- - -from glob import glob -from itertools import cycle -from sre_constants import SUCCESS -import rospy -import actionlib + +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 -from visualization_msgs.msg import Marker -from math import radians, pi -class Move2Grasp(): - successCount = 0 - def __init__(self): - rospy.init_node('move2grasp', anonymous=False) - +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 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 + 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) - #订阅机械臂抓取状态 - 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.clicked_point_pub = rospy.Publisher('clicked_point', PointStamped, 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) - # 发布机械臂抓取指令 + # 发布机械臂抓取指令 self.grasp_pub = rospy.Publisher('/grasp', String, queue_size=1) - - # Subscribe to the move_base action server - # 订阅move_base服务器的消息 - self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) - - rospy.loginfo("Waiting for move_base action server...") - - # Wait 60 seconds for the action server to become available - # 等待move_base服务器建立 - self.move_base.wait_for_server(rospy.Duration(60)) - - rospy.loginfo("Connected to move base server") - rospy.loginfo("Starting navigation test") - - - + + # Subscribe to the move_base action server + # 订阅move_base服务器的消息 + self.move_base = actionlib.SimpleActionClient( + "move_base", MoveBaseAction) + + rospy.loginfo("Waiting for move_base action server...") + + # Wait 60 seconds for the action server to become available + # 等待move_base服务器建立 + self.move_base.wait_for_server(rospy.Duration(60)) + + rospy.loginfo("Connected to move base server") + + 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 - # 初始化goal为MoveBaseGoal类型 - goal = MoveBaseGoal() - - # Use the map frame to define goal poses - # 使用map的frame定义goal的frame id - goal.target_pose.header.frame_id = 'map' - - # Set the time stamp to "now" - # 设置时间戳 - goal.target_pose.header.stamp = rospy.Time.now() - - # Set the goal pose - # 设置目标点 + # Intialize the waypoint goal + # 初始化goal为MoveBaseGoal类型 + goal = MoveBaseGoal() + + # Use the map frame to define goal poses + # 使用map的frame定义goal的frame id + goal.target_pose.header.frame_id = 'map' + + # Set the time stamp to "now" + # 设置时间戳 + goal.target_pose.header.stamp = rospy.Time.now() + + # 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)) - goal.target_pose.pose=pose - - # Start the robot moving toward the goal - # 机器人移动 - status=self.move(goal) + 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 + # 机器人移动 + status = self.move(goal) # 如果到达指定地点,就发送抓取指令 - if status==True: - print('goal reached and start grasp') - msg=String() - msg.data='1' - self.grasp_pub.publish(msg) + 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") def grasp_status_cp(self, msg): - # 物体抓取成功,让机器人回起始点 - if msg.data=='1': - 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)) - 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 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 + goal = MoveBaseGoal() + 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 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): - # Send the goal pose to the MoveBaseAction server - # 把目标位置发送给MoveBaseAction的服务器 - self.move_base.send_goal(goal) - - # Allow 1 minute to get there - # 设定1分钟的时间限制 - finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) - - # If we don't get there in time, abort the goal - # 如果一分钟之内没有到达，放弃目标 - if not finished_within_time: - self.move_base.cancel_goal() - rospy.loginfo("Timed out achieving goal") - return False - else: - # We made it! - state = self.move_base.get_state() - if state == GoalStatus.SUCCEEDED: - rospy.loginfo("Goal succeeded!!!!!!!") - return True - - - - def shutdown(self): - rospy.loginfo("Stopping the robot...") - # Cancel any active goals - self.move_base.cancel_goal() - rospy.sleep(2) - # Stop the robot - self.cmd_vel_pub.publish(Twist()) - rospy.sleep(1) - -if __name__ == '__main__': - try: + + def move(self, goal): + # Send the goal pose to the MoveBaseAction server + # 把目标位置发送给MoveBaseAction的服务器 + self.move_base.send_goal(goal) + + # Allow 1 minute to get there + # 设定1分钟的时间限制 + finished_within_time = self.move_base.wait_for_result( + rospy.Duration(60)) + + # If we don't get there in time, abort the goal + # 如果一分钟之内没有到达，放弃目标 + if not finished_within_time: + self.move_base.cancel_goal() + rospy.loginfo("Timed out achieving goal") + return False + 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() - rospy.spin() - except rospy.ROSInterruptException: + rospy.spin() + except rospy.ROSInterruptException: rospy.loginfo("Move2grasp finished.") diff --git a/src/3rd_app/move2grasp/scripts/teleop.py b/src/3rd_app/move2grasp/scripts/teleop.py index 7f5f824..d675491 100755 --- a/src/3rd_app/move2grasp/scripts/teleop.py +++ b/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: - 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 - speed = 0 - turn = 0 - elif ch == '3': - if can_release: - msg=String() - msg.data='3' - grasp_pub.publish(msg) - can_release=False - speed = 0 - turn = 0 - - elif ch == 'z': - if can_release: - msg=String() - msg.data='4' - grasp_pub.publish(msg) - can_release=False - speed = 0 - turn = 0 + 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: + 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 + speed = 0 + elif ch == 'k': + # if can_release: + msg=String() + msg.data='3' + grasp_pub.publish(msg) + # can_release=False + speed = 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: