From aba3c6ee5527b511ca69c76b51768556d089a85b Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Thu, 12 Sep 2024 18:44:26 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A0=E9=99=A4=E5=BC=83=E7=94=A8=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mecharm/mecharm/CMakeLists.txt | 3 - .../mecharm/config/mycobot_with_marker.rviz | 216 ------------------ mecharm/mecharm/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 31 --- mecharm/mecharm/scripts/detect_marker.py | 125 ---------- mecharm/mecharm/scripts/follow_and_pump.py | 197 ---------------- mecharm/mecharm/scripts/following_marker.py | 65 ------ mecharm/mecharm_pi/CMakeLists.txt | 3 - .../mecharm_pi/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 31 --- mecharm/mecharm_pi/scripts/detect_marker.py | 125 ---------- mecharm/mecharm_pi/scripts/follow_and_pump.py | 197 ---------------- .../mecharm_pi/scripts/following_marker.py | 65 ------ myArm/myarm/CMakeLists.txt | 3 - myArm/myarm/config/mycobot_with_marker.rviz | 216 ------------------ myArm/myarm/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 30 --- myArm/myarm/scripts/detect_marker.py | 123 ---------- myArm/myarm/scripts/follow_and_pump.py | 194 ---------------- myArm/myarm/scripts/following_marker.py | 64 ------ myArm/myarm_c650/CMakeLists.txt | 3 - myArm/myarm_c650/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 30 --- myArm/myarm_c650/scripts/detect_marker.py | 123 ---------- myArm/myarm_c650/scripts/follow_and_pump.py | 194 ---------------- myArm/myarm_c650/scripts/following_marker.py | 64 ------ myArm/myarm_m/CMakeLists.txt | 3 - myArm/myarm_m/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 30 --- myArm/myarm_m/scripts/detect_marker.py | 123 ---------- myArm/myarm_m/scripts/follow_and_pump.py | 194 ---------------- myArm/myarm_m/scripts/following_marker.py | 64 ------ .../scripts/obstacle_avoidance_demo.py | 163 ------------- mycobot_280/mycobot_280/CMakeLists.txt | 3 - .../mycobot_280/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 30 --- .../mycobot_280/scripts/detect_marker.py | 123 ---------- .../mycobot_280/scripts/follow_and_pump.py | 194 ---------------- .../mycobot_280/scripts/following_marker.py | 64 ------ .../mycobot_280_gripper_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 137 ----------- mycobot_280/mycobot_280_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 137 ----------- mycobot_280/mycobot_280arduino/CMakeLists.txt | 3 - .../launch/detect_marker.launch | 16 -- .../launch/detect_marker_with_topic.launch | 29 --- .../scripts/detect_marker.py | 123 ---------- .../scripts/follow_and_pump.py | 190 --------------- .../scripts/following_marker.py | 64 ------ .../mycobot_280arduino_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 125 ---------- mycobot_280/mycobot_280pi/CMakeLists.txt | 3 - .../config/mycobot_with_marker.rviz | 216 ------------------ .../mycobot_280pi/launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 30 --- .../mycobot_280pi/scripts/detect_marker.py | 124 ---------- .../mycobot_280pi/scripts/follow_and_pump.py | 194 ---------------- .../mycobot_280pi/scripts/following_marker.py | 65 ------ .../mycobot_280pi_moveit/CMakeLists.txt | 1 - ...th_planning_and_obstacle_avoidance_demo.py | 137 ----------- mycobot_320/mycobot_320/CMakeLists.txt | 3 - .../launch/mycobot_320_detect_marker.launch | 16 -- ...ycobot_320_detect_marker_with_topic.launch | 29 --- .../scripts/mycobot_320_detect_marker.py | 124 ---------- .../scripts/mycobot_320_follow_and_pump.py | 190 --------------- .../scripts/mycobot_320_following_marker.py | 65 ------ mycobot_320/new_mycobot_320/CMakeLists.txt | 3 - .../launch/mycobot_320_detect_marker.launch | 16 -- ...ycobot_320_detect_marker_with_topic.launch | 29 --- .../scripts/mycobot_320_detect_marker.py | 124 ---------- .../scripts/mycobot_320_follow_and_pump.py | 190 --------------- .../scripts/mycobot_320_following_marker.py | 65 ------ ...th_planning_and_obstacle_avoidance_demo.py | 137 ----------- mycobot_320/new_mycobot_320_pi/CMakeLists.txt | 3 - .../config/mycobot_320_with_marker.rviz | 216 ------------------ .../launch/mycobot_320_detect_marker.launch | 16 -- ...ycobot_320_detect_marker_with_topic.launch | 29 --- .../scripts/mycobot_320_detect_marker.py | 124 ---------- .../scripts/mycobot_320_follow_and_pump.py | 190 --------------- .../scripts/mycobot_320_following_marker.py | 65 ------ ...th_planning_and_obstacle_avoidance_demo.py | 137 ----------- .../mypalletizer_260/CMakeLists.txt | 3 - .../config/mycobot_with_marker.rviz | 216 ------------------ .../launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 31 --- .../mypalletizer_260/scripts/detect_marker.py | 124 ---------- .../scripts/follow_and_pump.py | 197 ---------------- .../scripts/following_marker.py | 64 ------ .../mypalletizer_260_pi/CMakeLists.txt | 3 - .../config/mycobot_with_marker.rviz | 216 ------------------ .../launch/detect_marker.launch | 18 -- .../launch/detect_marker_with_topic.launch | 31 --- .../scripts/detect_marker.py | 125 ---------- .../scripts/follow_and_pump.py | 197 ---------------- .../scripts/following_marker.py | 65 ------ 95 files changed, 7895 deletions(-) delete mode 100644 mecharm/mecharm/config/mycobot_with_marker.rviz delete mode 100644 mecharm/mecharm/launch/detect_marker.launch delete mode 100644 mecharm/mecharm/launch/detect_marker_with_topic.launch delete mode 100755 mecharm/mecharm/scripts/detect_marker.py delete mode 100755 mecharm/mecharm/scripts/follow_and_pump.py delete mode 100755 mecharm/mecharm/scripts/following_marker.py delete mode 100644 mecharm/mecharm_pi/launch/detect_marker.launch delete mode 100644 mecharm/mecharm_pi/launch/detect_marker_with_topic.launch delete mode 100644 mecharm/mecharm_pi/scripts/detect_marker.py delete mode 100644 mecharm/mecharm_pi/scripts/follow_and_pump.py delete mode 100644 mecharm/mecharm_pi/scripts/following_marker.py delete mode 100755 myArm/myarm/config/mycobot_with_marker.rviz delete mode 100755 myArm/myarm/launch/detect_marker.launch delete mode 100755 myArm/myarm/launch/detect_marker_with_topic.launch delete mode 100755 myArm/myarm/scripts/detect_marker.py delete mode 100755 myArm/myarm/scripts/follow_and_pump.py delete mode 100755 myArm/myarm/scripts/following_marker.py delete mode 100755 myArm/myarm_c650/launch/detect_marker.launch delete mode 100755 myArm/myarm_c650/launch/detect_marker_with_topic.launch delete mode 100755 myArm/myarm_c650/scripts/detect_marker.py delete mode 100755 myArm/myarm_c650/scripts/follow_and_pump.py delete mode 100755 myArm/myarm_c650/scripts/following_marker.py delete mode 100755 myArm/myarm_m/launch/detect_marker.launch delete mode 100755 myArm/myarm_m/launch/detect_marker_with_topic.launch delete mode 100755 myArm/myarm_m/scripts/detect_marker.py delete mode 100755 myArm/myarm_m/scripts/follow_and_pump.py delete mode 100755 myArm/myarm_m/scripts/following_marker.py delete mode 100755 myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py delete mode 100644 mycobot_280/mycobot_280/launch/detect_marker.launch delete mode 100644 mycobot_280/mycobot_280/launch/detect_marker_with_topic.launch delete mode 100755 mycobot_280/mycobot_280/scripts/detect_marker.py delete mode 100755 mycobot_280/mycobot_280/scripts/follow_and_pump.py delete mode 100755 mycobot_280/mycobot_280/scripts/following_marker.py delete mode 100644 mycobot_280/mycobot_280_gripper_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_280/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_280/mycobot_280arduino/launch/detect_marker.launch delete mode 100644 mycobot_280/mycobot_280arduino/launch/detect_marker_with_topic.launch delete mode 100644 mycobot_280/mycobot_280arduino/scripts/detect_marker.py delete mode 100644 mycobot_280/mycobot_280arduino/scripts/follow_and_pump.py delete mode 100644 mycobot_280/mycobot_280arduino/scripts/following_marker.py delete mode 100644 mycobot_280/mycobot_280arduino_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_280/mycobot_280pi/config/mycobot_with_marker.rviz delete mode 100644 mycobot_280/mycobot_280pi/launch/detect_marker.launch delete mode 100644 mycobot_280/mycobot_280pi/launch/detect_marker_with_topic.launch delete mode 100755 mycobot_280/mycobot_280pi/scripts/detect_marker.py delete mode 100755 mycobot_280/mycobot_280pi/scripts/follow_and_pump.py delete mode 100755 mycobot_280/mycobot_280pi/scripts/following_marker.py delete mode 100644 mycobot_280/mycobot_280pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_320/mycobot_320/launch/mycobot_320_detect_marker.launch delete mode 100644 mycobot_320/mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch delete mode 100755 mycobot_320/mycobot_320/scripts/mycobot_320_detect_marker.py delete mode 100755 mycobot_320/mycobot_320/scripts/mycobot_320_follow_and_pump.py delete mode 100755 mycobot_320/mycobot_320/scripts/mycobot_320_following_marker.py delete mode 100644 mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker.launch delete mode 100644 mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch delete mode 100755 mycobot_320/new_mycobot_320/scripts/mycobot_320_detect_marker.py delete mode 100755 mycobot_320/new_mycobot_320/scripts/mycobot_320_follow_and_pump.py delete mode 100755 mycobot_320/new_mycobot_320/scripts/mycobot_320_following_marker.py delete mode 100755 mycobot_320/new_mycobot_320_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mycobot_320/new_mycobot_320_pi/config/mycobot_320_with_marker.rviz delete mode 100644 mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker.launch delete mode 100644 mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker_with_topic.launch delete mode 100755 mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_detect_marker.py delete mode 100755 mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_follow_and_pump.py delete mode 100755 mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_following_marker.py delete mode 100755 mycobot_320/new_mycobot_320_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py delete mode 100644 mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz delete mode 100644 mypalletizer_260/mypalletizer_260/launch/detect_marker.launch delete mode 100644 mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch delete mode 100644 mypalletizer_260/mypalletizer_260/scripts/detect_marker.py delete mode 100644 mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py delete mode 100644 mypalletizer_260/mypalletizer_260/scripts/following_marker.py delete mode 100644 mypalletizer_260/mypalletizer_260_pi/config/mycobot_with_marker.rviz delete mode 100644 mypalletizer_260/mypalletizer_260_pi/launch/detect_marker.launch delete mode 100644 mypalletizer_260/mypalletizer_260_pi/launch/detect_marker_with_topic.launch delete mode 100644 mypalletizer_260/mypalletizer_260_pi/scripts/detect_marker.py delete mode 100644 mypalletizer_260/mypalletizer_260_pi/scripts/follow_and_pump.py delete mode 100644 mypalletizer_260/mypalletizer_260_pi/scripts/following_marker.py diff --git a/mecharm/mecharm/CMakeLists.txt b/mecharm/mecharm/CMakeLists.txt index e74a569b..6a36a90a 100644 --- a/mecharm/mecharm/CMakeLists.txt +++ b/mecharm/mecharm/CMakeLists.txt @@ -27,9 +27,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mecharm/mecharm/config/mycobot_with_marker.rviz b/mecharm/mecharm/config/mycobot_with_marker.rviz deleted file mode 100644 index 8b0dc7f4..00000000 --- a/mecharm/mecharm/config/mycobot_with_marker.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Marker1 - - /Marker1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - 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: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - joint1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - basic_shapes: - Value: true - joint1: - Value: true - joint2: - Value: true - joint3: - Value: true - joint4: - Value: true - joint5: - Value: true - joint6: - Value: true - joint6_flange: - Value: true - Marker Scale: 0.300000012 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - joint1: - joint2: - joint3: - joint4: - joint5: - joint6: - joint6_flange: - basic_shapes: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - basic_cube: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: joint1 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.11990476 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.375398338 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.235389769 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 diff --git a/mecharm/mecharm/launch/detect_marker.launch b/mecharm/mecharm/launch/detect_marker.launch deleted file mode 100644 index e8c6998c..00000000 --- a/mecharm/mecharm/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mecharm/mecharm/launch/detect_marker_with_topic.launch b/mecharm/mecharm/launch/detect_marker_with_topic.launch deleted file mode 100644 index 8b810584..00000000 --- a/mecharm/mecharm/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mecharm/mecharm/scripts/detect_marker.py b/mecharm/mecharm/scripts/detect_marker.py deleted file mode 100755 index acaf4214..00000000 --- a/mecharm/mecharm/scripts/detect_marker.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img进来 - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. 如果没有就计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. 检测 aruco 标记。 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测标记姿势 - # argument: - # marker corners, 标记角 - # marker size (meter), 标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. 只需选择第一个检测到的标记 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - # 根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mecharm/mecharm/scripts/follow_and_pump.py b/mecharm/mecharm/scripts/follow_and_pump.py deleted file mode 100755 index 866f7596..00000000 --- a/mecharm/mecharm/scripts/follow_and_pump.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message to communicate with mycobot -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制mycobot的话题，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging device: ttyUSB* is M5；ttyACM* is wio -# 判断设备：ttyUSB*为M5；ttyACM*为wio -robot = os.popen("ls /dev/ttyAMA*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# Instantiate the message object. 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from the real position of mycobot -# 与mycobot真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 使用此变量限制，抓取行为仅执行一次 -flag = False - -# In order to compare whether the QR code moves later -# 为了比较二维码后面是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates 发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle. 发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status. 发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves. 判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function. 回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value. 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # subscribers to mark information, mark信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mecharm/mecharm/scripts/following_marker.py b/mecharm/mecharm/scripts/following_marker.py deleted file mode 100755 index ce9257fd..00000000 --- a/mecharm/mecharm/scripts/following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding:utf-8 -*- -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker 标记 - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial. 标记初始位置 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mecharm/mecharm_pi/CMakeLists.txt b/mecharm/mecharm_pi/CMakeLists.txt index 1c5f4ed0..ab023a34 100644 --- a/mecharm/mecharm_pi/CMakeLists.txt +++ b/mecharm/mecharm_pi/CMakeLists.txt @@ -27,9 +27,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mecharm/mecharm_pi/launch/detect_marker.launch b/mecharm/mecharm_pi/launch/detect_marker.launch deleted file mode 100644 index e8c6998c..00000000 --- a/mecharm/mecharm_pi/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mecharm/mecharm_pi/launch/detect_marker_with_topic.launch b/mecharm/mecharm_pi/launch/detect_marker_with_topic.launch deleted file mode 100644 index 8b810584..00000000 --- a/mecharm/mecharm_pi/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mecharm/mecharm_pi/scripts/detect_marker.py b/mecharm/mecharm_pi/scripts/detect_marker.py deleted file mode 100644 index acaf4214..00000000 --- a/mecharm/mecharm_pi/scripts/detect_marker.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img进来 - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. 如果没有就计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. 检测 aruco 标记。 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测标记姿势 - # argument: - # marker corners, 标记角 - # marker size (meter), 标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. 只需选择第一个检测到的标记 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - # 根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mecharm/mecharm_pi/scripts/follow_and_pump.py b/mecharm/mecharm_pi/scripts/follow_and_pump.py deleted file mode 100644 index 866f7596..00000000 --- a/mecharm/mecharm_pi/scripts/follow_and_pump.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message to communicate with mycobot -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制mycobot的话题，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging device: ttyUSB* is M5；ttyACM* is wio -# 判断设备：ttyUSB*为M5；ttyACM*为wio -robot = os.popen("ls /dev/ttyAMA*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# Instantiate the message object. 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from the real position of mycobot -# 与mycobot真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 使用此变量限制，抓取行为仅执行一次 -flag = False - -# In order to compare whether the QR code moves later -# 为了比较二维码后面是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates 发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle. 发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status. 发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves. 判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function. 回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value. 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # subscribers to mark information, mark信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mecharm/mecharm_pi/scripts/following_marker.py b/mecharm/mecharm_pi/scripts/following_marker.py deleted file mode 100644 index ce9257fd..00000000 --- a/mecharm/mecharm_pi/scripts/following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding:utf-8 -*- -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker 标记 - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial. 标记初始位置 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/myArm/myarm/CMakeLists.txt b/myArm/myarm/CMakeLists.txt index f67a308e..c7e412cb 100644 --- a/myArm/myarm/CMakeLists.txt +++ b/myArm/myarm/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/myArm/myarm/config/mycobot_with_marker.rviz b/myArm/myarm/config/mycobot_with_marker.rviz deleted file mode 100755 index 8b0dc7f4..00000000 --- a/myArm/myarm/config/mycobot_with_marker.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Marker1 - - /Marker1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - 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: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - joint1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - basic_shapes: - Value: true - joint1: - Value: true - joint2: - Value: true - joint3: - Value: true - joint4: - Value: true - joint5: - Value: true - joint6: - Value: true - joint6_flange: - Value: true - Marker Scale: 0.300000012 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - joint1: - joint2: - joint3: - joint4: - joint5: - joint6: - joint6_flange: - basic_shapes: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - basic_cube: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: joint1 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.11990476 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.375398338 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.235389769 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 diff --git a/myArm/myarm/launch/detect_marker.launch b/myArm/myarm/launch/detect_marker.launch deleted file mode 100755 index 12fc1271..00000000 --- a/myArm/myarm/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm/launch/detect_marker_with_topic.launch b/myArm/myarm/launch/detect_marker_with_topic.launch deleted file mode 100755 index c0ec2801..00000000 --- a/myArm/myarm/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm/scripts/detect_marker.py b/myArm/myarm/scripts/detect_marker.py deleted file mode 100755 index 4283ea1c..00000000 --- a/myArm/myarm/scripts/detect_marker.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have.如果没有，则计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker.检测 aruco 标记 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data.处理标记数据 - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测marker位姿。 - # argument: - # marker corners,标记角 - # marker size (meter),标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker.只需选择第一个检测到的标记。 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1]，根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/myArm/myarm/scripts/follow_and_pump.py b/myArm/myarm/scripts/follow_and_pump.py deleted file mode 100755 index d889bbb2..00000000 --- a/myArm/myarm/scripts/follow_and_pump.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message communicated with mycobot，与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# instantiate the message object，实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 通过该变量限制，抓取行为只做一次 -flag = False - -# In order to compare whether the QR code moves later,为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates,发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle,发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status,发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves""" - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function,回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value,解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者,subscribers to mark information - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/myArm/myarm/scripts/following_marker.py b/myArm/myarm/scripts/following_marker.py deleted file mode 100755 index 8f59265c..00000000 --- a/myArm/myarm/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/base" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("base", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial,标记位置初始化 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/myArm/myarm_c650/CMakeLists.txt b/myArm/myarm_c650/CMakeLists.txt index 5bd757ad..d79c10c8 100644 --- a/myArm/myarm_c650/CMakeLists.txt +++ b/myArm/myarm_c650/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/myArm/myarm_c650/launch/detect_marker.launch b/myArm/myarm_c650/launch/detect_marker.launch deleted file mode 100755 index ec8ffe06..00000000 --- a/myArm/myarm_c650/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm_c650/launch/detect_marker_with_topic.launch b/myArm/myarm_c650/launch/detect_marker_with_topic.launch deleted file mode 100755 index ebf86e75..00000000 --- a/myArm/myarm_c650/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm_c650/scripts/detect_marker.py b/myArm/myarm_c650/scripts/detect_marker.py deleted file mode 100755 index 4283ea1c..00000000 --- a/myArm/myarm_c650/scripts/detect_marker.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have.如果没有，则计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker.检测 aruco 标记 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data.处理标记数据 - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测marker位姿。 - # argument: - # marker corners,标记角 - # marker size (meter),标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker.只需选择第一个检测到的标记。 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1]，根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/myArm/myarm_c650/scripts/follow_and_pump.py b/myArm/myarm_c650/scripts/follow_and_pump.py deleted file mode 100755 index d889bbb2..00000000 --- a/myArm/myarm_c650/scripts/follow_and_pump.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message communicated with mycobot，与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# instantiate the message object，实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 通过该变量限制，抓取行为只做一次 -flag = False - -# In order to compare whether the QR code moves later,为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates,发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle,发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status,发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves""" - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function,回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value,解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者,subscribers to mark information - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/myArm/myarm_c650/scripts/following_marker.py b/myArm/myarm_c650/scripts/following_marker.py deleted file mode 100755 index 8f59265c..00000000 --- a/myArm/myarm_c650/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/base" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("base", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial,标记位置初始化 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/myArm/myarm_m/CMakeLists.txt b/myArm/myarm_m/CMakeLists.txt index f7a3038a..450f4fae 100755 --- a/myArm/myarm_m/CMakeLists.txt +++ b/myArm/myarm_m/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/myArm/myarm_m/launch/detect_marker.launch b/myArm/myarm_m/launch/detect_marker.launch deleted file mode 100755 index 12fc1271..00000000 --- a/myArm/myarm_m/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm_m/launch/detect_marker_with_topic.launch b/myArm/myarm_m/launch/detect_marker_with_topic.launch deleted file mode 100755 index c0ec2801..00000000 --- a/myArm/myarm_m/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/myArm/myarm_m/scripts/detect_marker.py b/myArm/myarm_m/scripts/detect_marker.py deleted file mode 100755 index 4283ea1c..00000000 --- a/myArm/myarm_m/scripts/detect_marker.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have.如果没有，则计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker.检测 aruco 标记 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data.处理标记数据 - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测marker位姿。 - # argument: - # marker corners,标记角 - # marker size (meter),标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker.只需选择第一个检测到的标记。 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1]，根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/myArm/myarm_m/scripts/follow_and_pump.py b/myArm/myarm_m/scripts/follow_and_pump.py deleted file mode 100755 index d889bbb2..00000000 --- a/myArm/myarm_m/scripts/follow_and_pump.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message communicated with mycobot，与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# instantiate the message object，实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 通过该变量限制，抓取行为只做一次 -flag = False - -# In order to compare whether the QR code moves later,为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates,发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle,发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status,发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves""" - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function,回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value,解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者,subscribers to mark information - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/myArm/myarm_m/scripts/following_marker.py b/myArm/myarm_m/scripts/following_marker.py deleted file mode 100755 index 8f59265c..00000000 --- a/myArm/myarm_m/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/base" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("base", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial,标记位置初始化 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py b/myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py deleted file mode 100755 index 813b4d19..00000000 --- a/myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py +++ /dev/null @@ -1,163 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -import sys -import rospy -import moveit_commander -import moveit_msgs.msg -import geometry_msgs.msg -import tf - - -class MoveItPlanningDemo: - def __init__(self): - rospy.init_node('moveit_avoid_obstacles', anonymous=True) - # 初始化MoveIt - moveit_commander.roscpp_initialize(sys.argv) - - # 创建RobotCommander对象 - self.robot = moveit_commander.RobotCommander() - - # 创建PlanningSceneInterface对象 - self.scene = moveit_commander.PlanningSceneInterface() - - # 创建MoveGroupCommander对象 - self.arm_group = moveit_commander.MoveGroupCommander("arm_group") - - # 获取末端关节的名称 - self.end_effector_link = self.arm_group.get_end_effector_link() - - # 设置目标位置所使用的坐标参考系 - self.reference_frame = 'base' - self.arm_group.set_pose_reference_frame(self.reference_frame) - - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm_group.set_goal_position_tolerance(0.01) - self.arm_group.set_goal_orientation_tolerance(0.05) - - # 当运动规划失败后，允许重新规划 - self.arm_group.allow_replanning(True) - # 设置规划的最大时间为20秒 - self.arm_group.set_planning_time(20) - # 设置规划尝试次数为10次（或者更大的值） - self.arm_group.set_num_planning_attempts(20) - - - def add_scene(self): - # 添加第一个圆柱作为障碍物（垂直于平面） - cylinder1_pose = geometry_msgs.msg.PoseStamped() - cylinder1_pose.header.frame_id = self.robot.get_planning_frame() - cylinder1_pose.pose.position.x = 0.15 - cylinder1_pose.pose.position.y = 0 - cylinder1_pose.pose.position.z = 0.30 - cylinder1_pose.pose.orientation.w = 1.0 - self.scene.add_cylinder("cylinder1", cylinder1_pose, height=0.6, radius=0.01) - - # 添加第二个圆柱作为障碍物（水平于平面，构成十字架） - cylinder2_pose = geometry_msgs.msg.PoseStamped() - cylinder2_pose.header.frame_id = self.robot.get_planning_frame() - cylinder2_pose.pose.position.x = 0.15 - cylinder2_pose.pose.position.y = 0 - cylinder2_pose.pose.position.z = 0.40 - cylinder2_pose.pose.orientation.w = 1.0 - cylinder2_pose.pose.orientation.x = 0.707 # 围绕x轴旋转90度（水平方向） - cylinder2_pose.pose.orientation.y = 0.0 - cylinder2_pose.pose.orientation.z = 0.0 - cylinder2_pose.pose.orientation.w = 0.707 - self.scene.add_cylinder("cylinder2", cylinder2_pose, height=0.6, radius=0.02) - # 发布当前场景信息 - planning_scene = moveit_msgs.msg.PlanningScene() - planning_scene.world.collision_objects.extend(self.scene.get_objects().values()) - planning_scene.is_diff = True - - planning_scene_publisher = rospy.Publisher('/planning_scene', moveit_msgs.msg.PlanningScene, queue_size=1) - planning_scene_publisher.publish(planning_scene) - rospy.sleep(2) - - def robot_move(self): - - # 控制机械臂回到初始化位置 - self.arm_group.set_named_target('init_pose') - self.arm_group.go() - rospy.sleep(3) - - - # 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示 - target_pose = geometry_msgs.msg.PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.142 # 设置目标点的x坐标 - target_pose.pose.position.y = -0.140 # 设置目标点的y坐标 - target_pose.pose.position.z = 0.075 # 设置目标点的z坐标 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # 更新当前的位姿 - self.arm_group.set_start_state_to_current_state() - - # 获取机械臂当前的关节状态 - current_joint_values = self.arm_group.get_current_joint_values() - - # 打印当前关节状态 - print("Current Joint Values:", current_joint_values) - print("end Joint Values:", self.end_effector_link) - # 设置机械臂的目标姿态 - self.arm_group.set_pose_target(target_pose, self.end_effector_link) - # 进行运动规划 - plan = self.arm_group.plan() - # print('plan point:', plan[1]) - # 执行运动 - self.arm_group.execute(plan[1]) - rospy.sleep(3) - # 获取末端执行器的姿态 - end_effector_pose = self.arm_group.get_current_pose().pose - - # 打印末端执行器的坐标位置 - print("End Effector Position:", end_effector_pose.position) - print("End Effector Orientation:", end_effector_pose.orientation) - # 控制机械臂末端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - # self.arm_group.shift_pose_target(1, 0.22, self.end_effector_link) - # self.arm_group.go() - # rospy.sleep(5) - - # 设置机械臂的目标位置，使用7轴的位置数据进行描述（单位：弧度） - # joint_pose = [0.2967, 0, 0, -1.57000, 0, -1.3439, 0] - # joint_pose = [0.2967, 0, 0, 0, 0, -1.3439, 0] - # arm_group.set_joint_value_target(joint_pose) - - # 控制机械臂完成运动 - # arm_group.go() - # rospy.sleep(10) - # 控制机械臂回到初始化位置 - # arm_group.set_named_target('init_pose') - # arm_group.go() - - def run(self): - # 移除所有障碍物 - # self.scene.remove_world_object("cylinder1") - # self.scene.remove_world_object("cylinder2") - # 没有障碍物运行一次 - # self.robot_move() - - # 增加障碍物 - self.add_scene() - rospy.sleep(3) - # 获取当前场景中的所有障碍物 - current_obstacles = self.scene.get_known_object_names() - rospy.loginfo("Current obstacles in the scene: %s", current_obstacles) - rospy.sleep(2) - # 有障碍物后再运行一次 - self.robot_move() - # 关闭MoveIt - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == '__main__': - try: - obstacle = MoveItPlanningDemo() - obstacle.run() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280/mycobot_280/CMakeLists.txt b/mycobot_280/mycobot_280/CMakeLists.txt index 1d247587..4998774a 100644 --- a/mycobot_280/mycobot_280/CMakeLists.txt +++ b/mycobot_280/mycobot_280/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py scripts/follow_display_gripper.py scripts/slider_control_gripper.py diff --git a/mycobot_280/mycobot_280/launch/detect_marker.launch b/mycobot_280/mycobot_280/launch/detect_marker.launch deleted file mode 100644 index 27c9c176..00000000 --- a/mycobot_280/mycobot_280/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280/launch/detect_marker_with_topic.launch b/mycobot_280/mycobot_280/launch/detect_marker_with_topic.launch deleted file mode 100644 index dc3a58b7..00000000 --- a/mycobot_280/mycobot_280/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280/scripts/detect_marker.py b/mycobot_280/mycobot_280/scripts/detect_marker.py deleted file mode 100755 index 4283ea1c..00000000 --- a/mycobot_280/mycobot_280/scripts/detect_marker.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have.如果没有，则计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker.检测 aruco 标记 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data.处理标记数据 - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测marker位姿。 - # argument: - # marker corners,标记角 - # marker size (meter),标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker.只需选择第一个检测到的标记。 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1]，根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mycobot_280/mycobot_280/scripts/follow_and_pump.py b/mycobot_280/mycobot_280/scripts/follow_and_pump.py deleted file mode 100755 index d889bbb2..00000000 --- a/mycobot_280/mycobot_280/scripts/follow_and_pump.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message communicated with mycobot，与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# instantiate the message object，实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 通过该变量限制，抓取行为只做一次 -flag = False - -# In order to compare whether the QR code moves later,为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates,发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle,发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status,发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves""" - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function,回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value,解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者,subscribers to mark information - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mycobot_280/mycobot_280/scripts/following_marker.py b/mycobot_280/mycobot_280/scripts/following_marker.py deleted file mode 100755 index 797ec1ed..00000000 --- a/mycobot_280/mycobot_280/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial,标记位置初始化 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280/mycobot_280_gripper_moveit/CMakeLists.txt b/mycobot_280/mycobot_280_gripper_moveit/CMakeLists.txt index f8353d6e..35eae6b8 100644 --- a/mycobot_280/mycobot_280_gripper_moveit/CMakeLists.txt +++ b/mycobot_280/mycobot_280_gripper_moveit/CMakeLists.txt @@ -13,7 +13,6 @@ catkin_package() ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/sync_plan.py - scripts/path_planning_and_obstacle_avoidance_demo.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_280/mycobot_280_gripper_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_280/mycobot_280_gripper_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index cd4024d6..00000000 --- a/mycobot_280/mycobot_280_gripper_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # API to initialize move_group,初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # Initialize the ROS node,初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # Initialize the scene object to monitor changes in the external environment - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # Initialize self.arm group in the robotic arm that needs to be controlled by move group - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # Get the name of the terminal link,获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # Set the reference coordinate system used for the target position - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # Set the allowable error of position (unit: meter) and attitude (unit: radian) - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # Control the robotic arm to return to the initialization position first - # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # Pose is described by quaternion, based on base_link coordinate system - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # Set the current state of the robot arm as the initial state of motion - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # Set the target pose of the terminal motion of the robotic arm - # 设置机械臂终端运动的目标位姿 - self.arm.set_pose_target(target_pose, self.end_effector_link) - - # Plan the movement path,规划运动路径 - traj = self.arm.plan() - - # Control the motion of the robotic arm according to the planned motion path - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # Run once without obstacles,没有障碍物运行一次 - self.moving() - - # Add environment,添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # Run it again if there is an environmental impact,有环境影响后再运行一次 - self.moving() - - # close and exit moveit，关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run() diff --git a/mycobot_280/mycobot_280_moveit/CMakeLists.txt b/mycobot_280/mycobot_280_moveit/CMakeLists.txt index e3148b06..7215247e 100644 --- a/mycobot_280/mycobot_280_moveit/CMakeLists.txt +++ b/mycobot_280/mycobot_280_moveit/CMakeLists.txt @@ -13,7 +13,6 @@ catkin_package() ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/sync_plan.py - scripts/path_planning_and_obstacle_avoidance_demo.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_280/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_280/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index cd4024d6..00000000 --- a/mycobot_280/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # API to initialize move_group,初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # Initialize the ROS node,初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # Initialize the scene object to monitor changes in the external environment - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # Initialize self.arm group in the robotic arm that needs to be controlled by move group - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # Get the name of the terminal link,获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # Set the reference coordinate system used for the target position - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # Set the allowable error of position (unit: meter) and attitude (unit: radian) - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # Control the robotic arm to return to the initialization position first - # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # Pose is described by quaternion, based on base_link coordinate system - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # Set the current state of the robot arm as the initial state of motion - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # Set the target pose of the terminal motion of the robotic arm - # 设置机械臂终端运动的目标位姿 - self.arm.set_pose_target(target_pose, self.end_effector_link) - - # Plan the movement path,规划运动路径 - traj = self.arm.plan() - - # Control the motion of the robotic arm according to the planned motion path - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # Run once without obstacles,没有障碍物运行一次 - self.moving() - - # Add environment,添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # Run it again if there is an environmental impact,有环境影响后再运行一次 - self.moving() - - # close and exit moveit，关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run() diff --git a/mycobot_280/mycobot_280arduino/CMakeLists.txt b/mycobot_280/mycobot_280arduino/CMakeLists.txt index 0a118774..bfc6079a 100644 --- a/mycobot_280/mycobot_280arduino/CMakeLists.txt +++ b/mycobot_280/mycobot_280arduino/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_280/mycobot_280arduino/launch/detect_marker.launch b/mycobot_280/mycobot_280arduino/launch/detect_marker.launch deleted file mode 100644 index 92fa48d0..00000000 --- a/mycobot_280/mycobot_280arduino/launch/detect_marker.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280arduino/launch/detect_marker_with_topic.launch b/mycobot_280/mycobot_280arduino/launch/detect_marker_with_topic.launch deleted file mode 100644 index a5dc695f..00000000 --- a/mycobot_280/mycobot_280arduino/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280arduino/scripts/detect_marker.py b/mycobot_280/mycobot_280arduino/scripts/detect_marker.py deleted file mode 100644 index 04ca4834..00000000 --- a/mycobot_280/mycobot_280arduino/scripts/detect_marker.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. - # argument: - # marker corners - # marker size (meter) - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mycobot_280/mycobot_280arduino/scripts/follow_and_pump.py b/mycobot_280/mycobot_280arduino/scripts/follow_and_pump.py deleted file mode 100644 index f26b669b..00000000 --- a/mycobot_280/mycobot_280arduino/scripts/follow_and_pump.py +++ /dev/null @@ -1,190 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# 与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# 通过该变量限制，抓取行为只做一次 -flag = False - -# 为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mycobot_280/mycobot_280arduino/scripts/following_marker.py b/mycobot_280/mycobot_280arduino/scripts/following_marker.py deleted file mode 100644 index 4afe3f97..00000000 --- a/mycobot_280/mycobot_280arduino/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280/mycobot_280arduino_moveit/CMakeLists.txt b/mycobot_280/mycobot_280arduino_moveit/CMakeLists.txt index ec2a1285..6840d708 100644 --- a/mycobot_280/mycobot_280arduino_moveit/CMakeLists.txt +++ b/mycobot_280/mycobot_280arduino_moveit/CMakeLists.txt @@ -13,7 +13,6 @@ catkin_package() ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/sync_plan.py - scripts/path_planning_and_obstacle_avoidance_demo.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_280/mycobot_280arduino_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_280/mycobot_280arduino_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index f47c0e86..00000000 --- a/mycobot_280/mycobot_280arduino_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # 初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # 初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # 获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # 当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # 设置机械臂终端运动的目标位姿 - self.arm.set_pose_target(target_pose, self.end_effector_link) - - # 规划运动路径 - traj = self.arm.plan() - - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # 没有障碍物运行一次 - self.moving() - - # 添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # 有环境影响后在运行一次 - self.moving() - - # 关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run() diff --git a/mycobot_280/mycobot_280pi/CMakeLists.txt b/mycobot_280/mycobot_280pi/CMakeLists.txt index 67af0dcf..cabdd6fb 100644 --- a/mycobot_280/mycobot_280pi/CMakeLists.txt +++ b/mycobot_280/mycobot_280pi/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_280/mycobot_280pi/config/mycobot_with_marker.rviz b/mycobot_280/mycobot_280pi/config/mycobot_with_marker.rviz deleted file mode 100644 index 8b0dc7f4..00000000 --- a/mycobot_280/mycobot_280pi/config/mycobot_with_marker.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Marker1 - - /Marker1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - 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: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - joint1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - basic_shapes: - Value: true - joint1: - Value: true - joint2: - Value: true - joint3: - Value: true - joint4: - Value: true - joint5: - Value: true - joint6: - Value: true - joint6_flange: - Value: true - Marker Scale: 0.300000012 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - joint1: - joint2: - joint3: - joint4: - joint5: - joint6: - joint6_flange: - basic_shapes: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - basic_cube: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: joint1 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.11990476 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.375398338 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.235389769 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 diff --git a/mycobot_280/mycobot_280pi/launch/detect_marker.launch b/mycobot_280/mycobot_280pi/launch/detect_marker.launch deleted file mode 100644 index 0d4016f1..00000000 --- a/mycobot_280/mycobot_280pi/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280pi/launch/detect_marker_with_topic.launch b/mycobot_280/mycobot_280pi/launch/detect_marker_with_topic.launch deleted file mode 100644 index 58d9354b..00000000 --- a/mycobot_280/mycobot_280pi/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280pi/scripts/detect_marker.py b/mycobot_280/mycobot_280pi/scripts/detect_marker.py deleted file mode 100755 index 2d2e8ec4..00000000 --- a/mycobot_280/mycobot_280pi/scripts/detect_marker.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python -# encoding=utf-8 -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have.如果没有，则计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker.检测 aruco 标记 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data.处理标记数据 - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测marker位姿。 - # argument: - # marker corners,标记角 - # marker size (meter),标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker.只需选择第一个检测到的标记。 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1]，根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mycobot_280/mycobot_280pi/scripts/follow_and_pump.py b/mycobot_280/mycobot_280pi/scripts/follow_and_pump.py deleted file mode 100755 index 73c4f3b9..00000000 --- a/mycobot_280/mycobot_280pi/scripts/follow_and_pump.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message communicated with mycobot，与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyAMA*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# instantiate the message object，实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 通过该变量限制，抓取行为只做一次 -flag = False - -# In order to compare whether the QR code moves later,为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates,发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle,发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status,发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves""" - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function,回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value,解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者,subscribers to mark information - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mycobot_280/mycobot_280pi/scripts/following_marker.py b/mycobot_280/mycobot_280pi/scripts/following_marker.py deleted file mode 100755 index ae1c624f..00000000 --- a/mycobot_280/mycobot_280pi/scripts/following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# encoding=utf-8 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial,标记位置初始化 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280/mycobot_280pi_moveit/CMakeLists.txt b/mycobot_280/mycobot_280pi_moveit/CMakeLists.txt index d5a123f7..0c61979e 100644 --- a/mycobot_280/mycobot_280pi_moveit/CMakeLists.txt +++ b/mycobot_280/mycobot_280pi_moveit/CMakeLists.txt @@ -13,7 +13,6 @@ catkin_package() ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/sync_plan.py - scripts/path_planning_and_obstacle_avoidance_demo.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_280/mycobot_280pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_280/mycobot_280pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index cd4024d6..00000000 --- a/mycobot_280/mycobot_280pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # API to initialize move_group,初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # Initialize the ROS node,初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # Initialize the scene object to monitor changes in the external environment - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # Initialize self.arm group in the robotic arm that needs to be controlled by move group - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # Get the name of the terminal link,获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # Set the reference coordinate system used for the target position - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # Set the allowable error of position (unit: meter) and attitude (unit: radian) - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # Control the robotic arm to return to the initialization position first - # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # Pose is described by quaternion, based on base_link coordinate system - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # Set the current state of the robot arm as the initial state of motion - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # Set the target pose of the terminal motion of the robotic arm - # 设置机械臂终端运动的目标位姿 - self.arm.set_pose_target(target_pose, self.end_effector_link) - - # Plan the movement path,规划运动路径 - traj = self.arm.plan() - - # Control the motion of the robotic arm according to the planned motion path - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # Run once without obstacles,没有障碍物运行一次 - self.moving() - - # Add environment,添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # Run it again if there is an environmental impact,有环境影响后再运行一次 - self.moving() - - # close and exit moveit，关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run() diff --git a/mycobot_320/mycobot_320/CMakeLists.txt b/mycobot_320/mycobot_320/CMakeLists.txt index 4394d65b..e35af22e 100644 --- a/mycobot_320/mycobot_320/CMakeLists.txt +++ b/mycobot_320/mycobot_320/CMakeLists.txt @@ -36,9 +36,6 @@ catkin_install_python(PROGRAMS scripts/mycobot_320_teleop_keyboard.py scripts/mycobot_320_listen_real.py scripts/mycobot_320_listen_real_of_topic.py - scripts/mycobot_320_detect_marker.py - scripts/mycobot_320_following_marker.py - scripts/mycobot_320_follow_and_pump.py scripts/mycobot_320_simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mycobot_320/mycobot_320/launch/mycobot_320_detect_marker.launch b/mycobot_320/mycobot_320/launch/mycobot_320_detect_marker.launch deleted file mode 100644 index bc9b0e10..00000000 --- a/mycobot_320/mycobot_320/launch/mycobot_320_detect_marker.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/mycobot_320/mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch b/mycobot_320/mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch deleted file mode 100644 index ccfbfdad..00000000 --- a/mycobot_320/mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_320/mycobot_320/scripts/mycobot_320_detect_marker.py b/mycobot_320/mycobot_320/scripts/mycobot_320_detect_marker.py deleted file mode 100755 index 42bde897..00000000 --- a/mycobot_320/mycobot_320/scripts/mycobot_320_detect_marker.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. - # argument: - # marker corners - # marker size (meter) - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mycobot_320/mycobot_320/scripts/mycobot_320_follow_and_pump.py b/mycobot_320/mycobot_320/scripts/mycobot_320_follow_and_pump.py deleted file mode 100755 index ef07a9be..00000000 --- a/mycobot_320/mycobot_320/scripts/mycobot_320_follow_and_pump.py +++ /dev/null @@ -1,190 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding:utf-8 -*- -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# 与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# 通过该变量限制，抓取行为只做一次 -flag = False - -# 为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mycobot_320/mycobot_320/scripts/mycobot_320_following_marker.py b/mycobot_320/mycobot_320/scripts/mycobot_320_following_marker.py deleted file mode 100755 index 6de24277..00000000 --- a/mycobot_320/mycobot_320/scripts/mycobot_320_following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding:utf-8 -*- -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_320/new_mycobot_320/CMakeLists.txt b/mycobot_320/new_mycobot_320/CMakeLists.txt index 4b1e0a04..d5e2470a 100644 --- a/mycobot_320/new_mycobot_320/CMakeLists.txt +++ b/mycobot_320/new_mycobot_320/CMakeLists.txt @@ -70,9 +70,6 @@ catkin_install_python(PROGRAMS scripts/mycobot_320_teleop_keyboard.py scripts/mycobot_320_listen_real.py scripts/mycobot_320_listen_real_of_topic.py - scripts/mycobot_320_detect_marker.py - scripts/mycobot_320_following_marker.py - scripts/mycobot_320_follow_and_pump.py scripts/mycobot_320_simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker.launch b/mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker.launch deleted file mode 100644 index b717bac6..00000000 --- a/mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch b/mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch deleted file mode 100644 index 180d76c8..00000000 --- a/mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_320/new_mycobot_320/scripts/mycobot_320_detect_marker.py b/mycobot_320/new_mycobot_320/scripts/mycobot_320_detect_marker.py deleted file mode 100755 index 42bde897..00000000 --- a/mycobot_320/new_mycobot_320/scripts/mycobot_320_detect_marker.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python -# -*- coding:utf-8 -*- -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. - # argument: - # marker corners - # marker size (meter) - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mycobot_320/new_mycobot_320/scripts/mycobot_320_follow_and_pump.py b/mycobot_320/new_mycobot_320/scripts/mycobot_320_follow_and_pump.py deleted file mode 100755 index ef07a9be..00000000 --- a/mycobot_320/new_mycobot_320/scripts/mycobot_320_follow_and_pump.py +++ /dev/null @@ -1,190 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding:utf-8 -*- -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# 与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# 通过该变量限制，抓取行为只做一次 -flag = False - -# 为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mycobot_320/new_mycobot_320/scripts/mycobot_320_following_marker.py b/mycobot_320/new_mycobot_320/scripts/mycobot_320_following_marker.py deleted file mode 100755 index 6de24277..00000000 --- a/mycobot_320/new_mycobot_320/scripts/mycobot_320_following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding:utf-8 -*- -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_320/new_mycobot_320_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_320/new_mycobot_320_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100755 index cd4024d6..00000000 --- a/mycobot_320/new_mycobot_320_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # API to initialize move_group,初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # Initialize the ROS node,初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # Initialize the scene object to monitor changes in the external environment - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # Initialize self.arm group in the robotic arm that needs to be controlled by move group - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # Get the name of the terminal link,获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # Set the reference coordinate system used for the target position - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # Set the allowable error of position (unit: meter) and attitude (unit: radian) - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # Control the robotic arm to return to the initialization position first - # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # Pose is described by quaternion, based on base_link coordinate system - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # Set the current state of the robot arm as the initial state of motion - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # Set the target pose of the terminal motion of the robotic arm - # 设置机械臂终端运动的目标位姿 - self.arm.set_pose_target(target_pose, self.end_effector_link) - - # Plan the movement path,规划运动路径 - traj = self.arm.plan() - - # Control the motion of the robotic arm according to the planned motion path - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # Run once without obstacles,没有障碍物运行一次 - self.moving() - - # Add environment,添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # Run it again if there is an environmental impact,有环境影响后再运行一次 - self.moving() - - # close and exit moveit，关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run() diff --git a/mycobot_320/new_mycobot_320_pi/CMakeLists.txt b/mycobot_320/new_mycobot_320_pi/CMakeLists.txt index d06d3d5e..def2ecd8 100644 --- a/mycobot_320/new_mycobot_320_pi/CMakeLists.txt +++ b/mycobot_320/new_mycobot_320_pi/CMakeLists.txt @@ -71,9 +71,6 @@ catkin_install_python(PROGRAMS scripts/mycobot_320_teleop_keyboard.py scripts/mycobot_320_listen_real.py scripts/mycobot_320_listen_real_of_topic.py - scripts/mycobot_320_detect_marker.py - scripts/mycobot_320_following_marker.py - scripts/mycobot_320_follow_and_pump.py scripts/mycobot_320_simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/mycobot_320/new_mycobot_320_pi/config/mycobot_320_with_marker.rviz b/mycobot_320/new_mycobot_320_pi/config/mycobot_320_with_marker.rviz deleted file mode 100644 index d343e8c8..00000000 --- a/mycobot_320/new_mycobot_320_pi/config/mycobot_320_with_marker.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Marker1 - - /Marker1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 662 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - 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: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - joint1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - basic_shapes: - Value: true - joint1: - Value: true - joint2: - Value: true - joint3: - Value: true - joint4: - Value: true - joint5: - Value: true - joint6: - Value: true - joint6_flange: - Value: true - Marker Scale: 0.300000012 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - joint1: - joint2: - joint3: - joint4: - joint5: - joint6: - joint6_flange: - basic_shapes: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - basic_cube: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1.20289087 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.440398335 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.430389732 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 903 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1853 - X: 65 - Y: 24 diff --git a/mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker.launch b/mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker.launch deleted file mode 100644 index cc31b76d..00000000 --- a/mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker_with_topic.launch b/mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker_with_topic.launch deleted file mode 100644 index bfe56e5e..00000000 --- a/mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker_with_topic.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_detect_marker.py b/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_detect_marker.py deleted file mode 100755 index 9d9e4ef9..00000000 --- a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_detect_marker.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. - # argument: - # marker corners - # marker size (meter) - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_follow_and_pump.py b/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_follow_and_pump.py deleted file mode 100755 index 01f88ce0..00000000 --- a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_follow_and_pump.py +++ /dev/null @@ -1,190 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# 控制 mycobot 的 topic，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging equipment: ttyUSB* is M5；ttyACM* is wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# 与 mycobot 真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# 通过该变量限制，抓取行为只做一次 -flag = False - -# 为了后面比较二维码是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # mark 信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_following_marker.py b/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_following_marker.py deleted file mode 100755 index 64b86d05..00000000 --- a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_320/new_mycobot_320_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_320/new_mycobot_320_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100755 index cd4024d6..00000000 --- a/mycobot_320/new_mycobot_320_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # API to initialize move_group,初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # Initialize the ROS node,初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # Initialize the scene object to monitor changes in the external environment - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # Initialize self.arm group in the robotic arm that needs to be controlled by move group - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # Get the name of the terminal link,获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # Set the reference coordinate system used for the target position - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # Set the allowable error of position (unit: meter) and attitude (unit: radian) - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # Control the robotic arm to return to the initialization position first - # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # Pose is described by quaternion, based on base_link coordinate system - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # Set the current state of the robot arm as the initial state of motion - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # Set the target pose of the terminal motion of the robotic arm - # 设置机械臂终端运动的目标位姿 - self.arm.set_pose_target(target_pose, self.end_effector_link) - - # Plan the movement path,规划运动路径 - traj = self.arm.plan() - - # Control the motion of the robotic arm according to the planned motion path - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # Run once without obstacles,没有障碍物运行一次 - self.moving() - - # Add environment,添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # Run it again if there is an environmental impact,有环境影响后再运行一次 - self.moving() - - # close and exit moveit，关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run() diff --git a/mypalletizer_260/mypalletizer_260/CMakeLists.txt b/mypalletizer_260/mypalletizer_260/CMakeLists.txt index fa0337f9..e8001edd 100644 --- a/mypalletizer_260/mypalletizer_260/CMakeLists.txt +++ b/mypalletizer_260/mypalletizer_260/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz b/mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz deleted file mode 100644 index 8b0dc7f4..00000000 --- a/mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Marker1 - - /Marker1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - 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: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - joint1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - basic_shapes: - Value: true - joint1: - Value: true - joint2: - Value: true - joint3: - Value: true - joint4: - Value: true - joint5: - Value: true - joint6: - Value: true - joint6_flange: - Value: true - Marker Scale: 0.300000012 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - joint1: - joint2: - joint3: - joint4: - joint5: - joint6: - joint6_flange: - basic_shapes: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - basic_cube: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: joint1 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.11990476 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.375398338 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.235389769 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 diff --git a/mypalletizer_260/mypalletizer_260/launch/detect_marker.launch b/mypalletizer_260/mypalletizer_260/launch/detect_marker.launch deleted file mode 100644 index e8c6998c..00000000 --- a/mypalletizer_260/mypalletizer_260/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch b/mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch deleted file mode 100644 index 75c61b81..00000000 --- a/mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mypalletizer_260/mypalletizer_260/scripts/detect_marker.py b/mypalletizer_260/mypalletizer_260/scripts/detect_marker.py deleted file mode 100644 index cae6db24..00000000 --- a/mypalletizer_260/mypalletizer_260/scripts/detect_marker.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img进来 - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. 如果没有就计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. 检测 aruco 标记。 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测标记姿势 - # argument: - # marker corners, 标记角 - # marker size (meter), 标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. 只需选择第一个检测到的标记 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - # 根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py b/mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py deleted file mode 100644 index f62112be..00000000 --- a/mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/usr/bin/env python2 -# coding:utf-8 -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message to communicate with mycobot -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制mycobot的话题，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging device: ttyUSB* is M5；ttyACM* is wio -# 判断设备：ttyUSB*为M5；ttyACM*为wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# Instantiate the message object. 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from the real position of mycobot -# 与mycobot真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 使用此变量限制，抓取行为仅执行一次 -flag = False - -# In order to compare whether the QR code moves later -# 为了比较二维码后面是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates 发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle. 发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status. 发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves. 判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function. 回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value. 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # subscribers to mark information, mark信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mypalletizer_260/mypalletizer_260/scripts/following_marker.py b/mypalletizer_260/mypalletizer_260/scripts/following_marker.py deleted file mode 100644 index 144a52d4..00000000 --- a/mypalletizer_260/mypalletizer_260/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker 标记 - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial. 标记初始位置 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mypalletizer_260/mypalletizer_260_pi/CMakeLists.txt b/mypalletizer_260/mypalletizer_260_pi/CMakeLists.txt index d738b0ba..6a129f81 100644 --- a/mypalletizer_260/mypalletizer_260_pi/CMakeLists.txt +++ b/mypalletizer_260/mypalletizer_260_pi/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mypalletizer_260/mypalletizer_260_pi/config/mycobot_with_marker.rviz b/mypalletizer_260/mypalletizer_260_pi/config/mycobot_with_marker.rviz deleted file mode 100644 index 8b0dc7f4..00000000 --- a/mypalletizer_260/mypalletizer_260_pi/config/mycobot_with_marker.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - - /Marker1 - - /Marker1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - 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: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - joint1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - joint6_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - basic_shapes: - Value: true - joint1: - Value: true - joint2: - Value: true - joint3: - Value: true - joint4: - Value: true - joint5: - Value: true - joint6: - Value: true - joint6_flange: - Value: true - Marker Scale: 0.300000012 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - joint1: - joint2: - joint3: - joint4: - joint5: - joint6: - joint6_flange: - basic_shapes: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - basic_cube: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: joint1 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.11990476 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.375398338 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.235389769 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 diff --git a/mypalletizer_260/mypalletizer_260_pi/launch/detect_marker.launch b/mypalletizer_260/mypalletizer_260_pi/launch/detect_marker.launch deleted file mode 100644 index e8c6998c..00000000 --- a/mypalletizer_260/mypalletizer_260_pi/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mypalletizer_260/mypalletizer_260_pi/launch/detect_marker_with_topic.launch b/mypalletizer_260/mypalletizer_260_pi/launch/detect_marker_with_topic.launch deleted file mode 100644 index c9470d0c..00000000 --- a/mypalletizer_260/mypalletizer_260_pi/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mypalletizer_260/mypalletizer_260_pi/scripts/detect_marker.py b/mypalletizer_260/mypalletizer_260_pi/scripts/detect_marker.py deleted file mode 100644 index 4fb2b71a..00000000 --- a/mypalletizer_260/mypalletizer_260_pi/scripts/detect_marker.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. 订阅者，监听是否有img进来 - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。 - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. 如果没有就计算相机矩阵 - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. 检测 aruco 标记。 - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. 检测标记姿势 - # argument: - # marker corners, 标记角 - # marker size (meter), 标记尺寸（米） - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. 只需选择第一个检测到的标记 - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. 为ros获取四元数 - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - # 根据 [joint1] 变换姿势 - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/mypalletizer_260/mypalletizer_260_pi/scripts/follow_and_pump.py b/mypalletizer_260/mypalletizer_260_pi/scripts/follow_and_pump.py deleted file mode 100644 index 15ea732d..00000000 --- a/mypalletizer_260/mypalletizer_260_pi/scripts/follow_and_pump.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- -import rospy -from visualization_msgs.msg import Marker -import time -import os - -# Type of message to communicate with mycobot -# 与 mycobot 通信的消息类型 -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) - -# Control the topic of mycobot, followed by angle, coordinates, gripper -# 控制mycobot的话题，依次是角度、坐标、夹爪 -angle_pub = rospy.Publisher("mycobot/angles_goal", - MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", - MycobotSetCoords, queue_size=5) -# Judging device: ttyUSB* is M5；ttyACM* is wio -# 判断设备：ttyUSB*为M5；ttyACM*为wio -robot = os.popen("ls /dev/ttyUSB*").readline() - -if "dev" in robot: - Pin = [2, 5] -else: - Pin = [20, 21] - -pump_pub = rospy.Publisher("mycobot/pump_status", - MycobotPumpStatus, queue_size=5) - -# Instantiate the message object. 实例化消息对象 -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -# Deviation value from the real position of mycobot -# 与mycobot真实位置的偏差值 -x_offset = -20 -y_offset = 20 -z_offset = 110 - -# With this variable limit, the fetching behavior is only done once -# 使用此变量限制，抓取行为仅执行一次 -flag = False - -# In order to compare whether the QR code moves later -# 为了比较二维码后面是否移动 -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - """Post coordinates 发布坐标""" - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - """Publishing angle. 发布角度""" - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag, Pin): - """Publish gripper status. 发布夹爪状态""" - pump.Status = flag - pump.Pin1 = Pin[0] - pump.Pin2 = Pin[1] - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - """Determine whether the target moves. 判断目标是否移动""" - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - """callback function. 回调函数""" - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # Parse out the coordinate value. 解析出坐标值 - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - # When the running time is less than 30s, or the target position is still changing, perform tracking behavior - # 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为 - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取 - - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True, Pin) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, - - 177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False, Pin) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False, Pin) - # time.sleep(2.5) - - # subscribers to mark information, mark信息的订阅者 - rospy.Subscriber("visualization_marker", Marker, - grippercallback, queue_size=1) - - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/mypalletizer_260/mypalletizer_260_pi/scripts/following_marker.py b/mypalletizer_260/mypalletizer_260_pi/scripts/following_marker.py deleted file mode 100644 index 5bb0065e..00000000 --- a/mypalletizer_260/mypalletizer_260_pi/scripts/following_marker.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker 标记 - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial. 标记初始位置 - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass