280 Arduino repaired and optimized ros1 keyboard control movement effect, and changed to topic communication

mycobot_280/mycobot_280/scripts/teleop_keyboard.py

@@ -49,7 +49,7 @@ Other:
 COORD_LIMITS = {
     'x': (-281.45, 281.45),
     'y': (-281.45, 281.45),
-    'z': (-70, 412.67),
+    'z': (-70, 420.67),
     'rx': (-180, 180),
     'ry': (-180, 180),
     'rz': (-180, 180)

mycobot_280/mycobot_280arduino/launch/simple_gui.launch

@@ -1,6 +1,6 @@
 <launch>
 	<arg name="port" default="/dev/ttyACM0" />
-	<arg name="baud" default="1000000" />
+	<arg name="baud" default="115200" />
 
 	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_arduino/mycobot_280_arduino.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280arduino)/config/mycobot.rviz" />

mycobot_280/mycobot_280arduino/launch/teleop_keyboard.launch

@@ -1,21 +1,23 @@
 <launch>
-	<arg name="port" default="/dev/ttyACM0" />
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
-	<arg name="baud" default="1000000" />
+	<arg name="port" default="/dev/ttACM0" />
+	<arg name="baud" default="115200" />
 
 	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_arduino/mycobot_280_arduino.urdf"/>
-	<arg name="rvizconfig" default="$(find mycobot_280arduino)/config/mycobot_arduino.rviz" />
+	<arg name="rvizconfig" default="$(find mycobot_280arduino)/config/mycobot.rviz" />
 	<arg name="gui" default="false" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
 
-	<!-- Combinejoin values to TF -->
+	<!-- Combinejoin values to TF ，将值合并到TF-->
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
-	<!-- Show in Rviz -->
+	<!-- Show in Rviz ，显示在Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 
-    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+    <include file="$(find mycobot_communication)/launch/communication_topic.launch">
 		<arg name="port" value="$(arg port)" />	
 		<arg name="baud" value="$(arg baud)" />	
     </include>
-	<node name="real_listener" pkg="mycobot_280arduino" type="listen_real.py" />
+	<!-- listen and pub the real angles ，监听并发布真实角度-->
+	<node name="real_listener" pkg="mycobot_280arduino" type="listen_real_of_topic.py" />
 </launch>

mycobot_280/mycobot_280arduino/scripts/slider_control.py

@@ -45,7 +45,7 @@ def listener():
     rospy.init_node("control_slider", anonymous=True)
 
     port = rospy.get_param("~port", "/dev/ttyACM0")
-    baud = rospy.get_param("~baud", 1000000)
+    baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot280(port, baud)
     time.sleep(1) # open port,need wait

mycobot_280/mycobot_280arduino/scripts/teleop_keyboard.py

@@ -1,45 +1,63 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
 from __future__ import print_function
-from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
-import rospy
 import sys
-import select
+import time
 import termios
 import tty
-import time
+import math
-
+import rospy
-import roslib
 
+from mycobot_communication.msg import (
+    MycobotAngles,
+    MycobotCoords,
+    MycobotSetAngles,
+    MycobotSetCoords,
+    MycobotGripperStatus,
+    MycobotPumpStatus,
+)
 
 msg = """\
-Mycobot Teleop Keyboard Controller
+Mycobot Teleop Keyboard Controller (ROS1 - Topic Version)
----------------------------
+---------------------------------------------------------
-Movimg options(control coordinations [x,y,z,rx,ry,rz]):
+Movement (Cartesian):
-              w(x+)
+              w (x+)
+    a (y+)    s (x-)    d (y-)
+              z (z-)    x (z+)
 
-    a(y-)     s(x-)     d(y+)
+Rotation (Euler angles):
+    u (rx+)  i (ry+)  o (rz+)
+    j (rx-)  k (ry-)  l (rz-)
 
-    z(z-) x(z+)
+Movement Step:
+    + : Increase movement step size
+    - : Decrease movement step size
 
-u(rx+)   i(ry+)   o(rz+)
+Gripper:
-j(rx-)   k(ry-)   l(rz-)
+    g - open    h - close
 
-Gripper control:
+Pump:
-    g - open
+    b - open    m - close
-    h - close
 
 Other:
     1 - Go to init pose
     2 - Go to home pose
-    3 - Resave home pose
+    3 - Save current pose as home
     q - Quit
 """
 
+COORD_LIMITS = {
+    'x': (-281.45, 281.45),
+    'y': (-281.45, 281.45),
+    'z': (-70, 420),
+    'rx': (-180, 180),
+    'ry': (-180, 180),
+    'rz': (-180, 180)
+}
 
 def vels(speed, turn):
     return "currently:\tspeed: %s\tchange percent: %s  " % (speed, turn)
 
-
 class Raw(object):
     def __init__(self, stream):
         self.stream = stream
@@ -53,122 +71,139 @@ class Raw(object):
         termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
 
 
-def teleop_keyboard():
+class MycobotTeleopTopic:
-    rospy.init_node("teleop_keyboard")
+    def __init__(self):
+        rospy.init_node("teleop_keyboard_topic")
 
-    model = 0
+        self.speed = rospy.get_param("~speed", 50)
-    speed = rospy.get_param("~speed", 50)
+        self.model = 1
-    change_percent = rospy.get_param("~change_percent", 5)
+        self.change_percent = rospy.get_param("~change_percent", 5)
+        self.change_len = 250 * self.change_percent / 100.0
+        self.change_angle = 180 * self.change_percent / 100.0
 
-    change_angle = 180 * change_percent / 100
+        # 当前坐标和角度
-    change_len = 250 * change_percent / 100
+        self.curr_coords = [0] * 6
+        self.curr_angles = [0] * 6
 
-    rospy.wait_for_service("get_joint_angles")
+        # 保存坐标作为目标值
-    rospy.wait_for_service("set_joint_angles")
+        self.record_coords = None
-    rospy.wait_for_service("get_joint_coords")
+        self.home_pose = [0, 8, -127, 40, 0, 0]
-    rospy.wait_for_service("set_joint_coords")
-    rospy.wait_for_service("switch_gripper_status")
-    print("service ready.")
-    try:
-        get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
-        set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
-        get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
-        set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
-        switch_gripper = rospy.ServiceProxy(
-            "switch_gripper_status", GripperStatus)
-    except:
-        print("start error ...")
-        exit(1)
 
-    init_pose = [0, 0, 0, 0, 0, 0, speed]
+        # 订阅器
-    home_pose = [0, 8, -127, 40, 0, 0, speed]
+        rospy.Subscriber("mycobot/coords_real", MycobotCoords, self.coords_callback)
+        rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.angles_callback)
 
-    # rsp = set_angles(*init_pose)
+        # 发布器
-    while True:
+        self.coords_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=1)
-        res = get_coords()
+        self.angles_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=1)
-        time.sleep(1)
+        self.gripper_pub = rospy.Publisher("mycobot/gripper_status", MycobotGripperStatus, queue_size=1)
-        print('res:', res)
+        self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=1)
-        if res.x > 1:
-            break
-        time.sleep(0.1)
 
-    record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
+        rospy.loginfo("Waiting to receive current coordinates...")
-    print(record_coords)
+        while self.curr_coords == [0] * 6 and not rospy.is_shutdown():
+            time.sleep(0.1)
 
-    try:
+        self.record_coords = list(self.curr_coords)
         print(msg)
-        print(vels(speed, change_percent))
+        print(vels(self.speed, self.change_percent))
-        while 1:
+        rospy.loginfo("Current moving step: position %.1f mm, angle attitude %.1f°", self.change_len, self.change_angle)
+
+    def coords_callback(self, msg):
+        self.curr_coords = [msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz]
+
+    def angles_callback(self, msg):
+        self.curr_angles = [
+            msg.joint_1, msg.joint_2, msg.joint_3,
+            msg.joint_4, msg.joint_5, msg.joint_6
+        ]
+
+    def send_coords(self):
+        goal = MycobotSetCoords()
+        goal.x, goal.y, goal.z, goal.rx, goal.ry, goal.rz = self.record_coords
+        goal.speed = self.speed
+        goal.model = self.model
+        self.coords_pub.publish(goal)
+
+    def send_angles(self, angles):
+        goal = MycobotSetAngles()
+        goal.joint_1, goal.joint_2, goal.joint_3, goal.joint_4, goal.joint_5, goal.joint_6 = angles
+        goal.speed = self.speed
+        self.angles_pub.publish(goal)
+
+    def run(self):
+        while not rospy.is_shutdown():
             try:
-                # print("\r current coords: %s" % record_coords, end="")
                 with Raw(sys.stdin):
                     key = sys.stdin.read(1)
-                if key == "q":
+            except Exception:
+                continue
+
+            try:
+                if key == 'q':
                     break
-                elif key in ["w", "W"]:
+                elif key == '+':
-                    record_coords[0] += change_len
+                    self.change_percent = min(self.change_percent + 1, 20)  # 最大20%
-                    set_coords(*record_coords)
+                    self.change_angle = 180 * self.change_percent / 100.0
-                elif key in ["s", "S"]:
+                    self.change_len = 250 * self.change_percent / 100.0
-                    record_coords[0] -= change_len
+                    rospy.loginfo("Increase change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
-                    set_coords(*record_coords)
+                        (self.change_percent, self.change_len, self.change_angle))
-                elif key in ["a", "A"]:
+                elif key == '-':
-                    record_coords[1] -= change_len
+                    self.change_percent = max(self.change_percent - 1, 1)  # 最小1%
-                    set_coords(*record_coords)
+                    self.change_angle = 180 * self.change_percent / 100.0
-                elif key in ["d", "D"]:
+                    self.change_len = 250 * self.change_percent / 100.0
-                    record_coords[1] += change_len
+                    rospy.loginfo("Decrease change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
-                    set_coords(*record_coords)
+                        (self.change_percent, self.change_len, self.change_angle))
-                elif key in ["z", "Z"]:
+                    continue
-                    record_coords[2] -= change_len
+                elif key in 'wW': self.record_coords[0] += self.change_len
-                    set_coords(*record_coords)
+                elif key in 'sS': self.record_coords[0] -= self.change_len
-                elif key in ["x", "X"]:
+                elif key in 'aA': self.record_coords[1] += self.change_len
-                    record_coords[2] += change_len
+                elif key in 'dD': self.record_coords[1] -= self.change_len
-                    set_coords(*record_coords)
+                elif key in 'zZ': self.record_coords[2] -= self.change_len
-                elif key in ["u", "U"]:
+                elif key in 'xX': self.record_coords[2] += self.change_len
-                    record_coords[3] += change_angle
+                elif key in 'uU': self.record_coords[3] += self.change_angle
-                    set_coords(*record_coords)
+                elif key in 'jJ': self.record_coords[3] -= self.change_angle
-                elif key in ["j", "J"]:
+                elif key in 'iI': self.record_coords[4] += self.change_angle
-                    record_coords[3] -= change_angle
+                elif key in 'kK': self.record_coords[4] -= self.change_angle
-                    set_coords(*record_coords)
+                elif key in 'oO': self.record_coords[5] += self.change_angle
-                elif key in ["i", "I"]:
+                elif key in 'lL': self.record_coords[5] -= self.change_angle
-                    record_coords[4] += change_angle
+                elif key in 'gG':
-                    set_coords(*record_coords)
+                    self.gripper_pub.publish(MycobotGripperStatus(Status=True))
-                elif key in ["k", "K"]:
+                elif key in 'hH':
-                    record_coords[4] -= change_angle
+                    self.gripper_pub.publish(MycobotGripperStatus(Status=False))
-                    set_coords(*record_coords)
+                elif key in 'bB':
-                elif key in ["o", "O"]:
+                    self.pump_pub.publish(MycobotPumpStatus(Status=True, Pin1=2, Pin2=5))
-                    record_coords[5] += change_angle
+                elif key in 'mM':
-                    set_coords(*record_coords)
+                    self.pump_pub.publish(MycobotPumpStatus(Status=False, Pin1=2, Pin2=5))
-                elif key in ["l", "L"]:
+                elif key == '1':
-                    record_coords[5] -= change_angle
+                    self.send_angles([0, 0, 0, 0, 0, 0])
-                    set_coords(*record_coords)
+                    time.sleep(2)
-                elif key in ["g", "G"]:
+                    self.record_coords = list(self.curr_coords)
-                    switch_gripper(True)
+                elif key == '2':
-                elif key in ["h", "H"]:
+                    self.send_angles(self.home_pose)
-                    switch_gripper(False)
+                    time.sleep(2)
-                elif key == "1":
+                    self.record_coords = list(self.curr_coords)
-                    rsp = set_angles(*init_pose)
+                elif key == '3':
-                elif key in "2":
+                    self.home_pose = list(self.curr_angles)
-                    rsp = set_angles(*home_pose)
-                elif key in "3":
-                    rep = get_angles()
-                    home_pose[0] = rep.joint_1
-                    home_pose[1] = rep.joint_2
-                    home_pose[2] = rep.joint_3
-                    home_pose[3] = rep.joint_4
-                    home_pose[5] = rep.joint_5
                 else:
                     continue
 
+                # 范围检查
+                for val, axis in zip(self.record_coords, ['x', 'y', 'z', 'rx', 'ry', 'rz']):
+                    min_v, max_v = COORD_LIMITS[axis]
+                    if not (min_v <= val <= max_v):
+                        rospy.logwarn(f"{axis} Out of range: {val} not in [{min_v}, {max_v}]")
+                        raise ValueError("Out of range of motion")
+
+                self.send_coords()
+
             except Exception as e:
-                # print(e)
+                rospy.logwarn("Execution failed: {}".format(e))
                 continue
 
-    except Exception as e:
-        print(e)
 
-
+if __name__ == '__main__':
-if __name__ == "__main__":
     try:
-        teleop_keyboard()
+        teleop = MycobotTeleopTopic()
+        teleop.run()
     except rospy.ROSInterruptException:
         pass

mycobot_280/mycobot_280arduino/scripts/test.py

@@ -16,7 +16,7 @@ else:
 from pymycobot import *
 import time
 import datetime
-m = MyCobot280('/dev/ttyUSB0', 1000000)
+m = MyCobot280('/dev/ttyUSB0', 115200)
 time.sleep(2)
 delay_time = 0.1
 run_delay_time = 1