mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-03 02:27:04 +00:00
280 M5 Arduino PI、JetsonNano repaired and optimized ros1 keyboard control movement effect, and changed to topic communication
This commit is contained in:
wangWking
commit
f80d7bd698
17 changed files with 823 additions and 387 deletions
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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" />
|
||||
|
|
|
|||
|
|
@ -1,21 +1,23 @@
|
|||
<launch>
|
||||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<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>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 roslib
|
||||
import math
|
||||
import rospy
|
||||
|
||||
from mycobot_communication.msg import (
|
||||
MycobotAngles,
|
||||
MycobotCoords,
|
||||
MycobotSetAngles,
|
||||
MycobotSetCoords,
|
||||
MycobotGripperStatus,
|
||||
MycobotPumpStatus,
|
||||
)
|
||||
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
||||
w(x+)
|
||||
Mycobot Teleop Keyboard Controller (ROS1 - Topic Version)
|
||||
---------------------------------------------------------
|
||||
Movement (Cartesian):
|
||||
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+)
|
||||
j(rx-) k(ry-) l(rz-)
|
||||
Gripper:
|
||||
g - open h - close
|
||||
|
||||
Gripper control:
|
||||
g - open
|
||||
h - close
|
||||
Pump:
|
||||
b - open m - 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():
|
||||
rospy.init_node("teleop_keyboard")
|
||||
class MycobotTeleopTopic:
|
||||
def __init__(self):
|
||||
rospy.init_node("teleop_keyboard_topic")
|
||||
|
||||
model = 0
|
||||
speed = rospy.get_param("~speed", 50)
|
||||
change_percent = rospy.get_param("~change_percent", 5)
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
self.model = 1
|
||||
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")
|
||||
rospy.wait_for_service("get_joint_coords")
|
||||
rospy.wait_for_service("set_joint_coords")
|
||||
rospy.wait_for_service("switch_gripper_status")
|
||||
print("service ready.")
|
||||
try:
|
||||
get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
||||
set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
||||
get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
||||
switch_gripper = rospy.ServiceProxy(
|
||||
"switch_gripper_status", GripperStatus)
|
||||
except:
|
||||
print("start error ...")
|
||||
exit(1)
|
||||
# 保存坐标作为目标值
|
||||
self.record_coords = None
|
||||
self.home_pose = [0, 8, -127, 40, 0, 0]
|
||||
|
||||
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:
|
||||
res = get_coords()
|
||||
time.sleep(1)
|
||||
print('res:', res)
|
||||
if res.x > 1:
|
||||
break
|
||||
time.sleep(0.1)
|
||||
# 发布器
|
||||
self.coords_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=1)
|
||||
self.angles_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=1)
|
||||
self.gripper_pub = rospy.Publisher("mycobot/gripper_status", MycobotGripperStatus, queue_size=1)
|
||||
self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=1)
|
||||
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
print(record_coords)
|
||||
rospy.loginfo("Waiting to receive current coordinates...")
|
||||
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))
|
||||
while 1:
|
||||
print(vels(self.speed, self.change_percent))
|
||||
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"]:
|
||||
record_coords[0] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["s", "S"]:
|
||||
record_coords[0] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["a", "A"]:
|
||||
record_coords[1] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["d", "D"]:
|
||||
record_coords[1] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["z", "Z"]:
|
||||
record_coords[2] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["x", "X"]:
|
||||
record_coords[2] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["u", "U"]:
|
||||
record_coords[3] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["j", "J"]:
|
||||
record_coords[3] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["i", "I"]:
|
||||
record_coords[4] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["k", "K"]:
|
||||
record_coords[4] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["o", "O"]:
|
||||
record_coords[5] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["l", "L"]:
|
||||
record_coords[5] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["g", "G"]:
|
||||
switch_gripper(True)
|
||||
elif key in ["h", "H"]:
|
||||
switch_gripper(False)
|
||||
elif key == "1":
|
||||
rsp = set_angles(*init_pose)
|
||||
elif key in "2":
|
||||
rsp = set_angles(*home_pose)
|
||||
elif key in "3":
|
||||
rep = get_angles()
|
||||
home_pose[0] = rep.joint_1
|
||||
home_pose[1] = rep.joint_2
|
||||
home_pose[2] = rep.joint_3
|
||||
home_pose[3] = rep.joint_4
|
||||
home_pose[5] = rep.joint_5
|
||||
elif key == '+':
|
||||
self.change_percent = min(self.change_percent + 1, 20) # 最大20%
|
||||
self.change_angle = 180 * self.change_percent / 100.0
|
||||
self.change_len = 250 * self.change_percent / 100.0
|
||||
rospy.loginfo("Increase change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
|
||||
(self.change_percent, self.change_len, self.change_angle))
|
||||
elif key == '-':
|
||||
self.change_percent = max(self.change_percent - 1, 1) # 最小1%
|
||||
self.change_angle = 180 * self.change_percent / 100.0
|
||||
self.change_len = 250 * self.change_percent / 100.0
|
||||
rospy.loginfo("Decrease change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
|
||||
(self.change_percent, self.change_len, self.change_angle))
|
||||
continue
|
||||
elif key in 'wW': self.record_coords[0] += self.change_len
|
||||
elif key in 'sS': self.record_coords[0] -= self.change_len
|
||||
elif key in 'aA': self.record_coords[1] += self.change_len
|
||||
elif key in 'dD': self.record_coords[1] -= self.change_len
|
||||
elif key in 'zZ': self.record_coords[2] -= self.change_len
|
||||
elif key in 'xX': self.record_coords[2] += self.change_len
|
||||
elif key in 'uU': self.record_coords[3] += self.change_angle
|
||||
elif key in 'jJ': self.record_coords[3] -= self.change_angle
|
||||
elif key in 'iI': self.record_coords[4] += self.change_angle
|
||||
elif key in 'kK': self.record_coords[4] -= self.change_angle
|
||||
elif key in 'oO': self.record_coords[5] += self.change_angle
|
||||
elif key in 'lL': self.record_coords[5] -= self.change_angle
|
||||
elif key in 'gG':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=True))
|
||||
elif key in 'hH':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=False))
|
||||
elif key in 'bB':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=True, Pin1=2, Pin2=5))
|
||||
elif key in 'mM':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=False, Pin1=2, Pin2=5))
|
||||
elif key == '1':
|
||||
self.send_angles([0, 0, 0, 0, 0, 0])
|
||||
time.sleep(2)
|
||||
self.record_coords = list(self.curr_coords)
|
||||
elif key == '2':
|
||||
self.send_angles(self.home_pose)
|
||||
time.sleep(2)
|
||||
self.record_coords = list(self.curr_coords)
|
||||
elif key == '3':
|
||||
self.home_pose = list(self.curr_angles)
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/listen_real_of_topic_gripper.py
|
||||
scripts/slider_control_gripper.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<include file="$(find mycobot_communication)/launch/communication_jsnn.launch">
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic_jsnn.launch">
|
||||
<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" />
|
||||
<node name="real_listener" pkg="mycobot_280jn" type="listen_real_of_topic.py" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<include file="$(find mycobot_communication)/launch/communication_jsnn.launch">
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic_jsnn.launch">
|
||||
<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" />
|
||||
<node name="real_listener" pkg="mycobot_280jn" type="listen_real_of_topic_gripper.py" />
|
||||
</launch>
|
||||
|
|
|
|||
72
mycobot_280/mycobot_280jn/scripts/listen_real_of_topic_gripper.py
Executable file
72
mycobot_280/mycobot_280jn/scripts/listen_real_of_topic_gripper.py
Executable file
|
|
@ -0,0 +1,72 @@
|
|||
#!/usr/bin/env python3
|
||||
# encoding:utf-8
|
||||
|
||||
import math
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from mycobot_communication.msg import MycobotAngles, MycobotGetGripperValue
|
||||
|
||||
|
||||
class Listener(object):
|
||||
def __init__(self):
|
||||
super(Listener, self).__init__()
|
||||
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("real_listener_gripper", anonymous=True)
|
||||
self.gripper_angle = 0.0
|
||||
# init publisher.初始化发布者
|
||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
# init subscriber.初始化订阅者
|
||||
self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
|
||||
self.sub2 = rospy.Subscriber("mycobot/gripper_angle_real", MycobotGetGripperValue, self.gripper_callback)
|
||||
rospy.spin()
|
||||
|
||||
def gripper_callback(self, data):
|
||||
"""夹爪角度更新"""
|
||||
self.gripper_angle = data.gripper_angle
|
||||
|
||||
def callback(self, data):
|
||||
"""`mycobot/angles_real` subscriber callback method.
|
||||
|
||||
Args:
|
||||
data (MycobotAngles): callback argument.
|
||||
"""
|
||||
# ini publisher object. 初始化发布者对象
|
||||
joint_state_send = JointState()
|
||||
joint_state_send.header = Header()
|
||||
|
||||
joint_state_send.name = [
|
||||
"joint2_to_joint1",
|
||||
"joint3_to_joint2",
|
||||
"joint4_to_joint3",
|
||||
"joint5_to_joint4",
|
||||
"joint6_to_joint5",
|
||||
"joint6output_to_joint6",
|
||||
"gripper_controller",
|
||||
]
|
||||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
||||
# process callback data. 处理回调数据。
|
||||
radians_list = [
|
||||
data.joint_1 * (math.pi / 180),
|
||||
data.joint_2 * (math.pi / 180),
|
||||
data.joint_3 * (math.pi / 180),
|
||||
data.joint_4 * (math.pi / 180),
|
||||
data.joint_5 * (math.pi / 180),
|
||||
data.joint_6 * (math.pi / 180),
|
||||
self.gripper_angle * math.pi / 180.0,
|
||||
]
|
||||
# rospy.loginfo("res: {}".format(radians_list))
|
||||
|
||||
joint_state_send.position = radians_list
|
||||
self.pub.publish(joint_state_send)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
Listener()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -1,46 +1,63 @@
|
|||
#!/usr/bin/env python3
|
||||
# encoding=utf-8
|
||||
# -*- 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,
|
||||
)
|
||||
|
||||
# Terminal output prompt information. 终端输出提示信息
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
||||
w(x+)
|
||||
Mycobot Teleop Keyboard Controller (ROS1 - Topic Version)
|
||||
---------------------------------------------------------
|
||||
Movement (Cartesian):
|
||||
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+)
|
||||
j(rx-) k(ry-) l(rz-)
|
||||
Gripper:
|
||||
g - open h - close
|
||||
|
||||
Gripper control:
|
||||
g - open
|
||||
h - close
|
||||
Pump:
|
||||
b - open m - 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.67),
|
||||
'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
|
||||
|
|
@ -54,124 +71,139 @@ class Raw(object):
|
|||
termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
|
||||
|
||||
|
||||
def teleop_keyboard():
|
||||
rospy.init_node("teleop_keyboard")
|
||||
class MycobotTeleopTopic:
|
||||
def __init__(self):
|
||||
rospy.init_node("teleop_keyboard_topic")
|
||||
|
||||
model = 0
|
||||
speed = rospy.get_param("~speed", 50)
|
||||
change_percent = rospy.get_param("~change_percent", 5)
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
self.model = 1
|
||||
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")
|
||||
rospy.wait_for_service("get_joint_coords")
|
||||
rospy.wait_for_service("set_joint_coords")
|
||||
rospy.wait_for_service("switch_gripper_status")
|
||||
print("service ready.")
|
||||
try:
|
||||
get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
||||
set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
||||
get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
||||
switch_gripper = rospy.ServiceProxy(
|
||||
"switch_gripper_status", GripperStatus)
|
||||
except:
|
||||
print("start error ...")
|
||||
exit(1)
|
||||
# 保存坐标作为目标值
|
||||
self.record_coords = None
|
||||
self.home_pose = [0, 8, -127, 40, 0, 0]
|
||||
|
||||
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)
|
||||
# 发布器
|
||||
self.coords_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=1)
|
||||
self.angles_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=1)
|
||||
self.gripper_pub = rospy.Publisher("mycobot/gripper_status", MycobotGripperStatus, queue_size=1)
|
||||
self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=1)
|
||||
|
||||
while True:
|
||||
res = get_coords()
|
||||
if res.x > 1:
|
||||
break
|
||||
time.sleep(0.1)
|
||||
rospy.loginfo("Waiting to receive current coordinates...")
|
||||
while self.curr_coords == [0] * 6 and not rospy.is_shutdown():
|
||||
time.sleep(0.1)
|
||||
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
print(record_coords)
|
||||
|
||||
try:
|
||||
self.record_coords = list(self.curr_coords)
|
||||
print(msg)
|
||||
print(vels(speed, change_percent))
|
||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
||||
while 1:
|
||||
print(vels(self.speed, self.change_percent))
|
||||
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"]:
|
||||
record_coords[0] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["s", "S"]:
|
||||
record_coords[0] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["a", "A"]:
|
||||
record_coords[1] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["d", "D"]:
|
||||
record_coords[1] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["z", "Z"]:
|
||||
record_coords[2] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["x", "X"]:
|
||||
record_coords[2] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["u", "U"]:
|
||||
record_coords[3] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["j", "J"]:
|
||||
record_coords[3] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["i", "I"]:
|
||||
record_coords[4] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["k", "K"]:
|
||||
record_coords[4] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["o", "O"]:
|
||||
record_coords[5] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["l", "L"]:
|
||||
record_coords[5] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["g", "G"]:
|
||||
switch_gripper(True)
|
||||
elif key in ["h", "H"]:
|
||||
switch_gripper(False)
|
||||
elif key == "1":
|
||||
rsp = set_angles(*init_pose)
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
elif key in "2":
|
||||
rsp = set_angles(*home_pose)
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
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
|
||||
elif key == '+':
|
||||
self.change_percent = min(self.change_percent + 1, 20) # 最大20%
|
||||
self.change_angle = 180 * self.change_percent / 100.0
|
||||
self.change_len = 250 * self.change_percent / 100.0
|
||||
rospy.loginfo("Increase change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
|
||||
(self.change_percent, self.change_len, self.change_angle))
|
||||
elif key == '-':
|
||||
self.change_percent = max(self.change_percent - 1, 1) # 最小1%
|
||||
self.change_angle = 180 * self.change_percent / 100.0
|
||||
self.change_len = 250 * self.change_percent / 100.0
|
||||
rospy.loginfo("Decrease change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
|
||||
(self.change_percent, self.change_len, self.change_angle))
|
||||
continue
|
||||
elif key in 'wW': self.record_coords[0] += self.change_len
|
||||
elif key in 'sS': self.record_coords[0] -= self.change_len
|
||||
elif key in 'aA': self.record_coords[1] += self.change_len
|
||||
elif key in 'dD': self.record_coords[1] -= self.change_len
|
||||
elif key in 'zZ': self.record_coords[2] -= self.change_len
|
||||
elif key in 'xX': self.record_coords[2] += self.change_len
|
||||
elif key in 'uU': self.record_coords[3] += self.change_angle
|
||||
elif key in 'jJ': self.record_coords[3] -= self.change_angle
|
||||
elif key in 'iI': self.record_coords[4] += self.change_angle
|
||||
elif key in 'kK': self.record_coords[4] -= self.change_angle
|
||||
elif key in 'oO': self.record_coords[5] += self.change_angle
|
||||
elif key in 'lL': self.record_coords[5] -= self.change_angle
|
||||
elif key in 'gG':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=True))
|
||||
elif key in 'hH':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=False))
|
||||
elif key in 'bB':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=True, Pin1=2, Pin2=5))
|
||||
elif key in 'mM':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=False, Pin1=2, Pin2=5))
|
||||
elif key == '1':
|
||||
self.send_angles([0, 0, 0, 0, 0, 0])
|
||||
time.sleep(2)
|
||||
self.record_coords = list(self.curr_coords)
|
||||
elif key == '2':
|
||||
self.send_angles(self.home_pose)
|
||||
time.sleep(2)
|
||||
self.record_coords = list(self.curr_coords)
|
||||
elif key == '3':
|
||||
self.home_pose = list(self.curr_angles)
|
||||
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
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
<!-- 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_pi.launch">
|
||||
<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_280pi" type="listen_real.py" />
|
||||
<node name="real_listener" pkg="mycobot_280pi" type="listen_real_of_topic.py" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,46 +1,63 @@
|
|||
#!/usr/bin/env python3
|
||||
# encoding=utf-8
|
||||
# -*- 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,
|
||||
)
|
||||
|
||||
# Terminal output prompt information. 终端输出提示信息
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
||||
w(x+)
|
||||
Mycobot Teleop Keyboard Controller (ROS1 - Topic Version)
|
||||
---------------------------------------------------------
|
||||
Movement (Cartesian):
|
||||
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+)
|
||||
j(rx-) k(ry-) l(rz-)
|
||||
Gripper:
|
||||
g - open h - close
|
||||
|
||||
Gripper control:
|
||||
g - open
|
||||
h - close
|
||||
Pump:
|
||||
b - open m - 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.67),
|
||||
'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
|
||||
|
|
@ -54,124 +71,139 @@ class Raw(object):
|
|||
termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
|
||||
|
||||
|
||||
def teleop_keyboard():
|
||||
rospy.init_node("teleop_keyboard")
|
||||
class MycobotTeleopTopic:
|
||||
def __init__(self):
|
||||
rospy.init_node("teleop_keyboard_topic")
|
||||
|
||||
model = 0
|
||||
speed = rospy.get_param("~speed", 50)
|
||||
change_percent = rospy.get_param("~change_percent", 5)
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
self.model = 1
|
||||
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")
|
||||
rospy.wait_for_service("get_joint_coords")
|
||||
rospy.wait_for_service("set_joint_coords")
|
||||
rospy.wait_for_service("switch_gripper_status")
|
||||
print("service ready.")
|
||||
try:
|
||||
get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
||||
set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
||||
get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
||||
switch_gripper = rospy.ServiceProxy(
|
||||
"switch_gripper_status", GripperStatus)
|
||||
except:
|
||||
print("start error ...")
|
||||
exit(1)
|
||||
# 保存坐标作为目标值
|
||||
self.record_coords = None
|
||||
self.home_pose = [0, 8, -127, 40, 0, 0]
|
||||
|
||||
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)
|
||||
# 发布器
|
||||
self.coords_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=1)
|
||||
self.angles_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=1)
|
||||
self.gripper_pub = rospy.Publisher("mycobot/gripper_status", MycobotGripperStatus, queue_size=1)
|
||||
self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=1)
|
||||
|
||||
while True:
|
||||
res = get_coords()
|
||||
if res.x > 1:
|
||||
break
|
||||
time.sleep(0.1)
|
||||
rospy.loginfo("Waiting to receive current coordinates...")
|
||||
while self.curr_coords == [0] * 6 and not rospy.is_shutdown():
|
||||
time.sleep(0.1)
|
||||
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
print(record_coords)
|
||||
|
||||
try:
|
||||
self.record_coords = list(self.curr_coords)
|
||||
print(msg)
|
||||
print(vels(speed, change_percent))
|
||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
||||
while 1:
|
||||
print(vels(self.speed, self.change_percent))
|
||||
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"]:
|
||||
record_coords[0] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["s", "S"]:
|
||||
record_coords[0] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["a", "A"]:
|
||||
record_coords[1] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["d", "D"]:
|
||||
record_coords[1] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["z", "Z"]:
|
||||
record_coords[2] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["x", "X"]:
|
||||
record_coords[2] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ["u", "U"]:
|
||||
record_coords[3] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["j", "J"]:
|
||||
record_coords[3] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["i", "I"]:
|
||||
record_coords[4] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["k", "K"]:
|
||||
record_coords[4] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["o", "O"]:
|
||||
record_coords[5] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["l", "L"]:
|
||||
record_coords[5] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ["g", "G"]:
|
||||
switch_gripper(True)
|
||||
elif key in ["h", "H"]:
|
||||
switch_gripper(False)
|
||||
elif key == "1":
|
||||
rsp = set_angles(*init_pose)
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
elif key in "2":
|
||||
rsp = set_angles(*home_pose)
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
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
|
||||
elif key == '+':
|
||||
self.change_percent = min(self.change_percent + 1, 20) # 最大20%
|
||||
self.change_angle = 180 * self.change_percent / 100.0
|
||||
self.change_len = 250 * self.change_percent / 100.0
|
||||
rospy.loginfo("Increase change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
|
||||
(self.change_percent, self.change_len, self.change_angle))
|
||||
elif key == '-':
|
||||
self.change_percent = max(self.change_percent - 1, 1) # 最小1%
|
||||
self.change_angle = 180 * self.change_percent / 100.0
|
||||
self.change_len = 250 * self.change_percent / 100.0
|
||||
rospy.loginfo("Decrease change_percent to %d%%, move step: %.1f mm, angle step: %.1f°" %
|
||||
(self.change_percent, self.change_len, self.change_angle))
|
||||
continue
|
||||
elif key in 'wW': self.record_coords[0] += self.change_len
|
||||
elif key in 'sS': self.record_coords[0] -= self.change_len
|
||||
elif key in 'aA': self.record_coords[1] += self.change_len
|
||||
elif key in 'dD': self.record_coords[1] -= self.change_len
|
||||
elif key in 'zZ': self.record_coords[2] -= self.change_len
|
||||
elif key in 'xX': self.record_coords[2] += self.change_len
|
||||
elif key in 'uU': self.record_coords[3] += self.change_angle
|
||||
elif key in 'jJ': self.record_coords[3] -= self.change_angle
|
||||
elif key in 'iI': self.record_coords[4] += self.change_angle
|
||||
elif key in 'kK': self.record_coords[4] -= self.change_angle
|
||||
elif key in 'oO': self.record_coords[5] += self.change_angle
|
||||
elif key in 'lL': self.record_coords[5] -= self.change_angle
|
||||
elif key in 'gG':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=True))
|
||||
elif key in 'hH':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=False))
|
||||
elif key in 'bB':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=True, Pin1=2, Pin2=5))
|
||||
elif key in 'mM':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=False, Pin1=2, Pin2=5))
|
||||
elif key == '1':
|
||||
self.send_angles([0, 0, 0, 0, 0, 0])
|
||||
time.sleep(2)
|
||||
self.record_coords = list(self.curr_coords)
|
||||
elif key == '2':
|
||||
self.send_angles(self.home_pose)
|
||||
time.sleep(2)
|
||||
self.record_coords = list(self.curr_coords)
|
||||
elif key == '3':
|
||||
self.home_pose = list(self.curr_angles)
|
||||
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
|
||||
|
|
|
|||
11
mycobot_communication/launch/communication_topic_jsnn.launch
Normal file
11
mycobot_communication/launch/communication_topic_jsnn.launch
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyTHS1" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Open communication service --><!-- 开启通讯服务 -->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_jsnn.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -177,7 +177,6 @@ class MycobotTopics(object):
|
|||
with self.lock:
|
||||
try:
|
||||
angles = self.mc.get_angles()
|
||||
# rospy.loginfo("angles:{}".format(angles))
|
||||
if isinstance(angles, list) and len(angles) == 6 and all(c != -1 for c in angles):
|
||||
ma.joint_1 = angles[0]
|
||||
ma.joint_2 = angles[1]
|
||||
|
|
|
|||
|
|
@ -15,6 +15,10 @@ from mycobot_communication.msg import (
|
|||
MycobotSetCoords,
|
||||
MycobotGripperStatus,
|
||||
MycobotPumpStatus,
|
||||
MycobotSetEndType,
|
||||
MycobotSetFreshMode,
|
||||
MycobotSetToolReference,
|
||||
MycobotGetGripperValue,
|
||||
)
|
||||
|
||||
import pymycobot
|
||||
|
|
@ -28,7 +32,7 @@ if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
|||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
# from pymycobot import MyCobot280
|
||||
from pymycobot import MyCobot280
|
||||
|
||||
from pymycobot import MyCobot280Socket # pi
|
||||
|
||||
|
|
@ -76,6 +80,25 @@ class Watcher:
|
|||
os.kill(self.child, signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
robot_msg = """
|
||||
MyCobot Status
|
||||
--------------------------------
|
||||
Joint Limit:
|
||||
joint 1: -168 ~ +168
|
||||
joint 2: -135 ~ +135
|
||||
joint 3: -150 ~ +150
|
||||
joint 4: -145 ~ +145
|
||||
joint 5: -165 ~ +165
|
||||
joint 6: -180 ~ +180
|
||||
|
||||
Connect Status: %s
|
||||
|
||||
Servo Infomation: %s
|
||||
|
||||
Servo Temperature: %s
|
||||
|
||||
Atom Version: %s
|
||||
"""
|
||||
|
||||
|
||||
class MycobotTopics(object):
|
||||
|
|
@ -88,9 +111,9 @@ class MycobotTopics(object):
|
|||
port = rospy.get_param("~port", "/dev/ttyAMA0")
|
||||
baud = rospy.get_param("~baud", 1000000)
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
# self.mc = MyCobot280(port,baud)
|
||||
self.mc = MyCobot280Socket(port, baud) # port
|
||||
self.mc.connect()
|
||||
self.mc = MyCobot280(port,baud)
|
||||
# self.mc = MyCobot280Socket(port, baud) # port
|
||||
# self.mc.connect()
|
||||
|
||||
|
||||
self.lock = threading.Lock()
|
||||
|
|
@ -102,6 +125,11 @@ class MycobotTopics(object):
|
|||
sb = threading.Thread(target=self.sub_set_coords)
|
||||
sg = threading.Thread(target=self.sub_gripper_status)
|
||||
sp = threading.Thread(target=self.sub_pump_status)
|
||||
|
||||
sfm = threading.Thread(target=self.sub_fresh_mode_status)
|
||||
set = threading.Thread(target=self.sub_end_type_status)
|
||||
str = threading.Thread(target=self.sub_set_tool_reference)
|
||||
sgv = threading.Thread(target=self.sub_real_gripper_value)
|
||||
|
||||
pa.setDaemon(True)
|
||||
pa.start()
|
||||
|
|
@ -115,6 +143,15 @@ class MycobotTopics(object):
|
|||
sg.start()
|
||||
sp.setDaemon(True)
|
||||
sp.start()
|
||||
|
||||
sfm.setDaemon(True)
|
||||
sfm.start
|
||||
set.setDaemon(True)
|
||||
set.start()
|
||||
str.setDaemon(True)
|
||||
str.start()
|
||||
sgv.setDaemon(True)
|
||||
sgv.start()
|
||||
|
||||
pa.join()
|
||||
pb.join()
|
||||
|
|
@ -122,6 +159,11 @@ class MycobotTopics(object):
|
|||
sb.join()
|
||||
sg.join()
|
||||
sp.join()
|
||||
|
||||
sfm.join()
|
||||
set.join()
|
||||
str.join()
|
||||
sgv.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
|
|
@ -132,7 +174,7 @@ class MycobotTopics(object):
|
|||
self.lock.acquire()
|
||||
angles = self.mc.get_angles()
|
||||
self.lock.release()
|
||||
if angles:
|
||||
if isinstance(angles, list) and len(angles) == 6 and all(c != -1 for c in angles):
|
||||
ma.joint_1 = angles[0]
|
||||
ma.joint_2 = angles[1]
|
||||
ma.joint_3 = angles[2]
|
||||
|
|
@ -152,7 +194,7 @@ class MycobotTopics(object):
|
|||
self.lock.acquire()
|
||||
coords = self.mc.get_coords()
|
||||
self.lock.release()
|
||||
if coords:
|
||||
if isinstance(coords, list) and len(coords) == 6 and all(c != -1 for c in coords):
|
||||
ma.x = coords[0]
|
||||
ma.y = coords[1]
|
||||
ma.z = coords[2]
|
||||
|
|
@ -196,6 +238,22 @@ class MycobotTopics(object):
|
|||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_real_gripper_value(self):
|
||||
"""Get Gripper Value"""
|
||||
pub = rospy.Publisher("mycobot/gripper_angle_real",
|
||||
MycobotGetGripperValue, queue_size=5)
|
||||
ma = MycobotGetGripperValue()
|
||||
while not rospy.is_shutdown():
|
||||
with self.lock:
|
||||
try:
|
||||
gripper_value = self.mc.get_gripper_value()
|
||||
if gripper_value:
|
||||
ma.gripper_angle = gripper_value
|
||||
pub.publish(ma)
|
||||
except Exception as e:
|
||||
rospy.logerr(f"SerialException: {e}")
|
||||
time.sleep(0.25)
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
|
|
@ -211,19 +269,87 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_pump_status(self):
|
||||
self.mc.gpio_init()
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_basic_output(data.Pin1, 0)
|
||||
self.mc.gpio_output(data.Pin1, 0)
|
||||
time.sleep(0.05)
|
||||
self.mc.set_basic_output(data.Pin2, 0)
|
||||
else:
|
||||
self.mc.set_basic_output(data.Pin1, 1)
|
||||
self.mc.set_basic_output(data.Pin2, 1)
|
||||
self.mc.gpio_output(data.Pin1, 1)
|
||||
time.sleep(0.05)
|
||||
self.mc.gpio_output(data.Pin2, 0)
|
||||
time.sleep(0.05)
|
||||
self.mc.gpio_output(data.Pin2, 1)
|
||||
time.sleep(0.05)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/pump_status", MycobotPumpStatus, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_fresh_mode_status(self):
|
||||
"""Subscribe to fresh mode Status"""
|
||||
"""订阅运动模式状态"""
|
||||
def callback(data):
|
||||
if data.Status==1:
|
||||
self.mc.set_fresh_mode(1)
|
||||
else:
|
||||
self.mc.set_fresh_mode(0)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/fresh_mode_status", MycobotSetFreshMode, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_end_type_status(self):
|
||||
"""Subscribe to end type Status"""
|
||||
"""订阅末端类型状态"""
|
||||
def callback(data):
|
||||
if data.Status==1:
|
||||
self.mc.set_end_type(1)
|
||||
else:
|
||||
self.mc.set_end_type(0)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/end_type_status", MycobotSetEndType, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_set_tool_reference(self):
|
||||
def callback(data):
|
||||
coords = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
|
||||
self.mc.set_tool_reference(coords)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/tool_reference_goal", MycobotSetToolReference, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def output_robot_message(self):
|
||||
connect_status = False
|
||||
servo_infomation = "unknown"
|
||||
servo_temperature = "unknown"
|
||||
atom_version = "unknown"
|
||||
|
||||
if self.mc:
|
||||
cn = self.mc.is_controller_connected()
|
||||
if cn == 1:
|
||||
connect_status = True
|
||||
time.sleep(0.1)
|
||||
si = self.mc.is_all_servo_enable()
|
||||
if si == 1:
|
||||
servo_infomation = "all connected"
|
||||
version = self.mc.get_system_version()
|
||||
if version:
|
||||
atom_version = version
|
||||
|
||||
print(
|
||||
robot_msg % (connect_status, servo_infomation,
|
||||
servo_temperature, atom_version)
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
Watcher()
|
||||
mc_topics = MycobotTopics()
|
||||
|
|
|
|||
|
|
@ -15,6 +15,10 @@ from mycobot_communication.msg import (
|
|||
MycobotSetCoords,
|
||||
MycobotGripperStatus,
|
||||
MycobotPumpStatus,
|
||||
MycobotSetEndType,
|
||||
MycobotSetFreshMode,
|
||||
MycobotSetToolReference,
|
||||
MycobotGetGripperValue,
|
||||
)
|
||||
|
||||
import pymycobot
|
||||
|
|
@ -75,7 +79,25 @@ class Watcher:
|
|||
os.kill(self.child, signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
robot_msg = """
|
||||
MyCobot Status
|
||||
--------------------------------
|
||||
Joint Limit:
|
||||
joint 1: -168 ~ +168
|
||||
joint 2: -135 ~ +135
|
||||
joint 3: -150 ~ +150
|
||||
joint 4: -145 ~ +145
|
||||
joint 5: -165 ~ +165
|
||||
joint 6: -180 ~ +180
|
||||
|
||||
Connect Status: %s
|
||||
|
||||
Servo Infomation: %s
|
||||
|
||||
Servo Temperature: %s
|
||||
|
||||
Atom Version: %s
|
||||
"""
|
||||
|
||||
class MycobotTopics(object):
|
||||
def __init__(self):
|
||||
|
|
@ -91,6 +113,7 @@ class MycobotTopics(object):
|
|||
# self.mc.connect() #pi
|
||||
self.mc = MyCobot280(port, baud)
|
||||
self.lock = threading.Lock()
|
||||
self.output_robot_message()
|
||||
|
||||
def start(self):
|
||||
pa = threading.Thread(target=self.pub_real_angles)
|
||||
|
|
@ -99,6 +122,11 @@ class MycobotTopics(object):
|
|||
sb = threading.Thread(target=self.sub_set_coords)
|
||||
sg = threading.Thread(target=self.sub_gripper_status)
|
||||
sp = threading.Thread(target=self.sub_pump_status)
|
||||
|
||||
sfm = threading.Thread(target=self.sub_fresh_mode_status)
|
||||
set = threading.Thread(target=self.sub_end_type_status)
|
||||
str = threading.Thread(target=self.sub_set_tool_reference)
|
||||
sgv = threading.Thread(target=self.sub_real_gripper_value)
|
||||
|
||||
pa.setDaemon(True)
|
||||
pa.start()
|
||||
|
|
@ -112,6 +140,15 @@ class MycobotTopics(object):
|
|||
sg.start()
|
||||
sp.setDaemon(True)
|
||||
sp.start()
|
||||
|
||||
sfm.setDaemon(True)
|
||||
sfm.start
|
||||
set.setDaemon(True)
|
||||
set.start()
|
||||
str.setDaemon(True)
|
||||
str.start()
|
||||
sgv.setDaemon(True)
|
||||
sgv.start()
|
||||
|
||||
pa.join()
|
||||
pb.join()
|
||||
|
|
@ -119,6 +156,11 @@ class MycobotTopics(object):
|
|||
sb.join()
|
||||
sg.join()
|
||||
sp.join()
|
||||
|
||||
sfm.join()
|
||||
set.join()
|
||||
str.join()
|
||||
sgv.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
|
|
@ -129,7 +171,7 @@ class MycobotTopics(object):
|
|||
self.lock.acquire()
|
||||
angles = self.mc.get_angles()
|
||||
self.lock.release()
|
||||
if angles:
|
||||
if isinstance(angles, list) and len(angles) == 6 and all(c != -1 for c in angles):
|
||||
ma.joint_1 = angles[0]
|
||||
ma.joint_2 = angles[1]
|
||||
ma.joint_3 = angles[2]
|
||||
|
|
@ -149,7 +191,7 @@ class MycobotTopics(object):
|
|||
self.lock.acquire()
|
||||
coords = self.mc.get_coords()
|
||||
self.lock.release()
|
||||
if coords:
|
||||
if isinstance(coords, list) and len(coords) == 6 and all(c != -1 for c in coords):
|
||||
ma.x = coords[0]
|
||||
ma.y = coords[1]
|
||||
ma.z = coords[2]
|
||||
|
|
@ -191,6 +233,22 @@ class MycobotTopics(object):
|
|||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_real_gripper_value(self):
|
||||
"""Get Gripper Value"""
|
||||
pub = rospy.Publisher("mycobot/gripper_angle_real",
|
||||
MycobotGetGripperValue, queue_size=5)
|
||||
ma = MycobotGetGripperValue()
|
||||
while not rospy.is_shutdown():
|
||||
with self.lock:
|
||||
try:
|
||||
gripper_value = self.mc.get_gripper_value()
|
||||
if gripper_value:
|
||||
ma.gripper_angle = gripper_value
|
||||
pub.publish(ma)
|
||||
except Exception as e:
|
||||
rospy.logerr(f"SerialException: {e}")
|
||||
time.sleep(0.25)
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
|
|
@ -206,18 +264,86 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_pump_status(self):
|
||||
self.mc.gpio_init()
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_basic_output(data.Pin1, 0)
|
||||
self.mc.gpio_output(data.Pin1, 0)
|
||||
time.sleep(0.05)
|
||||
self.mc.set_basic_output(data.Pin2, 0)
|
||||
else:
|
||||
self.mc.set_basic_output(data.Pin1, 1)
|
||||
self.mc.set_basic_output(data.Pin2, 1)
|
||||
self.mc.gpio_output(data.Pin1, 1)
|
||||
time.sleep(0.05)
|
||||
self.mc.gpio_output(data.Pin2, 0)
|
||||
time.sleep(0.05)
|
||||
self.mc.gpio_output(data.Pin2, 1)
|
||||
time.sleep(0.05)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/pump_status", MycobotPumpStatus, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_fresh_mode_status(self):
|
||||
"""Subscribe to fresh mode Status"""
|
||||
"""订阅运动模式状态"""
|
||||
def callback(data):
|
||||
if data.Status==1:
|
||||
self.mc.set_fresh_mode(1)
|
||||
else:
|
||||
self.mc.set_fresh_mode(0)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/fresh_mode_status", MycobotSetFreshMode, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_end_type_status(self):
|
||||
"""Subscribe to end type Status"""
|
||||
"""订阅末端类型状态"""
|
||||
def callback(data):
|
||||
if data.Status==1:
|
||||
self.mc.set_end_type(1)
|
||||
else:
|
||||
self.mc.set_end_type(0)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/end_type_status", MycobotSetEndType, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_set_tool_reference(self):
|
||||
def callback(data):
|
||||
coords = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
|
||||
self.mc.set_tool_reference(coords)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/tool_reference_goal", MycobotSetToolReference, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def output_robot_message(self):
|
||||
connect_status = False
|
||||
servo_infomation = "unknown"
|
||||
servo_temperature = "unknown"
|
||||
atom_version = "unknown"
|
||||
|
||||
if self.mc:
|
||||
cn = self.mc.is_controller_connected()
|
||||
if cn == 1:
|
||||
connect_status = True
|
||||
time.sleep(0.1)
|
||||
si = self.mc.is_all_servo_enable()
|
||||
if si == 1:
|
||||
servo_infomation = "all connected"
|
||||
version = self.mc.get_system_version()
|
||||
if version:
|
||||
atom_version = version
|
||||
|
||||
print(
|
||||
robot_msg % (connect_status, servo_infomation,
|
||||
servo_temperature, atom_version)
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
Watcher()
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue