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: