From c3c3c2812d06591c874a8d25f27679a42fcd0733 Mon Sep 17 00:00:00 2001
From: wangWking <842749351@qq.com>
Date: Fri, 20 May 2022 17:25:00 +0800
Subject: [PATCH] add mycobot280_jetsonanno socket control

---
 .../mycobot_280jn/launch/simple_gui.launch    |  6 +-
 .../launch/teleop_keyboard.launch             |  6 +-
 .../mycobot_280jn/scripts/detect_marker.py    |  0
 .../mycobot_280jn/scripts/follow_and_pump.py  |  0
 .../mycobot_280jn/scripts/follow_display.py   | 15 +++--
 .../mycobot_280jn/scripts/following_marker.py |  0
 .../mycobot_280jn/scripts/listen_real.py      |  1 +
 .../scripts/listen_real_of_topic.py           |  0
 .../mycobot_280jn/scripts/simple_gui.py       |  0
 .../mycobot_280jn/scripts/slider_control.py   | 22 ++++---
 .../mycobot_280jn/scripts/teleop_keyboard.py  |  0
 mycobot_280/mycobot_280jn/scripts/test.py     | 12 ++++
 .../launch/communication_service.launch       |  8 +--
 .../scripts/mycobot_services.py               | 59 +++++++++++--------
 14 files changed, 81 insertions(+), 48 deletions(-)
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/detect_marker.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/follow_and_pump.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/follow_display.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/following_marker.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/listen_real.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/simple_gui.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/slider_control.py
 mode change 100644 => 100755 mycobot_280/mycobot_280jn/scripts/teleop_keyboard.py
 create mode 100755 mycobot_280/mycobot_280jn/scripts/test.py

diff --git a/mycobot_280/mycobot_280jn/launch/simple_gui.launch b/mycobot_280/mycobot_280jn/launch/simple_gui.launch
index 0a8d47a..d1b31e0 100644
--- a/mycobot_280/mycobot_280jn/launch/simple_gui.launch
+++ b/mycobot_280/mycobot_280jn/launch/simple_gui.launch
@@ -1,7 +1,7 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="port" default="/dev/ttyTHS1" />
-	<arg name="baud" default="1000000" />
+	<arg name="ip" default="192.168.125.226" />
+	<arg name="port" default="9000" />
 	<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
 	<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
@@ -15,8 +15,8 @@
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
     <include file="$(find mycobot_communication)/launch/communication_service.launch">
+		<arg name="ip" value="$(arg ip)" />	
 		<arg name="port" value="$(arg port)" />	
-		<arg name="baud" value="$(arg baud)" />	
     </include>
 	<!-- listen and pub the real angles ，监听并发布真实的角度-->
 	<node name="real_listener" pkg="mycobot_280jn" type="listen_real.py" />
diff --git a/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch b/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch
index 6e70fd2..f3a84db 100644
--- a/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch
+++ b/mycobot_280/mycobot_280jn/launch/teleop_keyboard.launch
@@ -1,7 +1,7 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="port" default="/dev/ttyTHS1" />
-	<arg name="baud" default="1000000" />
+	<arg name="ip" default="192.168.125.226" />
+	<arg name="port" default="9000" />
 	<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
 	<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
@@ -15,8 +15,8 @@
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
     <include file="$(find mycobot_communication)/launch/communication_service.launch">
+		<arg name="ip" value="$(arg ip)" />	
 		<arg name="port" value="$(arg port)" />	
-		<arg name="baud" value="$(arg baud)" />	
     </include>
 	<!-- listen and pub the real angles ，监听并发布真实的角度-->
 	<node name="real_listener" pkg="mycobot_280jn" type="listen_real.py" />
diff --git a/mycobot_280/mycobot_280jn/scripts/detect_marker.py b/mycobot_280/mycobot_280jn/scripts/detect_marker.py
old mode 100644
new mode 100755
diff --git a/mycobot_280/mycobot_280jn/scripts/follow_and_pump.py b/mycobot_280/mycobot_280jn/scripts/follow_and_pump.py
old mode 100644
new mode 100755
diff --git a/mycobot_280/mycobot_280jn/scripts/follow_display.py b/mycobot_280/mycobot_280jn/scripts/follow_display.py
old mode 100644
new mode 100755
index 0815622..7789d29
--- a/mycobot_280/mycobot_280jn/scripts/follow_display.py
+++ b/mycobot_280/mycobot_280jn/scripts/follow_display.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python2
+# encoding:utf8
 import time
 
 import rospy
