From 3b36203762bb34d7583408e951885b7f13205c21 Mon Sep 17 00:00:00 2001 From: weijian Date: Thu, 1 Dec 2022 16:40:31 +0800 Subject: [PATCH 1/3] update ubuntu20.04 280_jn ros1 --- .../config/{mycobot.rviz => mycobot_jn.rviz} | 45 ++++++++++--------- .../mycobot_280jn/launch/detect_marker.launch | 2 +- .../launch/detect_marker_with_topic.launch | 2 +- .../launch/mycobot_follow.launch | 2 +- .../mycobot_280jn/launch/simple_gui.launch | 10 ++--- .../launch/slider_control.launch | 2 +- .../launch/teleop_keyboard.launch | 10 ++--- mycobot_280/mycobot_280jn/launch/test.launch | 6 +-- .../mycobot_280jn/scripts/follow_display.py | 20 ++++----- .../mycobot_280jn/scripts/listen_real.py | 2 +- .../scripts/listen_real_of_topic.py | 2 +- .../mycobot_280jn/scripts/simple_gui.py | 7 +-- .../mycobot_280jn/scripts/slider_control.py | 26 +++++------ .../mycobot_280jn/scripts/teleop_keyboard.py | 2 +- .../launch/communication_jsnn.launch | 6 +-- .../scripts/mycobot_services.py | 2 +- 16 files changed, 76 insertions(+), 70 deletions(-) rename mycobot_280/mycobot_280jn/config/{mycobot.rviz => mycobot_jn.rviz} (78%) diff --git a/mycobot_280/mycobot_280jn/config/mycobot.rviz b/mycobot_280/mycobot_280jn/config/mycobot_jn.rviz similarity index 78% rename from mycobot_280/mycobot_280jn/config/mycobot.rviz rename to mycobot_280/mycobot_280jn/config/mycobot_jn.rviz index 51ba33e..a3590c8 100644 --- a/mycobot_280/mycobot_280jn/config/mycobot.rviz +++ b/mycobot_280/mycobot_280jn/config/mycobot_jn.rviz @@ -9,7 +9,7 @@ Panels: - /RobotModel1 - /TF1 Splitter Ratio: 0.5 - Tree Height: 775 + Tree Height: 579 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -18,17 +18,18 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -40,7 +41,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -122,7 +123,8 @@ Visualization Manager: Value: true joint6_flange: Value: true - Marker Scale: 0.300000012 + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: true Show Axes: true @@ -153,7 +155,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -163,33 +168,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.20289087 + Distance: 1.2028908729553223 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: -0.0706475973 - Y: -0.0814988762 - Z: 0.107583851 + X: -0.07064759731292725 + Y: -0.0814988762140274 + Z: 0.10758385062217712 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.440398335 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.44039833545684814 Target Frame: - Value: Orbit (rviz) - Yaw: 0.430389732 + Yaw: 0.43038973212242126 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 876 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ce000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f000002cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000393000002ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -198,6 +203,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1855 - X: 65 - Y: 24 + Width: 1848 + X: 72 + Y: 27 diff --git a/mycobot_280/mycobot_280jn/launch/detect_marker.launch b/mycobot_280/mycobot_280jn/launch/detect_marker.launch index 444638e..c41a96f 100644 --- a/mycobot_280/mycobot_280jn/launch/detect_marker.launch +++ b/mycobot_280/mycobot_280jn/launch/detect_marker.launch @@ -1,7 +1,7 @@ - + diff --git a/mycobot_280/mycobot_280jn/launch/detect_marker_with_topic.launch b/mycobot_280/mycobot_280jn/launch/detect_marker_with_topic.launch index 2bba726..c4222af 100644 --- a/mycobot_280/mycobot_280jn/launch/detect_marker_with_topic.launch +++ b/mycobot_280/mycobot_280jn/launch/detect_marker_with_topic.launch @@ -4,7 +4,7 @@ - + diff --git a/mycobot_280/mycobot_280jn/launch/mycobot_follow.launch b/mycobot_280/mycobot_280jn/launch/mycobot_follow.launch index 7511e1f..f5c89e8 100644 --- a/mycobot_280/mycobot_280jn/launch/mycobot_follow.launch +++ b/mycobot_280/mycobot_280jn/launch/mycobot_follow.launch @@ -1,7 +1,7 @@ - + diff --git a/mycobot_280/mycobot_280jn/launch/simple_gui.launch b/mycobot_280/mycobot_280jn/launch/simple_gui.launch index d1b31e0..79c92ee 100644 --- a/mycobot_280/mycobot_280jn/launch/simple_gui.launch +++ b/mycobot_280/mycobot_280jn/launch/simple_gui.launch @@ -1,10 +1,10 @@ - - + + - + @@ -14,9 +14,9 @@ - - + + diff --git a/mycobot_280/mycobot_280jn/launch/slider_control.launch b/mycobot_280/mycobot_280jn/launch/slider_control.launch index ac42e42..63fdaa3 100644 --- a/mycobot_280/mycobot_280jn/launch/slider_control.launch +++ b/mycobot_280/mycobot_280jn/launch/slider_control.launch @@ -3,7 +3,7 @@ --> - + diff --git a/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch b/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch index f3a84db..d915aae 100644 --- a/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch +++ b/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch @@ -1,10 +1,10 @@ - - + + - + @@ -14,9 +14,9 @@ - - + + diff --git a/mycobot_280/mycobot_280jn/launch/test.launch b/mycobot_280/mycobot_280jn/launch/test.launch index dbb1f00..4fa06e4 100644 --- a/mycobot_280/mycobot_280jn/launch/test.launch +++ b/mycobot_280/mycobot_280jn/launch/test.launch @@ -1,14 +1,14 @@ - - + + - + diff --git a/mycobot_280/mycobot_280jn/scripts/follow_display.py b/mycobot_280/mycobot_280jn/scripts/follow_display.py index 52d18b7..68e8095 100755 --- a/mycobot_280/mycobot_280jn/scripts/follow_display.py +++ b/mycobot_280/mycobot_280jn/scripts/follow_display.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # encoding:utf8 import time @@ -15,16 +15,16 @@ 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) - ip=rospy.get_param('~ip','192.168.125.226') - netport=rospy.get_param('~netport',9000) - # print("port: {}, baud: {}\n".format(port, baud)) - print('ip:{},port:{}'.format(ip,netport)) + port = rospy.get_param("~port", "/dev/ttyTHS1") + baud = rospy.get_param("~baud", 1000000) + # ip=rospy.get_param('~ip','192.168.125.226') + # netport=rospy.get_param('~netport',9000) + print("port: {}, baud: {}\n".format(port, baud)) + # print('ip:{},port:{}'.format(ip,netport)) try: - # mycobot = MyCobot(port, baud) - mycobot=MyCobotSocket(ip,netport) - mycobot.connect() + mycobot = MyCobot(port, baud) + # mycobot=MyCobotSocket(ip,netport) + # mycobot.connect() except Exception as e: print(e) print( diff --git a/mycobot_280/mycobot_280jn/scripts/listen_real.py b/mycobot_280/mycobot_280jn/scripts/listen_real.py index 1d38b61..aea0d53 100755 --- a/mycobot_280/mycobot_280jn/scripts/listen_real.py +++ b/mycobot_280/mycobot_280jn/scripts/listen_real.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # encoding:utf-8 # license removed for brevity import time diff --git a/mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py b/mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py index ea3df92..89f9430 100755 --- a/mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py +++ b/mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import math import rospy diff --git a/mycobot_280/mycobot_280jn/scripts/simple_gui.py b/mycobot_280/mycobot_280jn/scripts/simple_gui.py index 9f4c791..41f9258 100755 --- a/mycobot_280/mycobot_280jn/scripts/simple_gui.py +++ b/mycobot_280/mycobot_280jn/scripts/simple_gui.py @@ -1,6 +1,7 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- -import Tkinter as tk +# import Tkinter as tk +import tkinter as tk from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import time @@ -33,7 +34,7 @@ class Window: # 计算 Tk 根窗口的 x 和 y 坐标 x = (self.ws / 2) - 190 y = (self.hs / 2) - 250 - self.win.geometry("430x370+{}+{}".format(x, y)) + self.win.geometry("430x400+{}+{}".format(int(x), int(y))) # layout,布局 self.set_layout() # input section,输入部分 diff --git a/mycobot_280/mycobot_280jn/scripts/slider_control.py b/mycobot_280/mycobot_280jn/scripts/slider_control.py index 4bf44b9..bf58aae 100755 --- a/mycobot_280/mycobot_280jn/scripts/slider_control.py +++ b/mycobot_280/mycobot_280jn/scripts/slider_control.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # encoding:utf-8 """[summary] This file obtains the joint angle of the manipulator in ROS, @@ -16,7 +16,7 @@ from pymycobot.mycobot import MyCobot from pymycobot.mycobotsocket import MyCobotSocket -ms = None +mc = None def callback(data): @@ -26,24 +26,24 @@ def callback(data): for index, value in enumerate(data.position): data_list.append(value) - ms.send_radians(data_list, 80) + mc.send_radians(data_list, 80) # time.sleep(0.5) def listener(): - global ms + 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) - ip=rospy.get_param("~ip",'192.168.125.226') - port=rospy.get_param("~port",9000) - print(ip,port) - ms=MyCobotSocket(ip,port) - ms.connect() + port = rospy.get_param("~port", "/dev/ttyTHS1") + baud = rospy.get_param("~baud", 1000000) + print(port, baud) + mc = MyCobot(port, baud) + # ip=rospy.get_param("~ip",'192.168.125.226') + # port=rospy.get_param("~port",9000) + # print(ip,port) + # ms=MyCobotSocket(ip,port) + # ms.connect() # spin() simply keeps python from exiting until this node is stopped # spin()只是阻止python退出，直到该节点停止 diff --git a/mycobot_280/mycobot_280jn/scripts/teleop_keyboard.py b/mycobot_280/mycobot_280jn/scripts/teleop_keyboard.py index e5ee716..110f8a4 100755 --- a/mycobot_280/mycobot_280jn/scripts/teleop_keyboard.py +++ b/mycobot_280/mycobot_280jn/scripts/teleop_keyboard.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding=utf-8 from __future__ import print_function from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus diff --git a/mycobot_communication/launch/communication_jsnn.launch b/mycobot_communication/launch/communication_jsnn.launch index b49eba0..3472501 100644 --- a/mycobot_communication/launch/communication_jsnn.launch +++ b/mycobot_communication/launch/communication_jsnn.launch @@ -1,10 +1,10 @@ - - + + - + diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py index 0f105a8..1f5c89a 100755 --- a/mycobot_communication/scripts/mycobot_services.py +++ b/mycobot_communication/scripts/mycobot_services.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # -*- coding: utf-8 -* import time import rospy From 67b57bb96e514e552e1a588e46426bc3562eb118 Mon Sep 17 00:00:00 2001 From: weijian Date: Thu, 1 Dec 2022 16:44:40 +0800 Subject: [PATCH 2/3] update ubuntu20.04 280_jn ros1 --- .../mycobot_280jn_moveit/launch/moveit.rviz | 53 ++++++++++--------- .../mycobot_280jn_moveit/scripts/sync_plan.py | 2 +- 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz b/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz index 38b3736..8d7460d 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz +++ b/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz @@ -9,8 +9,8 @@ Panels: - /MotionPlanning1/Scene Robot1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 - Splitter Ratio: 0.742560029 - Tree Height: 325 + Splitter Ratio: 0.7425600290298462 + Tree Height: 172 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -18,6 +18,8 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -29,7 +31,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -45,16 +47,14 @@ Visualization Manager: Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_Approximate_IK: true MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 MoveIt_Workspace: Center: X: 0 @@ -118,20 +118,21 @@ Visualization Manager: State Display Time: 0.05 s Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path + Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.0799999982 + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 + Interactive Marker Size: 0.12999999523162842 Joint Violation Color: 255; 0; 255 - Planning Group: mycobot_arm + Planning Group: arm_group Query Goal State: true Query Start State: true Show Workspace: false @@ -142,7 +143,7 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.200000003 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -210,30 +211,30 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 1.39158678 + Distance: 1.3915867805480957 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 0.113567002 - Y: 0.105920002 - Z: 2.23518001e-07 + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-07 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.439796686 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.0647968053817749 Target Frame: joint1 - Value: XYOrbit (rviz) - Yaw: 2.5317657 + Yaw: 3.026766300201416 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1028 + Height: 876 Help: collapsed: false Hide Left Dock: false @@ -242,9 +243,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002ad000003befc0200000007fb000000100044006900730070006c0061007900730100000028000001da000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002080000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002580000018e0000018300ffffff00000471000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f300000312fc0200000007fb000000100044006900730070006c006100790073010000003d0000013d000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000180000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c7000001880000017d00ffffff0000053f0000031200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 92 - Y: 24 + Width: 1848 + X: 72 + Y: 27 diff --git a/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py index 8cdd97e..ed14da6 100644 --- a/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py +++ b/mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import time import rospy from sensor_msgs.msg import JointState From 282ecb34ddfc0e5115939c09a72d2026224ced5b Mon Sep 17 00:00:00 2001 From: weijian Date: Fri, 2 Dec 2022 17:21:10 +0800 Subject: [PATCH 3/3] update mycobot_communication/scripts/mycobot_services.py --- .../ai_mecharm_270/scripts/color_test.py | 164 ------ .../scripts/advance_detect_obj_color.py | 480 --------------- .../ai_mycobot_280/scripts/aikit_img.py | 555 ----------------- .../ai_mycobot_280/scripts/encode_test.py | 248 -------- .../ai_mypalletizer_260/scripts/aikit_img.py | 557 ------------------ .../scripts/mycobot_services.py | 57 +- 6 files changed, 55 insertions(+), 2006 deletions(-) delete mode 100644 mycobot_ai/ai_mecharm_270/scripts/color_test.py delete mode 100644 mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py delete mode 100644 mycobot_ai/ai_mycobot_280/scripts/aikit_img.py delete mode 100644 mycobot_ai/ai_mycobot_280/scripts/encode_test.py delete mode 100644 mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py diff --git a/mycobot_ai/ai_mecharm_270/scripts/color_test.py b/mycobot_ai/ai_mecharm_270/scripts/color_test.py deleted file mode 100644 index a21e333..0000000 --- a/mycobot_ai/ai_mecharm_270/scripts/color_test.py +++ /dev/null @@ -1,164 +0,0 @@ -# ecoding=utf-8 -import cv2 -import numpy as np -from imutils import contours -import math - -'''HSV中的颜色空间 -https://blog.csdn.net/wsp_1138886114/article/details/80660014 -''' -color_dist = {'red': {'Lower': np.array([0, 120, 120]), 'Upper': np.array([6, 255, 255])}, - # 'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])}, - # 'blue':{'Lower': np.array([100,43,46]),'Upper': np.array([124,255,255])}, - 'blue':{'Lower': np.array([100,43,46]),'Upper': np.array([124,255,255])}, - "cyan": {'Lower': np.array([78, 43, 46]),'Upper': np.array([99, 255, 255])}, - 'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])}, - 'yellow': {"Lower": np.array([22, 93, 0]), "Upper": np.array([45, 255, 255])}, - # 'orange': {"Lower": np.array([0, 100, 45]), "Upper":np.array([255, 250, 255])}, - 'orange': {"Lower": np.array([11, 43, 46]), "Upper":np.array([25, 255, 255])}, - } - -cap = cv2.VideoCapture(0) -# cap = cv2.VideoCapture("src/test_1_2.mp4") -cap.set(3,420) -cap.set(4,360) - -cv2.namedWindow('camera', cv2.WINDOW_AUTOSIZE) -# 内核 -kernel = np.ones((5, 5), np.uint8) - -while cap.isOpened(): - ret, frame = cap.read() - if ret: - if frame is not None: - - gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)# 高斯模糊 - hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV) # 转化成HSV图像 - erode_hsv = cv2.erode(hsv, None, iterations=2) # 腐蚀 作用是粗的变细 - for _color in color_dist: - _font_x_pos = 0 - _font_y_pos = 0 - ## 颜色阈值识别 - inRange_hsv = cv2.inRange(erode_hsv, color_dist[_color]['Lower'], color_dist[_color]['Upper']) - # 膨胀操作 - dilation = cv2.dilate(inRange_hsv, kernel, iterations=1) - # 闭操作 - closing = cv2.morphologyEx(dilation, cv2.MORPH_CLOSE, kernel) - # 边缘检测 - edges = cv2.Canny(closing, 10, 20) - # 检测物体边框 - cnts, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) - - # 判断轮廓数量也就是判断是否寻找到轮廓，如果没有找到轮廓就不继续进行操作 - if len(cnts) > 0: - for cnt in cnts: - if cv2.contourArea(cnt) >1500: - peri = cv2.arcLength(cnt,True) - #print(peri) - # 用于获得轮廓的近似值，使用cv2.drawCountors进行画图操作 ， - # 参数说明：cnt为输入的轮廓值， epsilon为阈值T，通常使用轮廓的周长作为阈值，True表示的是轮廓是闭合的 - ''' - cv2.approxPolyDP - @:param - cnt: - epsilon :算法参数 - True：表示是否闭合 - ''' - approx = cv2.approxPolyDP(cnt,0.02*peri,True) - # print(len(approx)) - # 提取拐点 - ''' - 返回列表元素，列表中的元素代表一个边沿信息。 - ''' - objCor = len(approx) - - if objCor ==3: - '''https://blog.51cto.com/hiszm/5201991''' - objectType = "Triangle" - mm = cv2.moments(cnt) - cx = int(mm['m10']/mm['m00']) - cy = int(mm['m01']/mm['m00']) - _font_x_pos = cx - _font_y_pos = cy - cv2.circle(frame,(cx,cy),3,(0,0,255),-1) - cv2.drawContours(frame, [cnt], 0, (0, 0, 255), 3) - print("三角形的坐标",cx,cy) - - - # 检测出是矩形 - elif objCor == 4: - # 可旋转矩形，即最小的外包矩形 - rect= cv2.minAreaRect(cnt) - ''' - @:param - 函数 cv2.minAreaRect() 返回一个Box2D结构 rect：（最小外接矩形的中心（x，y），（宽度，高度），旋转角度）。 - 分别对应于返回值：(rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2] - ''' - # 中心点坐标 - pos_x = int(rect[0][0]) - pos_y = int(rect[0][1]) - #print("position",pos_x,pos_y) - # 旋转角度 - theta = np.round(cv2.minAreaRect(cnt)[2],2) - box = cv2.boxPoints(rect) - box = np.int0(box) - print("物体的坐标为",(pos_x,pos_y),"旋转角度为",theta) - # 判断长方形还是正方形 - _font_x_pos = box[0][0] - _font_y_pos = box[0][1] - _W = math.sqrt(math.pow((box[0][0] - box[1][0]), 2) + math.pow((box[0][1] - box[1][1]), 2)) - _H = math.sqrt(math.pow((box[0][0] - box[3][0]), 2) + math.pow((box[0][1] - box[3][1]), 2)) - # 长宽比 - aspRatio = _W/float(_H) - if aspRatio >0.98 and aspRatio <1.03: - - objectType= "Square"#正方形 - else: - objectType="Rectangle"#长方形 - _pos = (pos_x,pos_y) - cv2.circle(frame,_pos,1,(0,0,255),2)#绘制中心点 - #cv2.putText(frame, 'teheta = ' + str(theta), (int(_font_x_pos),int(_font_y_pos+20)), cv2.FONT_HERSHEY_COMPLEX_SMALL,0.8, (0, 255, 0) ) - cv2.drawContours(frame,[box],0,(0,0,255),2) - # 圆形 - elif objCor>4: - objectType= "Circles" - ''' - void minEnclosingCircle(InputArray points, Point2f& center, float& radius) - @:param - points：输入信息，可以为包含点的容器(vector)或是Mat。 - center：包覆圆形的圆心。 - radius：包覆圆形的半径。 - ''' - (x,y),radius = cv2.minEnclosingCircle(cnt) - center = (int(x),int(y)) - radius = int(radius) - _font_x_pos = center[0] - _font_y_pos = center[1] - print("物体的圆心为:",(_font_x_pos, _font_y_pos),"半径为",radius) - cv2.circle(frame,center,1,(255,0,0),2)#绘制中心点 - cv2.circle(frame,center,radius,(255,0,0),2) - - else: - objectType="None" - # objectType 物体形状 - print("颜色类别：",_color) - cv2.putText(frame, str(_color + objectType), (int(_font_x_pos),int(_font_y_pos)), cv2.FONT_HERSHEY_COMPLEX_SMALL,1, (0, 255, 0) ) - - cv2.imshow('camera', frame) - # 存储识别结果视频 - # out.write(frame) - - cv2.waitKey(1) - if cv2.waitKey(10) & 0xFF == 27: - break - else: - print("无画面") - break - else: - print("无法读取摄像头！") - break - -cap.release() -# out.release() -cv2.waitKey(0) -cv2.destroyAllWindows() diff --git a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py b/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py deleted file mode 100644 index da893b8..0000000 --- a/mycobot_ai/ai_mycobot_280/scripts/advance_detect_obj_color.py +++ /dev/null @@ -1,480 +0,0 @@ -# encoding:utf-8 -#!/usr/bin/env python2 - -import cv2 -import numpy as np -import time -import os,sys -import rospy -from visualization_msgs.msg import Marker -from pymycobot.mycobot import MyCobot -from moving_utils import Movement - -IS_CV_4 = cv2.__version__[0] == '4' -__version__ = "1.0" -# Adaptive seeed - - -class Object_detect(Movement): - - def __init__(self, camera_x = 155, camera_y = 10): - # inherit the parent class - super(Object_detect, self).__init__() - # get path of file - dir_path = os.path.dirname(__file__) - self.mc = None - - # 移动角度 - self.move_angles = [ - [-7.11, -6.94, -55.01, -24.16, 0, -15], # init the point - [18.8, -7.91, -54.49, -23.02, -0.79, -14.76], # point to grab - ] - - # 移动坐标 - self.move_coords = [ - [132.2, -136.9, 200.8, -178.24, -3.72, -107.17], # above the red bucket - [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # green - [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # blue - [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # gray - ] - # which robot: USB* is m5; ACM* is wio; AMA* is raspi - self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] - self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] - self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1] - self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1] - self.raspi = False - if "dev" in self.robot_m5: - self.Pin = [2, 5] - elif "dev" in self.robot_wio: - # self.Pin = [20, 21] - self.Pin = [2, 5] - - for i in self.move_coords: - i[2] -= 20 - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - import RPi.GPIO as GPIO - GPIO.setwarnings(False) - self.GPIO = GPIO - GPIO.setmode(GPIO.BCM) - GPIO.setup(20, GPIO.OUT) - GPIO.setup(21, GPIO.OUT) - GPIO.output(20, 1) - GPIO.output(21, 1) - self.raspi = True - if self.raspi: - self.gpio_status(False) - - # choose place to set cube - self.color = 0 - # parameters to calculate camera clipping parameters - self.x1 = self.x2 = self.y1 = self.y2 = 0 - # set cache of real coord - self.cache_x = self.cache_y = 0 - # set color HSV - self.HSV = { - "yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])], - "red": [np.array([0, 43, 46]), np.array([8, 255, 255])], - "green": [np.array([35, 43, 35]), np.array([90, 255, 255])], # [77, 255, 255] - "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])], - "cyan": [np.array([89, 43, 46]), np.array([99, 255, 255])], # np.array([78, 43, 46]), np.array([99, 255, 255]) - } - # use to calculate coord between cube and mycobot - self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 - # The coordinates of the grab center point relative to the mycobot - self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mycobot - self.c_x, self.c_y = 0, 0 - # The ratio of pixels to actual values - self.ratio = 0 - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv2.aruco.DetectorParameters_create() - - # init a node and a publisher - rospy.init_node("marker", anonymous=True) - self.pub = rospy.Publisher('/cube', Marker, queue_size=1) - # init a Marker - self.marker = Marker() - self.marker.header.frame_id = "/joint1" - self.marker.ns = "cube" - self.marker.type = self.marker.CUBE - self.marker.action = self.marker.ADD - self.marker.scale.x = 0.04 - self.marker.scale.y = 0.04 - self.marker.scale.z = 0.04 - self.marker.color.a = 1.0 - self.marker.color.g = 1.0 - self.marker.color.r = 1.0 - - # marker position initial - self.marker.pose.position.x = 0 - self.marker.pose.position.y = 0 - self.marker.pose.position.z = 0.03 - self.marker.pose.orientation.x = 0 - self.marker.pose.orientation.y = 0 - self.marker.pose.orientation.z = 0 - self.marker.pose.orientation.w = 1.0 - - # publish marker - def pub_marker(self, x, y, z=0.03): - self.marker.header.stamp = rospy.Time.now() - self.marker.pose.position.x = x - self.marker.pose.position.y = y - self.marker.pose.position.z = z - self.marker.color.g = self.color - self.pub.publish(self.marker) - - def gpio_status(self, flag): - if flag: - # self.GPIO.output(20, 0) - self.GPIO.output(21, 0) - else: - # self.GPIO.output(20, 1) - self.GPIO.output(21, 1) - - # 开启吸泵 m5 - def pump_on(self): - # 让2号位工作 - # self.mc.set_basic_output(2, 0) - # 让5号位工作 - self.mc.set_basic_output(5, 0) - - # 停止吸泵 m5 - def pump_off(self): - # 让2号位停止工作 - # self.mc.set_basic_output(2, 1) - # 让5号位停止工作 - self.mc.set_basic_output(5, 1) - # Grasping motion - def move(self, x, y, color): - # send Angle to move mycobot - print (color) - self.mc.send_angles(self.move_angles[1], 25) - time.sleep(3) - - # send coordinates to move mycobot - self.mc.send_coords([x, y, 190.6, 179.87, -3.78, -62.75], 25, 1) # usb :rx,ry,rz -173.3, -5.48, -57.9 - time.sleep(3) - - # self.mc.send_coords([x, y, 150, 179.87, -3.78, -62.75], 25, 0) - # time.sleep(3) - - self.mc.send_coords([x, y, 103, 179.87, -3.78, -62.75], 25, 0) - time.sleep(3) - - # open pump - if "dev" in self.robot_m5 or "dev" in self.robot_wio: - self.pump_on() - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - self.gpio_status(True) - time.sleep(1.5) - - tmp = [] - while True: - if not tmp: - tmp = self.mc.get_angles() - else: - break - time.sleep(0.5) - - # print(tmp) - self.mc.send_angles([tmp[0], -0.71, -54.49, -23.02, -0.79, tmp[5]],25) # [18.8, -7.91, -54.49, -23.02, -0.79, -14.76] - time.sleep(2.5) - - self.pub_marker( - self.move_coords[2][0]/1000.0, self.move_coords[2][1]/1000.0, self.move_coords[2][2]/1000.0) - - self.mc.send_coords(self.move_coords[color], 25, 1) - self.pub_marker(self.move_coords[color][0]/1000.0, self.move_coords[color] - [1]/1000.0, self.move_coords[color][2]/1000.0) - time.sleep(3) - - # close pump - if "dev" in self.robot_m5 or "dev" in self.robot_wio: - self.pump_off() - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - self.gpio_status(False) - time.sleep(4) - - if color == 1: - self.pub_marker( - self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02) - elif color == 0: - self.pub_marker( - self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0) - # self.pub_angles(self.move_angles[0], 20) - self.mc.send_angles(self.move_angles[0], 25) - time.sleep(3) - - # decide whether grab cube - def decide_move(self, x, y, color): - print(x, y, self.cache_x, self.cache_y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - self.move(x, y, color) - - # init mycobot - def run(self): - if "dev" in self.robot_wio : - self.mc = MyCobot(self.robot_wio, 115200) - elif "dev" in self.robot_m5: - self.mc = MyCobot(self.robot_m5, 115200) - elif "dev" in self.robot_raspi: - self.mc = MyCobot(self.robot_raspi, 1000000) - if not self.raspi: - self.pub_pump(False, self.Pin) - self.mc.send_angles([-7.11, -6.94, -55.01, -24.16, 0, -15], 20) - time.sleep(3) - - # draw aruco - - def draw_marker(self, img, x, y): - # draw rectangle on img - cv2.rectangle( - img, - (x - 20, y - 20), - (x + 20, y + 20), - (0, 255, 0), - thickness=2, - lineType=cv2.FONT_HERSHEY_COMPLEX, - ) - # add text on rectangle - cv2.putText(img, "({},{})".format(x, y), (x, y), - cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,) - - # get points of two aruco - def get_calculate_params(self, img): - # Convert the image to a gray image - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params - ) - - """ - Two Arucos must be present in the picture and in the same order. - There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. - Determine the center of the aruco by the four corners of the aruco. - """ - if len(corners) > 0: - if ids is not None: - if len(corners) <= 1 or ids[0] == 1: - return None - x1 = x2 = y1 = y2 = 0 - point_11, point_21, point_31, point_41 = corners[0][0] - x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int( - (point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0) - point_1, point_2, point_3, point_4 = corners[1][0] - x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int( - (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0) - return x1, x2, y1, y2 - return None - - # set camera clipping parameters - def set_cut_params(self, x1, y1, x2, y2): - self.x1 = int(x1) - self.y1 = int(y1) - self.x2 = int(x2) - self.y2 = int(y2) - print(self.x1, self.y1, self.x2, self.y2) - - # set parameters to calculate the coords between cube and mycobot - def set_params(self, c_x, c_y, ratio): - self.c_x = c_x - self.c_y = c_y - self.ratio = 220.0/ratio - - # calculate the coords between cube and mycobot - def get_position(self, x, y): - return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y) - - """ - Calibrate the camera according to the calibration parameters. - Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. - If two ARuco values have been calculated, clip the video. - """ - - def transform_frame(self, frame): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy, - interpolation=cv2.INTER_CUBIC) - if self.x1 != self.x2: - # the cutting ratio here is adjusted according to the actual situation - frame = frame[int(self.y2*0.78):int(self.y1*1.1), - int(self.x1*0.86):int(self.x2*1.08)] - return frame - - # detect cube color - def color_detect(self, img): - # set the arrangement of color'HSV - x = y = 0 - gs_img = cv2.GaussianBlur(img, (3, 3), 0) # 高斯模糊 - # transfrom the img to model of gray - hsv = cv2.cvtColor(gs_img, cv2.COLOR_BGR2HSV) - - for mycolor, item in self.HSV.items(): - redLower = np.array(item[0]) - redUpper = np.array(item[1]) - # wipe off all color expect color in range - mask = cv2.inRange(hsv, item[0], item[1]) - # a etching operation on a picture to remove edge roughness - erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2) - # the image for expansion operation, its role is to deepen the color depth in the picture - dilation = cv2.dilate(erosion, np.ones( - (1, 1), np.uint8), iterations=2) - - - # adds pixels to the image - target = cv2.bitwise_and(img, img, mask=dilation) - # the filtered image is transformed into a binary image and placed in binary - ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY) - # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected - contours, hierarchy = cv2.findContours( - dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - if len(contours) > 0: - # do something about misidentification - boxes = [ - box - for box in [cv2.boundingRect(c) for c in contours] - if min(img.shape[0], img.shape[1]) / 10 - < min(box[2], box[3]) - < min(img.shape[0], img.shape[1]) / 1 - ] - if boxes: - for box in boxes: - x, y, w, h = box - # find the largest object that fits the requirements - c = max(contours, key=cv2.contourArea) - # get the lower left and upper right points of the positioning object - x, y, w, h = cv2.boundingRect(c) - # locate the target by drawing rectangle - cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2) - # calculate the rectangle center - x, y = (x*2+w)/2, (y*2+h)/2 - # calculate the real coordinates of mycobot relative to the target - if mycolor == "red": - self.color = 0 - - elif mycolor == "green": - self.color = 1 - - elif mycolor == "cyan" or mycolor == "blue": - self.color = 2 - - else: - self.color = 3 - - - if abs(x) + abs(y) > 0: - return x, y - else: - return None - - -if __name__ == "__main__": - # open the camera - cap_num = 0 - cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) - - if not cap.isOpened(): - cap.open() - # init a class of Object_detect - detect = Object_detect() - # init mycobot - detect.run() - - _init_ = 20 # - init_num = 0 - nparams = 0 - num = 0 - real_sx = real_sy = 0 - while cv2.waitKey(1) < 0: - # read camera - _, frame = cap.read() - # deal img - frame = detect.transform_frame(frame) - if _init_ > 0: - _init_ -= 1 - continue - # calculate the parameters of camera clipping - if init_num < 20: - if detect.get_calculate_params(frame) is None: - cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - init_num += 1 - continue - elif init_num == 20: - detect.set_cut_params( - (detect.sum_x1)/20.0, - (detect.sum_y1)/20.0, - (detect.sum_x2)/20.0, - (detect.sum_y2)/20.0, - ) - detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num += 1 - continue - - # calculate params of the coords between cube and mycobot - if nparams < 10: - if detect.get_calculate_params(frame) is None: - cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - nparams += 1 - continue - elif nparams == 10: - nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot - detect.set_params( - (detect.sum_x1+detect.sum_x2)/20.0, - (detect.sum_y1+detect.sum_y2)/20.0, - abs(detect.sum_x1-detect.sum_x2)/10.0 + - abs(detect.sum_y1-detect.sum_y2)/10.0 - ) - print ("ok") - continue - - # get detect result - detect_result = detect.color_detect(frame) - if detect_result is None: - cv2.imshow("figure", frame) - continue - else: - x, y = detect_result - # calculate real coord between cube and mycobot - real_x, real_y = detect.get_position(x, y) - if num == 20: - detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0) - detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color) - num = real_sx = real_sy = 0 - - else: - num += 1 - real_sy += real_y - real_sx += real_x - - cv2.imshow("figure", frame) diff --git a/mycobot_ai/ai_mycobot_280/scripts/aikit_img.py b/mycobot_ai/ai_mycobot_280/scripts/aikit_img.py deleted file mode 100644 index f071a8a..0000000 --- a/mycobot_ai/ai_mycobot_280/scripts/aikit_img.py +++ /dev/null @@ -1,555 +0,0 @@ -# encoding=utf-8 -from multiprocessing import Process, Pipe -from cgi import parse -from difflib import restore -# import queue -from sys import path -from tokenize import Pointfloat -from turtle import color -# from typing_extensions import Self -import cv2 -import numpy as np -import time -import json -import os,sys - -from PIL import Image -from threading import Thread -from pymycobot.mypalletizer import MyPalletizer - -IS_CV_4 = cv2.__version__[0] == '4' -__version__ = "1.0" # Adaptive seeed - - -class Object_detect(): - - def __init__(self, camera_x = 160, camera_y = 10): - # inherit the parent class - super(Object_detect, self).__init__() - - # declare mypal260 - self.mc = None - # 移动角度 - self.move_angles = [ - [0, 0, 0, 0], # init the point - [-29.0, 5.88, -4.92, -76.28], # point to grab - [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab - ] - - # 移动坐标 - self.move_coords = [ - [132.6, -155.6, 211.8, -20.9], # above the red bucket - [232.5, -134.1, 197.7, -45.26], # above the green bucket - [111.6, 159, 221.5, -120], # above the blue bucket - [-15.9, 164.6, 217.5, -119.35], # above the gray bucket - ] - - # choose place to set cube - self.color = 0 - # parameters to calculate camera clipping parameters - self.x1 = self.x2 = self.y1 = self.y2 = 0 - # set cache of real coord - self.cache_x = self.cache_y = 0 - - # use to calculate coord between cube and mycobot - self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 - # The coordinates of the grab center point relative to the mycobot - self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mycobot - self.c_x, self.c_y = 0, 0 - # The ratio of pixels to actual values - self.ratio = 0 - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv2.aruco.DetectorParameters_create() - - # 开启吸泵 m5 - def pump_on(self): - # 让2号位工作 - # self.mc.set_basic_output(2, 0) - # 让5号位工作 - self.mc.set_basic_output(5, 0) - - # 停止吸泵 m5 - def pump_off(self): - # 让2号位停止工作 - # self.mc.set_basic_output(2, 1) - # 让5号位停止工作 - self.mc.set_basic_output(5, 1) - - # Grasping motion - def move(self, x, y, color): - # send Angle to move mypal260 - self.mc.send_angles(self.move_angles[0], 20) - time.sleep(3) - - # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度 - self.mc.send_coords([x, y, 160, 0], 20, 0) - time.sleep(1.5) - self.mc.send_coords([x, y, 110, 0], 20, 0) - time.sleep(1.5) - - # open pump - self.pump_on() - time.sleep(1.5) - - self.mc.send_angle(2, 0, 20) - time.sleep(0.3) - self.mc.send_angle(3, -18, 20) - time.sleep(2) - - self.mc.send_coords(self.move_coords[color], 20, 1) - - time.sleep(3) - - # close pump - - self.pump_off() - time.sleep(6) - - self.mc.send_angles(self.move_angles[1], 20) - time.sleep(1.5) - self.mc.send_angles([-30, 0, 0, 0], 20) - time.sleep(1.5) - - # decide whether grab cube - def decide_move(self, x, y, color): - print(x, y, self.cache_x, self.cache_y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - print('start...') - self.move(x, y, color) - print('end....') - - # init mypal260 - def run(self): - - self.mc = MyPalletizer("COM9", 115200) - - self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) - time.sleep(3) - - # draw aruco - def draw_marker(self, img, x, y): - # draw rectangle on img - cv2.rectangle( - img, - (x - 20, y - 20), - (x + 20, y + 20), - (0, 255, 0), - thickness=2, - lineType=cv2.FONT_HERSHEY_COMPLEX, - ) - # add text on rectangle - cv2.putText( - img, - "({},{})".format(x, y), - (x, y), - cv2.FONT_HERSHEY_COMPLEX_SMALL, - 1, - (243, 0, 0), - 2, - ) - - # get points of two aruco - def get_calculate_params(self, img): - # Convert the image to a gray image - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params) - - """ - Two Arucos must be present in the picture and in the same order. - There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. - Determine the center of the aruco by the four corners of the aruco. - """ - if len(corners) > 0: - if ids is not None: - if len(corners) <= 1 or ids[0] == 1: - return None - x1 = x2 = y1 = y2 = 0 - point_11, point_21, point_31, point_41 = corners[0][0] - x1, y1 = int( - (point_11[0] + point_21[0] + point_31[0] + point_41[0]) / - 4.0), int( - (point_11[1] + point_21[1] + point_31[1] + point_41[1]) - / 4.0) - point_1, point_2, point_3, point_4 = corners[1][0] - x2, y2 = int( - (point_1[0] + point_2[0] + point_3[0] + point_4[0]) / - 4.0), int( - (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / - 4.0) - return x1, x2, y1, y2 - return None - - # set camera clipping parameters - def set_cut_params(self, x1, y1, x2, y2): - self.x1 = int(x1) - self.y1 = int(y1) - self.x2 = int(x2) - self.y2 = int(y2) - print(self.x1, self.y1, self.x2, self.y2) - - # set parameters to calculate the coords between cube and mycobot - def set_params(self, c_x, c_y, ratio): - self.c_x = c_x - self.c_y = c_y - self.ratio = 220.0 / ratio - - # calculate the coords between cube and mycobot - def get_position(self, x, y): - return ((y - self.c_y) * self.ratio + - self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y) - - """ - Calibrate the camera according to the calibration parameters. - Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. - If two ARuco values have been calculated, clip the video. - """ - - def transform_frame(self, frame): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) - if self.x1 != self.x2: - # the cutting ratio here is adjusted according to the actual situation - frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15), - int(self.x1 * 0.7):int(self.x2 * 1.15)] - return frame - - # according the class_id to get object name - def id_class_name(self, class_id): - for key, value in self.labels.items(): - if class_id == int(key): - return value - - # detect object - def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): - i = 0 - MIN_MATCH_COUNT = 5 - # sift = cv2.xfeatures2d.SIFT_create() - - # find the keypoints and descriptors with SIFT - # kp = [] - # des = [] - kp = kp_list - des = desc_list - - # for i in goal: - # kp0, des0 = sift.detectAndCompute(i, None) - # kp.append(kp0) - # des.append(des0) - - # kp1, des1 = sift.detectAndCompute(goal, None) - # kp2, des2 = sift.detectAndCompute(img, None) - kp2, des2 = kp_img, desc_img - - # FLANN parameters - FLANN_INDEX_KDTREE = 0 - index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) - search_params = dict(checks=50) # or pass empty dictionary - flann = cv2.FlannBasedMatcher(index_params, search_params) - - x, y = 0, 0 - try: - for i in range(len(des)): - matches = flann.knnMatch(des[i], des2, k=2) - # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 - good = [] - for m, n in matches: - if m.distance < 0.7 * n.distance: - good.append(m) - - # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 - if len(good) > MIN_MATCH_COUNT: - - # extract corresponding point pairs from matching 从匹配中提取出对应点对 - # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 - src_pts = np.float32([kp[i][m.queryIdx].pt - for m in good]).reshape(-1, 1, 2) - dst_pts = np.float32([kp2[m.trainIdx].pt - for m in good]).reshape(-1, 1, 2) - - # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 - M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, - 5.0) - matchesMask = mask.ravel().tolist() - # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 - h, w, d = goal[i].shape - pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], - [w - 1, 0]]).reshape(-1, 1, 2) - dst = cv2.perspectiveTransform(pts, M) - coord = (dst[0][0] + dst[1][0] + dst[2][0] + - dst[3][0]) / 4.0 - connection.send((DRAW_COORDS, coord)) - # cv2.putText(img, "{}".format(coord), (50, 60), - # fontFace=None, fontScale=1, - # color=(0, 255, 0), lineType=1) - print(format(dst[0][0][0])) - x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + - dst[3][0][0]) / 4.0 - y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + - dst[3][0][1]) / 4.0 - - # bound box 绘制边框 - # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - connection.send((DRAW_RECT, dst)) - # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) - except Exception as e: - pass - - if x + y > 0: - return x, y - else: - return None - -# The path to save the image folder -def parse_folder(folder): - restore = [] - path1 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder - path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' + folder - - # if os.path.exists(path1): - # path = path1 - # elif os.path.exists(path2): - path = path1 - - for i, j, k in os.walk(path): - for l in k: - restore.append(cv2.imread(folder + '/{}'.format(l))) - return restore - -def compute_keypoints_and_descriptors(sift, images_lists): - kp_list = [] - desc_list = [] - for images in images_lists: - kp_tmp = [] - desc_tmp = [] - for img in images: - kp, desc = sift.detectAndCompute(img, None) - kp_tmp.append(kp) - desc_tmp.append(desc) - kp_list.append(kp_tmp) - desc_list.append(desc_tmp) - - return kp_list, desc_list - -GET_FRAME = 1 -STOP_PROCESSING = 2 -DRAW_COORDS = 3 -DRAW_RECT = 4 -CLEAR_DRAW = 5 -CROP_FRAME = 6 - -def get_frame(connection): - connection.send(GET_FRAME) - frame = connection.recv() - return frame - -def process_transform_frame(frame, x1, y1, x2, y2): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) -# if x1 != x2: - # the cutting ratio here is adjusted according to the actual situation -# frame = frame[int(y2 * 0.2):int(y1 * 1.15), -# int(x1 * 0.7):int(x2 * 1.15)] - return frame - -def process_display_frame(connection): - cap_num = 1 - coord = None - dst = None - x1 = 0 - y1 = 0 - x2 = 0 - y2 = 0 - cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) - if not cap.isOpened(): - cap.open(1) - while cv2.waitKey(1) < 0: - _, frame = cap.read() - frame = process_transform_frame(frame, x1, y1, x2, y2) - if connection.poll(): - request = connection.recv() - if request == GET_FRAME: - connection.send(frame) - elif request == CLEAR_DRAW: - coord = None - dst = None - elif type(request) is tuple: - if request[0] == DRAW_COORDS: - coord = request[1] - elif request[0] == DRAW_RECT: - dst = request[1] - elif request[0] == CROP_FRAME: - x1 = request[1] - y1 = request[2] - x2 = request[3] - y2 = request[4] - - if not coord is None: - cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, - fontScale=1, color=(0, 255, 0), lineType=1) - if not dst is None: - frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - cv2.imshow("figure", frame) - time.sleep(0.04) - connection.send(STOP_PROCESSING) - -def run(): - parent_conn, child_conn = Pipe() - child = Process(target = process_display_frame, args=(child_conn,)) - child.start() - - res_queue = [[], [], [], []] - res_queue[0] = parse_folder('res/red') - res_queue[1] = parse_folder('res/green') - res_queue[2] = parse_folder('res/blue') - res_queue[3] = parse_folder('res/gray') - - sift = cv2.xfeatures2d.SIFT_create() - kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) - - # init a class of Object_detect - detect = Object_detect() - - # init mycobot - detect.run() - - # _init_ = 20 # - init_num = 0 - nparams = 0 - # num = 0 - # real_sx = real_sy = 0 - while True: - start_time = time.time() - if parent_conn.poll(): - data = parent_conn.recv() - if data == STOP_PROCESSING: - break - # read camera - frame = get_frame(parent_conn) - # deal img - #frame = detect.transform_frame(frame) - - # if _init_ > 0: - # _init_ -= 1 - # continue - # calculate the parameters of camera clipping - if init_num < 20: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - init_num += 1 - continue - elif init_num == 20: - detect.set_cut_params( - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0, - ) - parent_conn.send((CROP_FRAME, - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0)) - detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num += 1 - continue - - # calculate params of the coords between cube and mycobot - if nparams < 10: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - nparams += 1 - print ("ok") - continue - elif nparams == 10: - nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot - detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0, - (detect.sum_y1 + detect.sum_y2) / 20.0, - abs(detect.sum_x1 - detect.sum_x2) / 10.0 + - abs(detect.sum_y1 - detect.sum_y2) / 10.0) - print("ok") - continue - - # get detect result - kp_img, desc_img = sift.detectAndCompute(frame, None) - frame = get_frame(parent_conn) - for i, v in enumerate(res_queue): - # HACK: to update frame every time - detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) - if detect_result: - x, y = detect_result - # calculate real coord between cube and mycobot - real_x, real_y = detect.get_position(x, y) - detect.color = i - detect.pub_marker(real_x / 1000.0, real_y / 1000.0) - detect.decide_move(real_x, real_y, detect.color) - # if num == 5: - # detect.color = i - # detect.pub_marker(real_sx / 5.0 / 1000.0, - # real_sy / 5.0 / 1000.0) - # detect.decide_move(real_sx / 5.0, real_sy / 5.0, - # detect.color) - # num = real_sx = real_sy = 0 - # else: - # num += 1 - # real_sy += real_y - # real_sx += real_x - parent_conn.send(CLEAR_DRAW) - - # cv2.imshow("figure", frame) - time.sleep(0.05) - end_time = time.time() - # print("loop_time = ", end_time - start_time) - - # close the window - if cv2.waitKey(1) & 0xFF == ord('q'): - # cap.release() - cv2.destroyAllWindows() - sys.exit() - - child.join() - - -if __name__ == "__main__": - run() - # Object_detect().take_photo() - # Object_detect().cut_photo() \ No newline at end of file diff --git a/mycobot_ai/ai_mycobot_280/scripts/encode_test.py b/mycobot_ai/ai_mycobot_280/scripts/encode_test.py deleted file mode 100644 index dfd5c47..0000000 --- a/mycobot_ai/ai_mycobot_280/scripts/encode_test.py +++ /dev/null @@ -1,248 +0,0 @@ -# encoding: UTF-8 -#!/usr/bin/env python2 -import cv2 as cv -import os -import numpy as np -import time -import rospy -from visualization_msgs.msg import Marker -from moving_utils import Movement - -# y轴偏移量 -pump_y = -55 -# x轴偏移量 -pump_x = 15 - - -class Detect_marker(Movement): - def __init__(self): - super(Detect_marker, self).__init__() - # set cache of real coord - self.cache_x = self.cache_y = 0 - - # which robot - self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1] - self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1] - self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1] - self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1] - self.raspi = False - if "dev" in self.robot_m5: - self.Pin = [2, 5] - elif "dev" in self.robot_wio: - self.Pin = [20, 21] - # for i in self.move_coords: - # i[2] -= 20 - elif "dev" in self.robot_raspi or "dev" in self.robot_jes: - import RPi.GPIO as GPIO - self.GPIO = GPIO - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(20, GPIO.OUT) - GPIO.setup(21, GPIO.OUT) - self.raspi = True - if self.raspi: - self.gpio_status(False) - else: - self.pub_pump(False, self.Pin) - # Creating a Camera Object - cap_num = 0 - self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L) - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv.aruco.DetectorParameters_create() - self.calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ) - # Get distance coefficient. - self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat() - - height = self.cap.get(4) - focal_length = width = self.cap.get(3) - center = [width / 2, height / 2] - # Calculate the camera matrix. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - # init a node and a publisher - rospy.init_node("encode_marker", anonymous=True) - self.pub = rospy.Publisher('/cube', Marker, queue_size=1) - - self.marker = Marker() - self.marker.header.frame_id = "/joint1" - self.marker.ns = "cube" - self.marker.type = self.marker.CUBE - self.marker.action = self.marker.ADD - self.marker.scale.x = 0.04 - self.marker.scale.y = 0.04 - self.marker.scale.z = 0.04 - self.marker.color.a = 1 - self.marker.color.r = 0.3 - self.marker.color.g = 0.3 - self.marker.color.b = 0.3 - - # marker position initial - self.marker.pose.position.x = 0 - self.marker.pose.position.y = 0 - self.marker.pose.position.z = 0.03 - self.marker.pose.orientation.x = 0 - self.marker.pose.orientation.y = 0 - self.marker.pose.orientation.z = 0 - self.marker.pose.orientation.w = 1.0 - - # Grasping motion - def move(self, x, y): - if self.raspi: - coords = [ - [145.6, -64.9, 285.2, 179.88, 7.67, 179], - [130.1, -155.6, 243.9, 178.99, 5.38, -179.9] - ] - else: - coords = [ - [135.0, -65.5, 280.1, 178.99, 5.38, -179.9], - [136.1, -141.6, 243.9, 178.99, 5.38, -179.9] - ] - - # publish marker - self.marker.header.stamp = rospy.Time.now() - self.marker.pose.position.x = (coords[0][0]-x)/1000.0 - self.marker.pose.position.y = (coords[0][1]-y)/1000.0 - self.pub.publish(self.marker) - - # send coordinates to move mycobot - self.pub_coords(coords[0], 30, 1) - time.sleep(2) - self.pub_coords([coords[0][0]-x, coords[0][1]-y, - 240, 178.99, 5.38, -179.9], 25, 1) - time.sleep(2) - self.pub_coords([coords[0][0]-x, coords[0][1]-y, - 200, 178.99, 5.38, -179.9], 25, 1) - time.sleep(2) - if "dev" in self.robot_m5 or self.raspi: - self.pub_coords([coords[0][0]-x, coords[0][1]-y, - 90, 178.99, 5.38, -179.9], 25, 1) - elif "dev" in self.robot_wio: - self.pub_coords([coords[0][0]-x+20, coords[0][1] - - y-10, 70, 178.99, 5.38, -179.9], 25, 1) - time.sleep(2) - if self.raspi: - self.gpio_status(True) - else: - self.pub_pump(True, self.Pin) - time.sleep(1) - self.pub_coords(coords[0], 30, 1) - time.sleep(3) - self.pub_coords(coords[1], 30, 1) - time.sleep(2) - if self.raspi: - self.gpio_status(False) - else: - self.pub_pump(False, self.Pin) - # publish marker - time.sleep(1) - self.marker.header.stamp = rospy.Time.now() - self.marker.pose.position.x = coords[1][0]/1000.0 - self.marker.pose.position.y = coords[1][1]/1000.0 - self.pub.publish(self.marker) - - self.pub_coords(coords[0], 30, 1) - time.sleep(2) - - def gpio_status(self, flag): - if flag: - self.GPIO.output(20, 0) - self.GPIO.output(21, 0) - else: - self.GPIO.output(20, 1) - self.GPIO.output(21, 1) - - # decide whether grab cube - def decide_move(self, x, y): - - print(x, y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - if "dev" in self.robot_jes: - if x > -20: - y += 10 - if y > -25: - x -= 5 - x += 10 - - self.move(x, y) - - # init mycobot - def init_mycobot(self): - - for _ in range(5): - print(_) - self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1) - time.sleep(0.5) - - def run(self): - global pump_y, pump_x - self.init_mycobot() - num = sum_x = sum_y = 0 - while cv.waitKey(1) < 0: - success, img = self.cap.read() - if not success: - print("It seems that the image cannot be acquired correctly.") - break - - # transfrom the img to model of gray - gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params - ) - font = cv.FONT_HERSHEY_SIMPLEX - if len(corners) > 0: - if ids is not None: - # get informations of aruco - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.03, self.camera_matrix, self.dist_coeffs - ) - # rvec:rotation offset,tvec:translation deviator - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - xyz = tvec[0, 0, :] - # calculate the coordinates of the aruco relative to the pump - xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1] - * 1000+pump_x, 2), round(xyz[2]*1000, 2)] - cv.putText(img, "Id: " + str(ids[0][0]), (0, 40), font, 0.6, (0, 255, 0), 2, cv.LINE_AA) - for i in range(rvec.shape[0]): - # draw the aruco on img - cv.aruco.drawDetectedMarkers(img, corners) - cv.aruco.drawAxis( - img, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - if num < 40: - if self.raspi: - sum_x -= 30 - sum_x += xyz[1] - sum_y += xyz[0] - num += 1 - elif num == 40: - self.decide_move(sum_x/40.0, sum_y/40.0) - num = sum_x = sum_y = 0 - - cv.imshow("encode_image", img) - - -if __name__ == "__main__": - detect = Detect_marker() - detect.run() diff --git a/mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py b/mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py deleted file mode 100644 index 770ee11..0000000 --- a/mycobot_ai/ai_mypalletizer_260/scripts/aikit_img.py +++ /dev/null @@ -1,557 +0,0 @@ -# encoding=utf-8 -#!/usr/bin/env python2 -from multiprocessing import Process, Pipe -from cgi import parse -from difflib import restore -# import queue -from sys import path -from tokenize import Pointfloat -from turtle import color -# from typing_extensions import Self -import cv2 -import numpy as np -import time -import json -import os,sys - -from PIL import Image -from threading import Thread -from pymycobot.mypalletizer import MyPalletizer - -IS_CV_4 = cv2.__version__[0] == '4' -__version__ = "1.0" # Adaptive seeed - -__metaclass__ = type - -class Object_detect(): - - def __init__(self, camera_x = 160, camera_y = 10): - # inherit the parent class - super(Object_detect, self).__init__() - - # declare mypal260 - self.mc = None - # 移动角度 - self.move_angles = [ - [0, 0, 0, 0], # init the point - [-29.0, 5.88, -4.92, -76.28], # point to grab - [17.4, -10.1, -87.27, 5.8, -2.02, 15], # point to grab - ] - - # 移动坐标 - self.move_coords = [ - [132.6, -155.6, 211.8, -20.9], # above the red bucket - [232.5, -134.1, 197.7, -45.26], # above the green bucket - [111.6, 159, 221.5, -120], # above the blue bucket - [-15.9, 164.6, 217.5, -119.35], # above the gray bucket - ] - - # choose place to set cube - self.color = 0 - # parameters to calculate camera clipping parameters - self.x1 = self.x2 = self.y1 = self.y2 = 0 - # set cache of real coord - self.cache_x = self.cache_y = 0 - - # use to calculate coord between cube and mycobot - self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 - # The coordinates of the grab center point relative to the mycobot - self.camera_x, self.camera_y = camera_x, camera_y - # The coordinates of the cube relative to the mycobot - self.c_x, self.c_y = 0, 0 - # The ratio of pixels to actual values - self.ratio = 0 - # Get ArUco marker dict that can be detected. - self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) - # Get ArUco marker params. - self.aruco_params = cv2.aruco.DetectorParameters_create() - - # 开启吸泵 m5 - def pump_on(self): - # 让2号位工作 - # self.mc.set_basic_output(2, 0) - # 让5号位工作 - self.mc.set_basic_output(5, 0) - - # 停止吸泵 m5 - def pump_off(self): - # 让2号位停止工作 - # self.mc.set_basic_output(2, 1) - # 让5号位停止工作 - self.mc.set_basic_output(5, 1) - - # Grasping motion - def move(self, x, y, color): - # send Angle to move mypal260 - self.mc.send_angles(self.move_angles[0], 20) - time.sleep(3) - - # send coordinates to move mypal260 根据不同底板机械臂，调整吸泵高度 - self.mc.send_coords([x, y, 160, 0], 20, 0) - time.sleep(1.5) - self.mc.send_coords([x, y, 110, 0], 20, 0) - time.sleep(1.5) - - # open pump - self.pump_on() - time.sleep(1.5) - - self.mc.send_angle(2, 0, 20) - time.sleep(0.3) - self.mc.send_angle(3, -18, 20) - time.sleep(2) - - self.mc.send_coords(self.move_coords[color], 20, 1) - - time.sleep(3) - - # close pump - - self.pump_off() - time.sleep(6) - - self.mc.send_angles(self.move_angles[1], 20) - time.sleep(1.5) - self.mc.send_angles([-30, 0, 0, 0], 20) - time.sleep(1.5) - - # decide whether grab cube - def decide_move(self, x, y, color): - print(x, y, self.cache_x, self.cache_y) - # detect the cube status move or run - if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm - self.cache_x, self.cache_y = x, y - return - else: - self.cache_x = self.cache_y = 0 - # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动 - print('start...') - self.move(x, y, color) - print('end....') - - # init mypal260 - def run(self): - - self.mc = MyPalletizer("/dev/ttyUSB0", 115200) - - self.mc.send_angles([-29.0, 5.88, -4.92, -76.28], 20) - time.sleep(3) - - # draw aruco - def draw_marker(self, img, x, y): - # draw rectangle on img - cv2.rectangle( - img, - (x - 20, y - 20), - (x + 20, y + 20), - (0, 255, 0), - thickness=2, - lineType=cv2.FONT_HERSHEY_COMPLEX, - ) - # add text on rectangle - cv2.putText( - img, - "({},{})".format(x, y), - (x, y), - cv2.FONT_HERSHEY_COMPLEX_SMALL, - 1, - (243, 0, 0), - 2, - ) - - # get points of two aruco - def get_calculate_params(self, img): - # Convert the image to a gray image - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # Detect ArUco marker. - corners, ids, rejectImaPoint = cv2.aruco.detectMarkers( - gray, self.aruco_dict, parameters=self.aruco_params) - - """ - Two Arucos must be present in the picture and in the same order. - There are two Arucos in the Corners, and each aruco contains the pixels of its four corners. - Determine the center of the aruco by the four corners of the aruco. - """ - if len(corners) > 0: - if ids is not None: - if len(corners) <= 1 or ids[0] == 1: - return None - x1 = x2 = y1 = y2 = 0 - point_11, point_21, point_31, point_41 = corners[0][0] - x1, y1 = int( - (point_11[0] + point_21[0] + point_31[0] + point_41[0]) / - 4.0), int( - (point_11[1] + point_21[1] + point_31[1] + point_41[1]) - / 4.0) - point_1, point_2, point_3, point_4 = corners[1][0] - x2, y2 = int( - (point_1[0] + point_2[0] + point_3[0] + point_4[0]) / - 4.0), int( - (point_1[1] + point_2[1] + point_3[1] + point_4[1]) / - 4.0) - return x1, x2, y1, y2 - return None - - # set camera clipping parameters - def set_cut_params(self, x1, y1, x2, y2): - self.x1 = int(x1) - self.y1 = int(y1) - self.x2 = int(x2) - self.y2 = int(y2) - print(self.x1, self.y1, self.x2, self.y2) - - # set parameters to calculate the coords between cube and mycobot - def set_params(self, c_x, c_y, ratio): - self.c_x = c_x - self.c_y = c_y - self.ratio = 220.0 / ratio - - # calculate the coords between cube and mycobot - def get_position(self, x, y): - return ((y - self.c_y) * self.ratio + - self.camera_x), ((x - self.c_x) * self.ratio + self.camera_y) - - """ - Calibrate the camera according to the calibration parameters. - Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times. - If two ARuco values have been calculated, clip the video. - """ - - def transform_frame(self, frame): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) - if self.x1 != self.x2: - # the cutting ratio here is adjusted according to the actual situation - frame = frame[int(self.y2 * 0.2):int(self.y1 * 1.15), - int(self.x1 * 0.7):int(self.x2 * 1.15)] - return frame - - # according the class_id to get object name - def id_class_name(self, class_id): - for key, value in self.labels.items(): - if class_id == int(key): - return value - - # detect object - def obj_detect(self, img, goal, kp_img, desc_img, kp_list, desc_list, connection): - i = 0 - MIN_MATCH_COUNT = 5 - # sift = cv2.xfeatures2d.SIFT_create() - - # find the keypoints and descriptors with SIFT - # kp = [] - # des = [] - kp = kp_list - des = desc_list - - # for i in goal: - # kp0, des0 = sift.detectAndCompute(i, None) - # kp.append(kp0) - # des.append(des0) - - # kp1, des1 = sift.detectAndCompute(goal, None) - # kp2, des2 = sift.detectAndCompute(img, None) - kp2, des2 = kp_img, desc_img - - # FLANN parameters - FLANN_INDEX_KDTREE = 0 - index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) - search_params = dict(checks=50) # or pass empty dictionary - flann = cv2.FlannBasedMatcher(index_params, search_params) - - x, y = 0, 0 - try: - for i in range(len(des)): - matches = flann.knnMatch(des[i], des2, k=2) - # store all the good matches as per Lowe's ratio test. 根据Lowe比率测试存储所有良好匹配项。 - good = [] - for m, n in matches: - if m.distance < 0.7 * n.distance: - good.append(m) - - # When there are enough robust matching point pairs 当有足够的健壮匹配点对（至少个MIN_MATCH_COUNT）时 - if len(good) > MIN_MATCH_COUNT: - - # extract corresponding point pairs from matching 从匹配中提取出对应点对 - # query index of small objects, training index of scenarios 小对象的查询索引，场景的训练索引 - src_pts = np.float32([kp[i][m.queryIdx].pt - for m in good]).reshape(-1, 1, 2) - dst_pts = np.float32([kp2[m.trainIdx].pt - for m in good]).reshape(-1, 1, 2) - - # Using matching points to find homography matrix in cv2.ransac 利用匹配点找到CV2.RANSAC中的单应矩阵 - M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, - 5.0) - matchesMask = mask.ravel().tolist() - # Calculate the distortion of image, that is the corresponding position in frame 计算图1的畸变，也就是在图2中的对应的位置 - h, w, d = goal[i].shape - pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], - [w - 1, 0]]).reshape(-1, 1, 2) - dst = cv2.perspectiveTransform(pts, M) - coord = (dst[0][0] + dst[1][0] + dst[2][0] + - dst[3][0]) / 4.0 - connection.send((DRAW_COORDS, coord)) - # cv2.putText(img, "{}".format(coord), (50, 60), - # fontFace=None, fontScale=1, - # color=(0, 255, 0), lineType=1) - print(format(dst[0][0][0])) - x = (dst[0][0][0] + dst[1][0][0] + dst[2][0][0] + - dst[3][0][0]) / 4.0 - y = (dst[0][0][1] + dst[1][0][1] + dst[2][0][1] + - dst[3][0][1]) / 4.0 - - # bound box 绘制边框 - # img = cv2.polylines(img, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - connection.send((DRAW_RECT, dst)) - # cv2.polylines(mixture, [np.int32(dst)], True, (0, 255, 0), 2, cv2.LINE_AA) - except Exception as e: - pass - - if x + y > 0: - return x, y - else: - return None - -# The path to save the image folder -def parse_folder(folder): - restore = [] - path1 = '/home/h/catkin_ws/src/mycobot_ros/mycobot_ai/ai_mypalletizer_260/' + folder - path2 = r'D:/BaiduSyncdisk/PythonProject/OpenCV' + folder - - # if os.path.exists(path1): - # path = path1 - # elif os.path.exists(path2): - path = path1 - - for i, j, k in os.walk(path): - for l in k: - restore.append(cv2.imread(folder + '/{}'.format(l))) - return restore - -def compute_keypoints_and_descriptors(sift, images_lists): - kp_list = [] - desc_list = [] - for images in images_lists: - kp_tmp = [] - desc_tmp = [] - for img in images: - kp, desc = sift.detectAndCompute(img, None) - kp_tmp.append(kp) - desc_tmp.append(desc) - kp_list.append(kp_tmp) - desc_list.append(desc_tmp) - - return kp_list, desc_list - -GET_FRAME = 1 -STOP_PROCESSING = 2 -DRAW_COORDS = 3 -DRAW_RECT = 4 -CLEAR_DRAW = 5 -CROP_FRAME = 6 - -def get_frame(connection): - connection.send(GET_FRAME) - frame = connection.recv() - return frame - -def process_transform_frame(frame, x1, y1, x2, y2): - # enlarge the image by 1.5 times - fx = 1.5 - fy = 1.5 - frame = cv2.resize(frame, (0, 0), - fx=fx, - fy=fy, - interpolation=cv2.INTER_CUBIC) -# if x1 != x2: - # the cutting ratio here is adjusted according to the actual situation -# frame = frame[int(y2 * 0.2):int(y1 * 1.15), -# int(x1 * 0.7):int(x2 * 1.15)] - return frame - -def process_display_frame(connection): - cap_num = 0 - coord = None - dst = None - x1 = 0 - y1 = 0 - x2 = 0 - y2 = 0 - cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L) - if not cap.isOpened(): - cap.open() - while cv2.waitKey(1) < 0: - _, frame = cap.read() - frame = process_transform_frame(frame, x1, y1, x2, y2) - if connection.poll(): - request = connection.recv() - if request == GET_FRAME: - connection.send(frame) - elif request == CLEAR_DRAW: - coord = None - dst = None - elif type(request) is tuple: - if request[0] == DRAW_COORDS: - coord = request[1] - elif request[0] == DRAW_RECT: - dst = request[1] - elif request[0] == CROP_FRAME: - x1 = request[1] - y1 = request[2] - x2 = request[3] - y2 = request[4] - - if not coord is None: - cv2.putText(frame, "{}".format(coord), (50, 60), fontFace=None, - fontScale=1, color=(0, 255, 0), lineType=1) - if not dst is None: - frame = cv2.polylines(frame, [np.int32(dst)], True, 244, 3, cv2.LINE_AA) - cv2.imshow("figure", frame) - time.sleep(0.04) - connection.send(STOP_PROCESSING) - -def run(): - parent_conn, child_conn = Pipe() - child = Process(target = process_display_frame, args=(child_conn,)) - child.start() - - res_queue = [[], [], [], []] - res_queue[0] = parse_folder('res/red') - res_queue[1] = parse_folder('res/green') - res_queue[2] = parse_folder('res/blue') - res_queue[3] = parse_folder('res/gray') - - sift = cv2.xfeatures2d.SIFT_create() - kp_list, desc_list = compute_keypoints_and_descriptors(sift, res_queue) - - # init a class of Object_detect - detect = Object_detect() - - # init mycobot - detect.run() - - # _init_ = 20 # - init_num = 0 - nparams = 0 - # num = 0 - # real_sx = real_sy = 0 - while True: - start_time = time.time() - if parent_conn.poll(): - data = parent_conn.recv() - if data == STOP_PROCESSING: - break - # read camera - frame = get_frame(parent_conn) - # deal img - #frame = detect.transform_frame(frame) - - # if _init_ > 0: - # _init_ -= 1 - # continue - # calculate the parameters of camera clipping - if init_num < 20: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - init_num += 1 - continue - elif init_num == 20: - detect.set_cut_params( - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0, - ) - parent_conn.send((CROP_FRAME, - (detect.sum_x1) / 20.0, - (detect.sum_y1) / 20.0, - (detect.sum_x2) / 20.0, - (detect.sum_y2) / 20.0)) - detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0 - init_num += 1 - continue - - # calculate params of the coords between cube and mycobot - if nparams < 10: - if detect.get_calculate_params(frame) is None: - # cv2.imshow("figure", frame) - continue - else: - x1, x2, y1, y2 = detect.get_calculate_params(frame) - detect.draw_marker(frame, x1, y1) - detect.draw_marker(frame, x2, y2) - detect.sum_x1 += x1 - detect.sum_x2 += x2 - detect.sum_y1 += y1 - detect.sum_y2 += y2 - nparams += 1 - print ("ok") - continue - elif nparams == 10: - nparams += 1 - # calculate and set params of calculating real coord between cube and mycobot - detect.set_params((detect.sum_x1 + detect.sum_x2) / 20.0, - (detect.sum_y1 + detect.sum_y2) / 20.0, - abs(detect.sum_x1 - detect.sum_x2) / 10.0 + - abs(detect.sum_y1 - detect.sum_y2) / 10.0) - print("ok") - continue - - # get detect result - kp_img, desc_img = sift.detectAndCompute(frame, None) - frame = get_frame(parent_conn) - for i, v in enumerate(res_queue): - # HACK: to update frame every time - detect_result = detect.obj_detect(frame, v, kp_img, desc_img, kp_list[i], desc_list[i], parent_conn) - if detect_result: - x, y = detect_result - # calculate real coord between cube and mycobot - real_x, real_y = detect.get_position(x, y) - detect.color = i - # detect.pub_marker(real_x / 1000.0, real_y / 1000.0) - detect.decide_move(real_x, real_y, detect.color) - # if num == 5: - # detect.color = i - # detect.pub_marker(real_sx / 5.0 / 1000.0, - # real_sy / 5.0 / 1000.0) - # detect.decide_move(real_sx / 5.0, real_sy / 5.0, - # detect.color) - # num = real_sx = real_sy = 0 - # else: - # num += 1 - # real_sy += real_y - # real_sx += real_x - parent_conn.send(CLEAR_DRAW) - - # cv2.imshow("figure", frame) - time.sleep(0.05) - end_time = time.time() - # print("loop_time = ", end_time - start_time) - - # close the window - if cv2.waitKey(1) & 0xFF == ord('q'): - # cap.release() - cv2.destroyAllWindows() - sys.exit() - - child.join() - - -if __name__ == "__main__": - run() - # Object_detect().take_photo() - # Object_detect().cut_photo() \ No newline at end of file diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py index 1f5c89a..ce2dc4d 100755 --- a/mycobot_communication/scripts/mycobot_services.py +++ b/mycobot_communication/scripts/mycobot_services.py @@ -2,12 +2,51 @@ # -*- coding: utf-8 -* import time import rospy +import os +import fcntl from mycobot_communication.srv import * from pymycobot.mycobot import MyCobot mc = None +# Avoid serial port conflicts and need to be locked +def acquire(lock_file): + open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC + fd = os.open(lock_file, open_mode) + + pid = os.getpid() + lock_file_fd = None + + timeout = 50.0 + start_time = current_time = time.time() + while current_time < start_time + timeout: + try: + # The LOCK_EX means that only one process can hold the lock + # The LOCK_NB means that the fcntl.flock() is not blocking + # and we are able to implement termination of while loop, + # when timeout is reached. + fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB) + except (IOError, OSError): + pass + else: + lock_file_fd = fd + break + print('pid waiting for lock:%d'% pid) + time.sleep(1.0) + current_time = time.time() + if lock_file_fd is None: + os.close(fd) + return lock_file_fd + + +def release(lock_file_fd): + # Do not remove the lockfile: + fcntl.flock(lock_file_fd, fcntl.LOCK_UN) + os.close(lock_file_fd) + return None + + def create_handle(): global mc @@ -43,7 +82,9 @@ def set_angles(req): sp = req.speed print('angles1:',angles) if mc: + lock = acquire("/tmp/mycobot_lock") mc.send_angles(angles, sp) + release(lock) return SetAnglesResponse(True) @@ -51,8 +92,9 @@ def set_angles(req): def get_angles(req): """get angles,获取角度""" if mc: + lock = acquire("/tmp/mycobot_lock") angles = mc.get_angles() - print('angles2:',angles) + release(lock) return GetAnglesResponse(*angles) @@ -69,15 +111,18 @@ def set_coords(req): mod = req.model if mc: + lock = acquire("/tmp/mycobot_lock") mc.send_coords(coords, sp, mod) + release(lock) return SetCoordsResponse(True) def get_coords(req): if mc: + lock = acquire("/tmp/mycobot_lock") coords = mc.get_coords() - print('coords:',coords) + release(lock) return GetCoordsResponse(*coords) @@ -85,22 +130,26 @@ def switch_status(req): """Gripper switch status""" """夹爪开关状态""" if mc: + lock = acquire("/tmp/mycobot_lock") if req.Status: mc.set_gripper_state(0, 80) else: mc.set_gripper_state(1, 80) + release(lock) return GripperStatusResponse(True) def toggle_pump(req): if mc: + lock = acquire("/tmp/mycobot_lock") if req.Status: mc.set_basic_output(req.Pin1, 0) mc.set_basic_output(req.Pin2, 0) else: mc.set_basic_output(req.Pin1, 1) mc.set_basic_output(req.Pin2, 1) + release(lock) return PumpStatusResponse(True) @@ -133,11 +182,15 @@ def output_robot_message(): atom_version = "unknown" if mc: + lock = acquire("/tmp/mycobot_lock") cn = mc.is_controller_connected() + release(lock) if cn == 1: connect_status = True time.sleep(0.1) + lock = acquire("/tmp/mycobot_lock") si = mc.is_all_servo_enable() + release(lock) if si == 1: servo_infomation = "all connected"