diff --git a/mycobot_280jn/CMakeLists.txt b/mycobot_280jn/CMakeLists.txt deleted file mode 100644 index 8ce314b..0000000 --- a/mycobot_280jn/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mycobot_280jn) -add_compile_options(-std=c++11) - -## Find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - actionlib - image_transport - cv_bridge -) - -## Declare a catkin package -catkin_package( - CATKIN_DEPENDS std_msgs actionlib -) - -## Build talker and listener -include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) - -catkin_install_python(PROGRAMS - scripts/follow_display.py - scripts/slider_control.py - 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} -) - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "setup_assistant.launch" EXCLUDE) -install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - - diff --git a/mycobot_280jn/LICENSE b/mycobot_280jn/LICENSE deleted file mode 100644 index b8468e6..0000000 --- a/mycobot_280jn/LICENSE +++ /dev/null @@ -1,25 +0,0 @@ -BSD 2-Clause License - -Copyright (c) 2020, Elephant Robotics -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/mycobot_280jn/config/mycobot.rviz b/mycobot_280jn/config/mycobot.rviz deleted file mode 100644 index 51ba33e..0000000 --- a/mycobot_280jn/config/mycobot.rviz +++ /dev/null @@ -1,203 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - 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 - 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: - {} - Update Interval: 0 - 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: 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: 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_280jn/config/mycobot_with_marker.rviz b/mycobot_280jn/config/mycobot_with_marker.rviz deleted file mode 100644 index 8b0dc7f..0000000 --- a/mycobot_280jn/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_280jn/launch/detect_marker.launch b/mycobot_280jn/launch/detect_marker.launch deleted file mode 100644 index 444638e..0000000 --- a/mycobot_280jn/launch/detect_marker.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280jn/launch/detect_marker_with_topic.launch b/mycobot_280jn/launch/detect_marker_with_topic.launch deleted file mode 100644 index 2bba726..0000000 --- a/mycobot_280jn/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280jn/launch/mycobot_follow.launch b/mycobot_280jn/launch/mycobot_follow.launch deleted file mode 100644 index 7511e1f..0000000 --- a/mycobot_280jn/launch/mycobot_follow.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - ["joint_states"] - - - - diff --git a/mycobot_280jn/launch/simple_gui.launch b/mycobot_280jn/launch/simple_gui.launch deleted file mode 100644 index 0a8d47a..0000000 --- a/mycobot_280jn/launch/simple_gui.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280jn/launch/slider_control.launch b/mycobot_280jn/launch/slider_control.launch deleted file mode 100644 index ac42e42..0000000 --- a/mycobot_280jn/launch/slider_control.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280jn/launch/teleop_keyboard.launch b/mycobot_280jn/launch/teleop_keyboard.launch deleted file mode 100644 index 6e70fd2..0000000 --- a/mycobot_280jn/launch/teleop_keyboard.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280jn/launch/test.launch b/mycobot_280jn/launch/test.launch deleted file mode 100644 index dbb1f00..0000000 --- a/mycobot_280jn/launch/test.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/mycobot_280jn/package.xml b/mycobot_280jn/package.xml deleted file mode 100644 index 8335d7e..0000000 --- a/mycobot_280jn/package.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - mycobot_280jn - 0.3.0 - The mycobot 280jn package - - ZhangLijun - ZhangLijun - - BSD - - https://github.com/elephantrobotics/mycobot_ros - - catkin - - roscpp - rospy - std_msgs - actionlib - mycobot_description - mycobot_communication - - mycobot_communication - mycobot_description - - roscpp - rospy - std_msgs - actionlib - joint_state_publisher - joint_state_publisher_gui - robot_state_publisher - xacro - joy - rviz - controller_manager - python-tk - mycobot_description - mycobot_communication - - - - - - - - diff --git a/mycobot_280jn/scripts/detect_marker.py b/mycobot_280jn/scripts/detect_marker.py deleted file mode 100644 index 4283ea1..0000000 --- a/mycobot_280jn/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_280jn/scripts/follow_and_pump.py b/mycobot_280jn/scripts/follow_and_pump.py deleted file mode 100644 index 3a26319..0000000 --- a/mycobot_280jn/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) - - # 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/mycobot_280jn/scripts/follow_display.py b/mycobot_280jn/scripts/follow_display.py deleted file mode 100644 index 0815622..0000000 --- a/mycobot_280jn/scripts/follow_display.py +++ /dev/null @@ -1,103 +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 - -from pymycobot.mycobot import MyCobot - - -def talker(): - rospy.init_node("display", anonymous=True) - - print("Try connect real mycobot...") - port = rospy.get_param("~port", "/dev/ttyTHS1") - baud = rospy.get_param("~baud", 1000000) - print("port: {}, baud: {}\n".format(port, baud)) - try: - mycobot = MyCobot(port, baud) - except Exception as e: - print(e) - print( - """\ - \rTry connect mycobot failed! - \rPlease check wether connected with mycobot. - \rPlease chckt wether the port or baud is right. - """ - ) - exit(1) - mycobot.release_all_servos() - time.sleep(0.1) - print("Rlease all servos over.\n") - - pub = rospy.Publisher("joint_states", JointState, queue_size=10) - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(30) # 30hz - - # pub joint state - joint_state_send = JointState() - joint_state_send.header = Header() - - joint_state_send.name = [ - "joint2_to_joint1", - "joint3_to_joint2", - "joint4_to_joint3", - "joint5_to_joint4", - "joint6_to_joint5", - "joint6output_to_joint6", - ] - joint_state_send.velocity = [0] - joint_state_send.effort = [] - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "my_namespace" - - print("publishing ...") - while not rospy.is_shutdown(): - joint_state_send.header.stamp = rospy.Time.now() - - angles = mycobot.get_radians() - data_list = [] - for index, value in enumerate(angles): - data_list.append(value) - - # rospy.loginfo('{}'.format(data_list)) - joint_state_send.position = data_list - - pub.publish(joint_state_send) - - coords = mycobot.get_coords() - - # marker - marker_.header.stamp = rospy.Time.now() - marker_.type = marker_.SPHERE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial.标记位置初始 - # print(coords) - if not coords: - coords = [0, 0, 0, 0, 0, 0] - rospy.loginfo("error [101]: can not get coord values") - - marker_.pose.position.x = coords[1] / 1000 * -1 - marker_.pose.position.y = coords[0] / 1000 - marker_.pose.position.z = coords[2] / 1000 - - 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_280jn/scripts/following_marker.py b/mycobot_280jn/scripts/following_marker.py deleted file mode 100644 index 797ec1e..0000000 --- a/mycobot_280jn/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_280jn/scripts/listen_real.py b/mycobot_280jn/scripts/listen_real.py deleted file mode 100644 index 5e4d9bc..0000000 --- a/mycobot_280jn/scripts/listen_real.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2 -# license removed for brevity -import time -import math - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from mycobot_communication.srv import GetAngles - - -def talker(): - rospy.loginfo("start ...") - rospy.init_node("real_listener", anonymous=True) - pub = rospy.Publisher("joint_states", JointState, queue_size=10) - rate = rospy.Rate(30) # 30hz - - # pub joint state，发布关节状态 - joint_state_send = JointState() - joint_state_send.header = Header() - - joint_state_send.name = [ - "joint2_to_joint1", - "joint3_to_joint2", - "joint4_to_joint3", - "joint5_to_joint4", - "joint6_to_joint5", - "joint6output_to_joint6", - ] - joint_state_send.velocity = [0] - joint_state_send.effort = [] - - # waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用 - rospy.loginfo("wait service") - rospy.wait_for_service("get_joint_angles") - func = rospy.ServiceProxy("get_joint_angles", GetAngles) - - rospy.loginfo("start loop ...") - while not rospy.is_shutdown(): - # get real angles from server.从服务器获得真实的角度。 - res = func() - if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: - continue - radians_list = [ - res.joint_1 * (math.pi / 180), - res.joint_2 * (math.pi / 180), - res.joint_3 * (math.pi / 180), - res.joint_4 * (math.pi / 180), - res.joint_5 * (math.pi / 180), - res.joint_6 * (math.pi / 180), - ] - rospy.loginfo("res: {}".format(radians_list)) - - # publish angles.发布角度 - joint_state_send.header.stamp = rospy.Time.now() - joint_state_send.position = radians_list - pub.publish(joint_state_send) - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280jn/scripts/listen_real_of_topic.py b/mycobot_280jn/scripts/listen_real_of_topic.py deleted file mode 100644 index ea3df92..0000000 --- a/mycobot_280jn/scripts/listen_real_of_topic.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python2 -import math - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from mycobot_communication.msg import MycobotAngles - - -class Listener(object): - def __init__(self): - super(Listener, self).__init__() - - rospy.loginfo("start ...") - rospy.init_node("real_listener_1", anonymous=True) - # init publisher.初始化发布者 - self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) - # init subscriber.初始化订阅者 - self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback) - rospy.spin() - - def callback(self, data): - """`mycobot/angles_real` subscriber callback method. - - Args: - data (MycobotAngles): callback argument. - """ - # ini publisher object. 初始化发布者对象 - joint_state_send = JointState() - joint_state_send.header = Header() - - joint_state_send.name = [ - "joint2_to_joint1", - "joint3_to_joint2", - "joint4_to_joint3", - "joint5_to_joint4", - "joint6_to_joint5", - "joint6output_to_joint6", - ] - joint_state_send.velocity = [0] - joint_state_send.effort = [] - joint_state_send.header.stamp = rospy.Time.now() - - # process callback data. 处理回调数据。 - radians_list = [ - data.joint_1 * (math.pi / 180), - data.joint_2 * (math.pi / 180), - data.joint_3 * (math.pi / 180), - data.joint_4 * (math.pi / 180), - data.joint_5 * (math.pi / 180), - data.joint_6 * (math.pi / 180), - ] - rospy.loginfo("res: {}".format(radians_list)) - - joint_state_send.position = radians_list - self.pub.publish(joint_state_send) - - -if __name__ == "__main__": - try: - Listener() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280jn/scripts/simple_gui.py b/mycobot_280jn/scripts/simple_gui.py deleted file mode 100644 index 9f4c791..0000000 --- a/mycobot_280jn/scripts/simple_gui.py +++ /dev/null @@ -1,478 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -import Tkinter as tk -from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus -import rospy -import time -from rospy import ServiceException - - -class Window: - def __init__(self, handle): - self.win = handle - self.win.resizable(0, 0) # fixed window size，固定窗口大小 - - self.model = 0 - self.speed = rospy.get_param("~speed", 50) - - # set default speed，设置默认速度 - self.speed_d = tk.StringVar() - self.speed_d.set(str(self.speed)) - # print(self.speed) - self.connect_ser() - - # Get the data of the robotic arm，获取机械臂数据 - self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model] - self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model] - self.get_date() - - # get screen width and height.获取屏幕宽度和高度 - self.ws = self.win.winfo_screenwidth() # width of the screen - self.hs = self.win.winfo_screenheight() # height of the screen - # calculate x and y coordinates for the Tk root window - # 计算 Tk 根窗口的 x 和 y 坐标 - x = (self.ws / 2) - 190 - y = (self.hs / 2) - 250 - self.win.geometry("430x370+{}+{}".format(x, y)) - # layout,布局 - self.set_layout() - # input section,输入部分 - self.need_input() - # Show part,展示部分 - self.show_init() - - # Set the joint buttons 设置joint按钮 - tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid( - row=6, column=1, sticky="w", padx=3, pady=2 - ) - - # coordination settings button,coordination 设置按钮 - tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid( - row=6, column=1, sticky="w", padx=3, pady=2 - ) - - # Gripper switch button,夹爪开关按钮 - tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid( - row=1, column=0, sticky="w", padx=3, pady=50 - ) - tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid( - row=1, column=1, sticky="w", padx=3, pady=2 - ) - - def connect_ser(self): - rospy.init_node("simple_gui", anonymous=True, disable_signals=True) - - rospy.wait_for_service("get_joint_angles") - rospy.wait_for_service("set_joint_angles") - rospy.wait_for_service("get_joint_coords") - rospy.wait_for_service("set_joint_coords") - rospy.wait_for_service("switch_gripper_status") - try: - self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords) - self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) - self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles) - self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) - self.switch_gripper = rospy.ServiceProxy( - "switch_gripper_status", GripperStatus - ) - except: - print("start error ...") - exit(1) - - print("Connect service success.") - - def set_layout(self): - self.frmLT = tk.Frame(width=200, height=200) - self.frmLC = tk.Frame(width=200, height=200) - self.frmLB = tk.Frame(width=200, height=200) - self.frmRT = tk.Frame(width=200, height=200) - self.frmLT.grid(row=0, column=0, padx=1, pady=3) - self.frmLC.grid(row=1, column=0, padx=1, pady=3) - self.frmLB.grid(row=1, column=1, padx=2, pady=3) - self.frmRT.grid(row=0, column=1, padx=2, pady=3) - - def need_input(self): - # input hint,输入提示 - tk.Label(self.frmLT, text="Joint 1 ").grid(row=0) - tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # the second row,第二行 - tk.Label(self.frmLT, text="Joint 3 ").grid(row=2) - tk.Label(self.frmLT, text="Joint 4 ").grid(row=3) - tk.Label(self.frmLT, text="Joint 5 ").grid(row=4) - tk.Label(self.frmLT, text="Joint 6 ").grid(row=5) - - tk.Label(self.frmRT, text=" x ").grid(row=0) - tk.Label(self.frmRT, text=" y ").grid(row=1) # the second row,第二行 - tk.Label(self.frmRT, text=" z ").grid(row=2) - tk.Label(self.frmRT, text=" rx ").grid(row=3) - tk.Label(self.frmRT, text=" ry ").grid(row=4) - tk.Label(self.frmRT, text=" rz ").grid(row=5) - - # Set the default value of the input box,设置输入框的默认值 - self.j1_default = tk.StringVar() - self.j1_default.set(self.res_angles[0]) - self.j2_default = tk.StringVar() - self.j2_default.set(self.res_angles[1]) - self.j3_default = tk.StringVar() - self.j3_default.set(self.res_angles[2]) - self.j4_default = tk.StringVar() - self.j4_default.set(self.res_angles[3]) - self.j5_default = tk.StringVar() - self.j5_default.set(self.res_angles[4]) - self.j6_default = tk.StringVar() - self.j6_default.set(self.res_angles[5]) - - self.x_default = tk.StringVar() - self.x_default.set(self.record_coords[0]) - self.y_default = tk.StringVar() - self.y_default.set(self.record_coords[1]) - self.z_default = tk.StringVar() - self.z_default.set(self.record_coords[2]) - self.rx_default = tk.StringVar() - self.rx_default.set(self.record_coords[3]) - self.ry_default = tk.StringVar() - self.ry_default.set(self.record_coords[4]) - self.rz_default = tk.StringVar() - self.rz_default.set(self.record_coords[5]) - - # joint input box,joint 输入框 - self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default) - self.J_1.grid(row=0, column=1, pady=3) - self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default) - self.J_2.grid(row=1, column=1, pady=3) - self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default) - self.J_3.grid(row=2, column=1, pady=3) - self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default) - self.J_4.grid(row=3, column=1, pady=3) - self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default) - self.J_5.grid(row=4, column=1, pady=3) - self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default) - self.J_6.grid(row=5, column=1, pady=3) - - # coord input box,coord 输入框 - self.x = tk.Entry(self.frmRT, textvariable=self.x_default) - self.x.grid(row=0, column=1, pady=3, padx=0) - self.y = tk.Entry(self.frmRT, textvariable=self.y_default) - self.y.grid(row=1, column=1, pady=3) - self.z = tk.Entry(self.frmRT, textvariable=self.z_default) - self.z.grid(row=2, column=1, pady=3) - self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default) - self.rx.grid(row=3, column=1, pady=3) - self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default) - self.ry.grid(row=4, column=1, pady=3) - self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default) - self.rz.grid(row=5, column=1, pady=3) - - # All input boxes, used to get the input data,所有输入框，用于拿输入的数据 - self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6] - self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz] - - # speed input box,速度输入框 - tk.Label( - self.frmLB, - text="speed", - ).grid(row=0, column=0) - self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10) - self.get_speed.grid(row=0, column=1) - - def show_init(self): - # show,显示 - tk.Label(self.frmLC, text="Joint 1 ").grid(row=0) - tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # the second row,第二行 - tk.Label(self.frmLC, text="Joint 3 ").grid(row=2) - tk.Label(self.frmLC, text="Joint 4 ").grid(row=3) - tk.Label(self.frmLC, text="Joint 5 ").grid(row=4) - tk.Label(self.frmLC, text="Joint 6 ").grid(row=5) - - # get数据 - - # show,展示出来 - self.cont_1 = tk.StringVar(self.frmLC) - self.cont_1.set(str(self.res_angles[0]) + "°") - self.cont_2 = tk.StringVar(self.frmLC) - self.cont_2.set(str(self.res_angles[1]) + "°") - self.cont_3 = tk.StringVar(self.frmLC) - self.cont_3.set(str(self.res_angles[2]) + "°") - self.cont_4 = tk.StringVar(self.frmLC) - self.cont_4.set(str(self.res_angles[3]) + "°") - self.cont_5 = tk.StringVar(self.frmLC) - self.cont_5.set(str(self.res_angles[4]) + "°") - self.cont_6 = tk.StringVar(self.frmLC) - self.cont_6.set(str(self.res_angles[5]) + "°") - self.cont_all = [ - self.cont_1, - self.cont_2, - self.cont_3, - self.cont_4, - self.cont_5, - self.cont_6, - self.speed, - self.model, - ] - - self.show_j1 = tk.Label( - self.frmLC, - textvariable=self.cont_1, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=0, column=1, padx=0, pady=5) - - self.show_j2 = tk.Label( - self.frmLC, - textvariable=self.cont_2, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=1, column=1, padx=0, pady=5) - self.show_j3 = tk.Label( - self.frmLC, - textvariable=self.cont_3, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=2, column=1, padx=0, pady=5) - self.show_j4 = tk.Label( - self.frmLC, - textvariable=self.cont_4, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=3, column=1, padx=0, pady=5) - self.show_j5 = tk.Label( - self.frmLC, - textvariable=self.cont_5, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=4, column=1, padx=0, pady=5) - self.show_j6 = tk.Label( - self.frmLC, - textvariable=self.cont_6, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=5, column=1, padx=5, pady=5) - - self.all_jo = [ - self.show_j1, - self.show_j2, - self.show_j3, - self.show_j4, - self.show_j5, - self.show_j6, - ] - - # show,显示 - tk.Label(self.frmLC, text=" x ").grid(row=0, column=3) - tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) - tk.Label(self.frmLC, text=" z ").grid(row=2, column=3) - tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3) - tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3) - tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3) - self.coord_x = tk.StringVar() - self.coord_x.set(str(self.record_coords[0])) - self.coord_y = tk.StringVar() - self.coord_y.set(str(self.record_coords[1])) - self.coord_z = tk.StringVar() - self.coord_z.set(str(self.record_coords[2])) - self.coord_rx = tk.StringVar() - self.coord_rx.set(str(self.record_coords[3])) - self.coord_ry = tk.StringVar() - self.coord_ry.set(str(self.record_coords[4])) - self.coord_rz = tk.StringVar() - self.coord_rz.set(str(self.record_coords[5])) - - self.coord_all = [ - self.coord_x, - self.coord_y, - self.coord_z, - self.coord_rx, - self.coord_ry, - self.coord_rz, - self.speed, - self.model, - ] - - self.show_x = tk.Label( - self.frmLC, - textvariable=self.coord_x, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=0, column=4, padx=5, pady=5) - self.show_y = tk.Label( - self.frmLC, - textvariable=self.coord_y, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=1, column=4, padx=5, pady=5) - self.show_z = tk.Label( - self.frmLC, - textvariable=self.coord_z, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=2, column=4, padx=5, pady=5) - self.show_rx = tk.Label( - self.frmLC, - textvariable=self.coord_rx, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=3, column=4, padx=5, pady=5) - self.show_ry = tk.Label( - self.frmLC, - textvariable=self.coord_ry, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=4, column=4, padx=5, pady=5) - self.show_rz = tk.Label( - self.frmLC, - textvariable=self.coord_rz, - font=("Arial", 9), - width=7, - height=1, - bg="white", - ).grid(row=5, column=4, padx=5, pady=5) - - # mm， Unit show，单位展示 - self.unit = tk.StringVar() - self.unit.set("mm") - for i in range(6): - tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid( - row=i, column=5 - ) - - def gripper_open(self): - try: - self.switch_gripper(True) - except ServiceException: - # Probably because the method has no return value, the service throws an unhandled error - # 可能由于该方法没有返回值，服务抛出无法处理的错误 - pass - - def gripper_close(self): - try: - self.switch_gripper(False) - except ServiceException: - pass - - def get_coord_input(self): - # Get the data input by coord and send it to the robotic arm - # 获取 coord 输入的数据，发送给机械臂 - c_value = [] - for i in self.all_c: - # print(type(i.get())) - c_value.append(float(i.get())) - self.speed = ( - int(float(self.get_speed.get())) if self.get_speed.get() else self.speed - ) - c_value.append(self.speed) - c_value.append(self.model) - # print(c_value) - try: - self.set_coords(*c_value) - except ServiceException: - pass - self.show_j_date(c_value[:-2], "coord") - - def get_joint_input(self): - # Get the data input by the joint and send it to the robotic arm - # 获取joint输入的数据，发送给机械臂 - j_value = [] - for i in self.all_j: - # print(type(i.get())) - j_value.append(float(i.get())) - self.speed = ( - int(float(self.get_speed.get())) if self.get_speed.get() else self.speed - ) - j_value.append(self.speed) - - try: - self.set_angles(*j_value) - except ServiceException: - pass - self.show_j_date(j_value[:-1]) - # return j_value,c_value,speed - - def get_date(self): - # Take the data of the robotic arm for display.拿机械臂的数据，用于展示 - t = time.time() - while time.time() - t < 2: - self.res = self.get_coords() - if self.res.x > 1: - break - time.sleep(0.1) - - t = time.time() - while time.time() - t < 2: - self.angles = self.get_angles() - if self.angles.joint_1 > 1: - break - time.sleep(0.1) - # print(self.angles.joint_1) - self.record_coords = [ - round(self.res.x, 2), - round(self.res.y, 2), - round(self.res.z, 2), - round(self.res.rx, 2), - round(self.res.ry, 2), - round(self.res.rz, 2), - self.speed, - self.model, - ] - self.res_angles = [ - round(self.angles.joint_1, 2), - round(self.angles.joint_2, 2), - round(self.angles.joint_3, 2), - round(self.angles.joint_4, 2), - round(self.angles.joint_5, 2), - round(self.angles.joint_6, 2), - ] - # print('coord:',self.record_coords) - # print('angles:',self.res_angles) - - # def send_input(self,dates): - def show_j_date(self, date, way=""): - # Show data,展示数据 - if way == "coord": - for i, j in zip(date, self.coord_all): - # print(i) - j.set(str(i)) - else: - for i, j in zip(date, self.cont_all): - j.set(str(i) + "°") - - def run(self): - while True: - try: - self.win.update() - time.sleep(0.001) - except tk.TclError as e: - if "application has been destroyed" in str(e): - break - else: - raise - - -def main(): - window = tk.Tk() - window.title("mycobot ros GUI") - Window(window).run() - - -if __name__ == "__main__": - main() diff --git a/mycobot_280jn/scripts/slider_control.py b/mycobot_280jn/scripts/slider_control.py deleted file mode 100644 index a40abc0..0000000 --- a/mycobot_280jn/scripts/slider_control.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python2 - -"""[summary] -This file obtains the joint angle of the manipulator in ROS, -and then sends it directly to the real manipulator using `pymycobot` API. -This file is [slider_control.launch] related script. -Passable parameters: - port: serial prot string. Defaults is '/dev/ttyTHS1' - baud: serial prot baudrate. Defaults is 1000000. -""" - -import rospy -from sensor_msgs.msg import JointState - -from pymycobot.mycobot import MyCobot - - -mc = None - - -def callback(data): - # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - print(data.position) - data_list = [] - for index, value in enumerate(data.position): - data_list.append(value) - - mc.send_radians(data_list, 80) - # time.sleep(0.5) - - -def listener(): - global mc - rospy.init_node("control_slider", anonymous=True) - - rospy.Subscriber("joint_states", JointState, callback) - port = rospy.get_param("~port", "/dev/ttyTHS1") - baud = rospy.get_param("~baud", 1000000) - print(port, baud) - mc = MyCobot(port, baud) - - # spin() simply keeps python from exiting until this node is stopped - # spin()只是阻止python退出，直到该节点停止 - print("spin ...") - rospy.spin() - - -if __name__ == "__main__": - listener() diff --git a/mycobot_280jn/scripts/teleop_keyboard.py b/mycobot_280jn/scripts/teleop_keyboard.py deleted file mode 100644 index 6bb47af..0000000 --- a/mycobot_280jn/scripts/teleop_keyboard.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python -from __future__ import print_function -from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus -import rospy -import sys -import select -import termios -import tty -import time - -import roslib - -# Terminal output prompt information. 终端输出提示信息 -msg = """\ -Mycobot Teleop Keyboard Controller ---------------------------- -Movimg options(control coordinations [x,y,z,rx,ry,rz]): - w(x+) - - a(y-) s(x-) d(y+) - - z(z-) x(z+) - -u(rx+) i(ry+) o(rz+) -j(rx-) k(ry-) l(rz-) - -Gripper control: - g - open - h - close - -Other: - 1 - Go to init pose - 2 - Go to home pose - 3 - Resave home pose - q - Quit -""" - - -def vels(speed, turn): - return "currently:\tspeed: %s\tchange percent: %s " % (speed, turn) - - -class Raw(object): - def __init__(self, stream): - self.stream = stream - self.fd = self.stream.fileno() - - def __enter__(self): - self.original_stty = termios.tcgetattr(self.stream) - tty.setcbreak(self.stream) - - def __exit__(self, type, value, traceback): - termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty) - - -def teleop_keyboard(): - rospy.init_node("teleop_keyboard") - - model = 0 - speed = rospy.get_param("~speed", 50) - change_percent = rospy.get_param("~change_percent", 5) - - change_angle = 180 * change_percent / 100 - change_len = 250 * change_percent / 100 - - rospy.wait_for_service("get_joint_angles") - rospy.wait_for_service("set_joint_angles") - rospy.wait_for_service("get_joint_coords") - rospy.wait_for_service("set_joint_coords") - rospy.wait_for_service("switch_gripper_status") - print("service ready.") - try: - get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords) - set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) - get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles) - set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) - switch_gripper = rospy.ServiceProxy( - "switch_gripper_status", GripperStatus) - except: - print("start error ...") - exit(1) - - init_pose = [0, 0, 0, 0, 0, 0, speed] - home_pose = [0, 8, -127, 40, 0, 0, speed] - - # rsp = set_angles(*init_pose) - - while True: - res = get_coords() - if res.x > 1: - break - time.sleep(0.1) - - record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] - print(record_coords) - - try: - print(msg) - print(vels(speed, change_percent)) - # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能 - while 1: - try: - # print("\r current coords: %s" % record_coords, end="") - with Raw(sys.stdin): - key = sys.stdin.read(1) - if key == "q": - break - elif key in ["w", "W"]: - record_coords[0] += change_len - set_coords(*record_coords) - elif key in ["s", "S"]: - record_coords[0] -= change_len - set_coords(*record_coords) - elif key in ["a", "A"]: - record_coords[1] -= change_len - set_coords(*record_coords) - elif key in ["d", "D"]: - record_coords[1] += change_len - set_coords(*record_coords) - elif key in ["z", "Z"]: - record_coords[2] -= change_len - set_coords(*record_coords) - elif key in ["x", "X"]: - record_coords[2] += change_len - set_coords(*record_coords) - elif key in ["u", "U"]: - record_coords[3] += change_angle - set_coords(*record_coords) - elif key in ["j", "J"]: - record_coords[3] -= change_angle - set_coords(*record_coords) - elif key in ["i", "I"]: - record_coords[4] += change_angle - set_coords(*record_coords) - elif key in ["k", "K"]: - record_coords[4] -= change_angle - set_coords(*record_coords) - elif key in ["o", "O"]: - record_coords[5] += change_angle - set_coords(*record_coords) - elif key in ["l", "L"]: - record_coords[5] -= change_angle - set_coords(*record_coords) - elif key in ["g", "G"]: - switch_gripper(True) - elif key in ["h", "H"]: - switch_gripper(False) - elif key == "1": - rsp = set_angles(*init_pose) - elif key in "2": - rsp = set_angles(*home_pose) - elif key in "3": - rep = get_angles() - home_pose[0] = rep.joint_1 - home_pose[1] = rep.joint_2 - home_pose[2] = rep.joint_3 - home_pose[3] = rep.joint_4 - home_pose[5] = rep.joint_5 - else: - continue - - except Exception as e: - # print(e) - continue - - except Exception as e: - print(e) - - -if __name__ == "__main__": - try: - teleop_keyboard() - except rospy.ROSInterruptException: - pass diff --git a/mycobot_280jn/src/camera_display.cpp b/mycobot_280jn/src/camera_display.cpp deleted file mode 100644 index 1f3abeb..0000000 --- a/mycobot_280jn/src/camera_display.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include -#include -#include - -void imageCallback(const sensor_msgs::ImageConstPtr &msg) -{ - try - { - cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); - cv::waitKey(30); - } - catch (cv_bridge::Exception &e) - { - ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "image_listener"); - ros::NodeHandle nh; - cv::namedWindow("view"); - cv::startWindowThread(); - image_transport::ImageTransport it(nh); - image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback); - ros::spin(); - cv::destroyWindow("view"); -} diff --git a/mycobot_280jn/src/opencv_camera.cpp b/mycobot_280jn/src/opencv_camera.cpp deleted file mode 100644 index 6501814..0000000 --- a/mycobot_280jn/src/opencv_camera.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include -#include -#include -#include -#include // for converting the command line parameter to integer,用于将命令行参数转换为整数 - -int main(int argc, char **argv) -{ - // Check if video source has been passed as a parameter,检查视频源是否已作为参数传递 - if (argv[1] == NULL) - { - ROS_INFO("argv[1]=NULL\n"); - return 1; - } - - ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点 - ros::NodeHandle nh; - image_transport::ImageTransport it(nh); - image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题 - - ros::Rate loop_rate(200); // refresh Hz. - - // Convert the passed as command line parameter index for the video device to an integer， - // 将作为命令行参数传递的视频设备索引转换为整数 - std::istringstream video_sourceCmd(argv[1]); - int video_source; - // Check if it is indeed a number，检查它是否确实是一个数字 - if (!(video_sourceCmd >> video_source)) - { - ROS_INFO("video_sourceCmd is %d\n", video_source); - return 1; - } - - cv::VideoCapture cap(video_source); - // Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备 - if (!cap.isOpened()) - { - ROS_INFO("can not opencv video device\n"); - return 1; - } - cv::Mat frame; - sensor_msgs::ImagePtr msg; - - while (nh.ok()) - { - cap >> frame; - // cv::imshow("veiwer", frame); - // Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容 - if (!frame.empty()) - { - msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); - pub.publish(msg); - //cv::Wait(1); - } - ros::spinOnce(); - loop_rate.sleep(); - // if(cv::waitKey(2) >= 0) - // break; - } -}