@@ -8,16 +9,22 @@ from visualization_msgs.msg import Marker
 
 from pymycobot.mycobot import MyCobot
 
+from pymycobot import MyCobotSocket
 
 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))
+    # 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 = MyCobot(port, baud)
+        mycobot=MyCobotSocket(ip,netport)
+        mycobot.connect(serialport="/dev/ttyTHS1", baudrate="1000000")
     except Exception as e:
         print(e)
         print(
diff --git a/mycobot_280/mycobot_280jn/scripts/following_marker.py b/mycobot_280/mycobot_280jn/scripts/following_marker.py
old mode 100644
new mode 100755
diff --git a/mycobot_280/mycobot_280jn/scripts/listen_real.py b/mycobot_280/mycobot_280jn/scripts/listen_real.py
old mode 100644
new mode 100755
index 5e4d9bc..1d38b61
--- a/mycobot_280/mycobot_280jn/scripts/listen_real.py
+++ b/mycobot_280/mycobot_280jn/scripts/listen_real.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python2
+# encoding:utf-8
 # license removed for brevity
 import time
 import math
diff --git a/mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py b/mycobot_280/mycobot_280jn/scripts/listen_real_of_topic.py
old mode 100644
new mode 100755
diff --git a/mycobot_280/mycobot_280jn/scripts/simple_gui.py b/mycobot_280/mycobot_280jn/scripts/simple_gui.py
old mode 100644
new mode 100755
diff --git a/mycobot_280/mycobot_280jn/scripts/slider_control.py b/mycobot_280/mycobot_280jn/scripts/slider_control.py
old mode 100644
new mode 100755
index a40abc0..da901f6
--- a/mycobot_280/mycobot_280jn/scripts/slider_control.py
+++ b/mycobot_280/mycobot_280jn/scripts/slider_control.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python2
-
+# encoding:utf-8
 """[summary]
 This file obtains the joint angle of the manipulator in ROS,
 and then sends it directly to the real manipulator using `pymycobot` API.
@@ -13,9 +13,10 @@ import rospy
 from sensor_msgs.msg import JointState
 
 from pymycobot.mycobot import MyCobot
+from pymycobot.mycobotsocket import MyCobotSocket
 
 
-mc = None
+ms = None
 
 
 def callback(data):
@@ -25,19 +26,24 @@ def callback(data):
     for index, value in enumerate(data.position):
         data_list.append(value)
 
-    mc.send_radians(data_list, 80)
+    ms.send_radians(data_list, 80)
     # time.sleep(0.5)
 
 
 def listener():
-    global mc
+    global ms
     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)
+    # 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(serialport="/dev/ttyTHS1", baudrate="1000000")
 
     # 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
old mode 100644
new mode 100755
diff --git a/mycobot_280/mycobot_280jn/scripts/test.py b/mycobot_280/mycobot_280jn/scripts/test.py
new file mode 100755
index 0000000..fce6c08
--- /dev/null
+++ b/mycobot_280/mycobot_280jn/scripts/test.py
@@ -0,0 +1,12 @@
+import time
+from pymycobot.mycobotsocket import MyCobotSocket
+# from pymycobot.mycobot import MyCobot
+ms=MyCobotSocket('192.168.125.226',9000)
+
+ms.connect(serialport="/dev/ttyTHS1", baudrate="1000000")
+
+ms.send_angles([50,0,0,0,0,0],50)
+time.sleep(2)
+
+print(ms.get_angles())
+ms.release_all_servos()
\ No newline at end of file
diff --git a/mycobot_communication/launch/communication_service.launch b/mycobot_communication/launch/communication_service.launch
index 0117bd7..9a5b748 100644
--- a/mycobot_communication/launch/communication_service.launch
+++ b/mycobot_communication/launch/communication_service.launch
@@ -1,11 +1,11 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="port" default="/dev/ttyUSB0" />
-	<arg name="baud" default="115200" />
+	<arg name="ip" default="192.168.125.226" />
+	<arg name="port" default="9000" />
 
 	<!-- Open communication service ，开启通讯服务-->
 	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_services.py" output="screen">
-		<param name="port" type="string" value="$(arg port)" />
-		<param name="baud" type="int" value="$(arg baud)" />
+		<param name="ip" type="string" value="$(arg ip)" />
+		<param name="port" type="int" value="$(arg port)" />
     </node>
 </launch>
diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py
index 2b97386..c0427d2 100755
--- a/mycobot_communication/scripts/mycobot_services.py
+++ b/mycobot_communication/scripts/mycobot_services.py
@@ -5,18 +5,25 @@ import rospy
 from mycobot_communication.srv import *
 
 from pymycobot.mycobot import MyCobot
+from pymycobot.mycobotsocket import MyCobotSocket
 
-mc = None
-
+# mc = None
+ms = None
 
 def create_handle():
-    global mc
+    # global mc
+    global ms
     rospy.init_node("mycobot_services")
     rospy.loginfo("start ...")
-    port = rospy.get_param("~port")
-    baud = rospy.get_param("~baud")
-    rospy.loginfo("%s,%s" % (port, baud))
-    mc = MyCobot(port, baud)
+    # port = rospy.get_param("~port")
+    # baud = rospy.get_param("~baud")
+    # rospy.loginfo("%s,%s" % (port, baud))
+    # ms = MyCobot(port, baud)
+    ip=rospy.get_param("~ip")
+    port=rospy.get_param("~port")
+    rospy.loginfo("%s,%s" % (ip,port))
+    ms=MyCobotSocket(ip,port)
+    ms.connect(serialport="/dev/ttyTHS1", baudrate="1000000")
 
 
 def create_services():
@@ -42,16 +49,16 @@ def set_angles(req):
     ]
     sp = req.speed
 
-    if mc:
-        mc.send_angles(angles, sp)
+    if ms:
+        ms.send_angles(angles, sp)
 
     return SetAnglesResponse(True)
 
 
 def get_angles(req):
     """get angles,获取角度"""
-    if mc:
-        angles = mc.get_angles()
+    if ms:
+        angles = ms.get_angles()
         return GetAnglesResponse(*angles)
 
 
@@ -67,38 +74,38 @@ def set_coords(req):
     sp = req.speed
     mod = req.model
 
-    if mc:
-        mc.send_coords(coords, sp, mod)
+    if ms:
+        ms.send_coords(coords, sp, mod)
 
     return SetCoordsResponse(True)
 
 
 def get_coords(req):
-    if mc:
-        coords = mc.get_coords()
+    if ms:
+        coords = ms.get_coords()
         return GetCoordsResponse(*coords)
 
 
 def switch_status(req):
     """Gripper switch status"""
     """夹爪开关状态"""
-    if mc:
+    if ms:
         if req.Status:
-            mc.set_gripper_state(0, 80)
+            ms.set_gripper_state(0, 80)
         else:
-            mc.set_gripper_state(1, 80)
+            ms.set_gripper_state(1, 80)
 
     return GripperStatusResponse(True)
 
 
 def toggle_pump(req):
-    if mc:
+    if ms:
         if req.Status:
-            mc.set_basic_output(req.Pin1, 0)
-            mc.set_basic_output(req.Pin2, 0)
+            ms.set_basic_output(req.Pin1, 0)
+            ms.set_basic_output(req.Pin2, 0)
         else:
-            mc.set_basic_output(req.Pin1, 1)
-            mc.set_basic_output(req.Pin2, 1)
+            ms.set_basic_output(req.Pin1, 1)
+            ms.set_basic_output(req.Pin2, 1)
 
     return PumpStatusResponse(True)
 
@@ -130,12 +137,12 @@ def output_robot_message():
     servo_temperature = "unknown"
     atom_version = "unknown"
 
-    if mc:
-        cn = mc.is_controller_connected()
+    if ms:
+        cn = ms.is_controller_connected()
         if cn == 1:
             connect_status = True
         time.sleep(0.1)
-        si = mc.is_all_servo_enable()
+        si = ms.is_all_servo_enable()
         if si == 1:
             servo_infomation = "all connected"