def __init__(self):
port_str = rospy.get_param("/hardware_interface/mycobot_port",
"default")
if port_str == "default":
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True)
else:
port = subprocess.check_output(['echo -n ' + port_str], shell=True)
self.mycobot_ = MyCobot(port)
self.mycobot_.power_on()
rospy.init_node('mycobot_hw_interface', anonymous=True)
self.joint_state_msg_pub_ = rospy.Publisher('joint_states_array',
Float32MultiArray,
queue_size=10)
self.joint_cmd_sub = rospy.Subscriber("joint_cmd_array",
Float32MultiArray,
self.jointCommandCallback)
self.rate_ = rospy.Rate(10) # 10hz
self.joint_state_array_ = []
JOINT_NUMBER = 6
for tmp in range(JOINT_NUMBER): # initialize array
tmp = 0.0
self.joint_state_array_.append(tmp)
self.pre_data_list = []
self.first_flag = True
def __init__(self):
port_str = rospy.get_param("/hardware_interface/mycobot_port",
"default")
if port_str == "default":
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True)
else:
port = subprocess.check_output(['echo -n ' + port_str], shell=True)
self.mycobot_ = MyCobot(port)
self.mycobot_.power_on()
rospy.init_node('mycobot_bridge', anonymous=True)
self.joint_res_pub_ = rospy.Publisher('/joint_res_',
Float32MultiArray,
queue_size=10)
self.joint_cmd_sub = rospy.Subscriber("/joint_cmd_", Float32MultiArray,
self._jointCommandCallback)
self.rate_ = rospy.Rate(10)
self.joint_state_array_ = []
self.pre_data_list = []
self.flag_first = True
NUM_JOINT = 6
for tmp_zero in range(NUM_JOINT):
tmp_zero = 0.0
self.joint_state_array_.append(tmp_zero)
def __init__(self):
super(MycobotTopics, self).__init__()
rospy.init_node("mycobot_topics")
rospy.loginfo("start ...")
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = MyCobot(port, baud)
self.lock = threading.Lock()
def setup():
print("")
global port, mc
plist = list(serial.tools.list_ports.comports())
idx = 1
for port in plist:
print("{} : {}".format(idx, port))
idx += 1
_in = input("\nPlease input 1 - {} to choice:".format(idx - 1))
port = str(plist[int(_in) - 1]).split(" - ")[0].strip()
print(port)
print("")
baud = 115200
_baud = input("Please input baud(default:115200):")
try:
baud = int(_baud)
except Exception:
pass
print(baud)
print("")
DEBUG = False
f = input("Wether DEBUG mode[Y/n]:")
if f in ["y", "Y", "yes", "Yes"]:
DEBUG = True
# mc = MyCobot(port, debug=True)
mc = MyCobot(port, baud, debug=DEBUG)
def create_handle():
global mc
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)
def listener():
global mc
rospy.init_node("control_slider", anonymous=True)
rospy.Subscriber("joint_states", JointState, callback)
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
# spin() simply keeps python from exiting until this node is stopped
print("spin ...")
rospy.spin()
def connect_mycobot(self):
self.prot = port = self.port_list.get()
if not port:
self.write_log_to_Text("请选择串口")
return
self.baud = baud = self.baud_list.get()
if not baud:
self.write_log_to_Text("请选择波特率")
return
baud = int(baud)
try:
# self.mycobot = MyCobot(PI_PORT, PI_BAUD)
self.mycobot = MyCobot(port, baud)
# self.mycobot = MyCobot("/dev/cu.usbserial-0213245D", 115200)
self.write_log_to_Text("连接成功 !")
except Exception as e:
err_log = """\
\r连接失败 !!!
\r=================================================
{}
\r=================================================
""".format(e)
self.write_log_to_Text(err_log)
def setup(): #机械臂检测函数，选择正确的串口
global mc
print("")
plist = list(serial.tools.list_ports.comports())
idx = 0
for port in plist:
print("{} : {}".format(idx, port))
idx += 1
if idx == 0:
print("The connected device was not detected. Please try reconnecting.")
exit(1)
_in = input("\nPlease input 0 - {} to choice, you can choice many like: '2,1,3':".format(idx))
idxes = _in.split(',')
try:
idxes = [int(i) for i in idxes]
except Exception:
print('Error: Input format error.')
exit(1)
ports = [str(plist[i]).split(' - ')[0].strip() for i in idxes]
print(ports)
print("")
baud = 115200
_baud = input("Please input baud(default:115200):")
try:
baud = int(_baud)
except Exception:
pass
print(baud)
print("")
for port in ports:
try:
mycobot = MyCobot(port, baud)
except Exception as e:
print(e)
exit(1)
mc.append(mycobot)
from pymycobot.mycobot import MyCobot
def callback(data):
#rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
# print(data.position)
data_list = []
print(data.position)
for index, value in enumerate(data.position):
data_list.append(value)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
def listener():
rospy.init_node('control_slider', anonymous=True)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
print('sart')
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True).decode()
print(port)
mc = MyCobot(port)
print(mc)
listener()
This file for test the API if right.
Just can run in Linux.
"""
import time, random, subprocess
from pymycobot.mycobot import MyCobot
from pymycobot.genre import Angle, Coord
if __name__ == "__main__":
sys_ = subprocess.check_output(["uname"], shell=True).decode()
if not sys_.startswith("Linux"):
print("This script just can run on Linux.")
exit(0)
port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode()
mycobot = MyCobot(port)
print("Start check api\n")
print("::get_angles()")
print("==> degrees: {}\n".format(mycobot.get_angles()))
time.sleep(0.5)
print("::get_radians()")
print("==> radians: {}\n".format(mycobot.get_radians()))
time.sleep(0.5)
print("::send_angles()")
mycobot.send_angles([0, 0, 0, 0, 0, 0], 80)
print("==> set angles [0,0,0,0,0,0], speed 80\n")
print("Is moving: {}".format(mycobot.is_moving()))
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
#marker position initial
# print(coords)
if not coords:
coords = [0, 0, 0, 0, 0, 0]
rospy.loginfo('error [101]: can not get coord values')
marker_.pose.position.x = coords[1] / 1000 * -1
marker_.pose.position.y = coords[0] / 1000
marker_.pose.position.z = coords[2] / 1000
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == '__main__':
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True).decode()
mycobot = MyCobot(port)
try:
talker()
except rospy.ROSInterruptException:
pass
#!/usr/bin/env python2
from pymycobot import mycobot
import rospy
from std_msgs.msg import String
from std_msgs.msg import Float64
from std_msgs.msg import Int8
from pymycobot import PI_PORT, PI_BAUD
from pymycobot.mycobot import MyCobot
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
mc = MyCobot(PI_PORT, PI_BAUD)
pub = rospy.Publisher("gripper_status", Float64, queue_size=10)
# Defining the subscriber publisher
def talker():
rospy.init_node("gripper_node")
rate = rospy.Rate(5)
while not rospy.is_shutdown():
gripper_value = mc.get_gripper_value()
print(gripper_value)
pub.publish(gripper_value)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
class MycobotTopics(object):
def __init__(self):
super(MycobotTopics, self).__init__()
rospy.init_node("mycobot_topics")
rospy.loginfo("start ...")
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = MyCobot(port, baud)
self.lock = threading.Lock()
def start(self):
pa = threading.Thread(target=self.pub_real_angles)
pb = threading.Thread(target=self.pub_real_coords)
sa = threading.Thread(target=self.sub_set_angles)
sb = threading.Thread(target=self.sub_set_coords)
sg = threading.Thread(target=self.sub_gripper_status)
sp = threading.Thread(target=self.sub_pump_status)
pa.setDaemon(True)
pa.start()
pb.setDaemon(True)
pb.start()
sa.setDaemon(True)
sa.start()
sb.setDaemon(True)
sb.start()
sg.setDaemon(True)
sg.start()
sp.setDaemon(True)
sp.start()
pa.join()
pb.join()
sa.join()
sb.join()
sg.join()
sp.join()
def pub_real_angles(self):
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
ma = MycobotAngles()
while not rospy.is_shutdown():
self.lock.acquire()
angles = self.mc.get_angles()
self.lock.release()
if angles:
ma.joint_1 = angles[0]
ma.joint_2 = angles[1]
ma.joint_3 = angles[2]
ma.joint_4 = angles[3]
ma.joint_5 = angles[4]
ma.joint_6 = angles[5]
pub.publish(ma)
time.sleep(0.25)
def pub_real_coords(self):
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
ma = MycobotCoords()
while not rospy.is_shutdown():
self.lock.acquire()
coords = self.mc.get_coords()
self.lock.release()
if coords:
ma.x = coords[0]
ma.y = coords[1]
ma.z = coords[2]
ma.rx = coords[3]
ma.ry = coords[4]
ma.rz = coords[5]
pub.publish(ma)
time.sleep(0.25)
def sub_set_angles(self):
def callback(data):
angles = [
data.joint_1,
data.joint_2,
data.joint_3,
data.joint_4,
data.joint_5,
data.joint_6,
]
sp = int(data.speed)
self.mc.send_angles(angles, sp)
sub = rospy.Subscriber(
"mycobot/angles_goal", MycobotSetAngles, callback=callback
)
rospy.spin()
def sub_set_coords(self):
def callback(data):
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
sp = int(data.speed)
model = int(data.model)
self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber(
"mycobot/coords_goal", MycobotSetCoords, callback=callback
)
rospy.spin()
def sub_gripper_status(self):
def callback(data):
if data.Status:
self.mc.set_gripper_state(0, 80)
else:
self.mc.set_gripper_state(1, 80)
sub = rospy.Subscriber(
"mycobot/gripper_status", MycobotGripperStatus, callback=callback
)
rospy.spin()
def sub_pump_status(self):
def callback(data):
if data.Status:
self.mc.set_basic_output(2, 0)
self.mc.set_basic_output(5, 0)
else:
self.mc.set_basic_output(2, 1)
self.mc.set_basic_output(5, 1)
sub = rospy.Subscriber(
"mycobot/pump_status", MycobotPumpStatus, callback=callback
)
rospy.spin()
for angles in special_angles:
mc.send_angles(angles, 80)
time.sleep(4)
coords = mc.get_coords()
time.sleep(0.1)
print(coords)
"""
[-278.1, -16.8, 104.2, 79.82, -81.6, -169.53]
[-279.2, -15.2, 117.5, 34.72, -85.83, -124.83]
[171.6, -113.1, 229.8, 114.15, -84.21, -24.27]
[-211.5, -113.8, 188.5, -89.79, 6.14, -177.87]
[190.8, -113.1, 210.7, 104.26, -82.19, -14.39]
"""
if __name__ == "__main__":
time.sleep(1)
with open(os.path.dirname(__file__) + "/port.txt") as f:
port = f.read().strip().replace("\n", "")
print(port)
mc = MyCobot(port, boudrate="1000000")
# test(mc)
# coords = [-279.2, -15.2, 117.5, 34.72, -85.83, -124.83]
# mc.send_coords(coords, 80, 1)
# exit(0)
while True:
print(mc.get_coords())
time.sleep(1)
import time, random, subprocess
from pymycobot.mycobot import MyCobot
# from pythonAPI.mycobot3 import MyCobot as MyCobot3
from pymycobot.genre import Angle, Coord
if __name__ == '__main__':
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True).decode()
mycobot = MyCobot(port)
# port = subprocess.run(['echo -n /dev/ttyUSB*'],
# stdout=subprocess.PIPE,
# shell=True).stdout.decode('utf-8')
# mycobot = MyCobot3(port)
print('Start check api\n')
color_name = ['red', 'green', 'blue']
color_code = ['ff0000', '00ff00', '0000ff']
print('::ser_led_color()')
i = random.randint(0, len(color_code) - 1)
mycobot.set_led_color(color_code[i])
print('==>set color {}\n'.format(color_name[i]))
time.sleep(0.5)
print('::get_angles()')
print('==> degrees: {}\n'.format(mycobot.get_angles()))
time.sleep(0.5)
print('::get_radians()')
print('==> radians: {}\n'.format(mycobot.get_radians()))
def init(mc: MyCobot):
degrees = [0, 0, 0, 0, 0, 0]
mc.send_angles(degrees, 20)
from pymycobot.mycobot import MyCobot
import time
DEVICE_NAME = '/dev/tty.usbserial-0203B064'
mycobot = MyCobot(DEVICE_NAME)
mycobot.send_radians([0.0, 0.0, 0.0, 1.3, 0.0, 0.0], 30)
buffer = input('input any key: ')
mycobot.send_radians([0.0, -0.9, 2.18, 0.17, 0.0, 0.0], 30)
time.sleep(2)
mycobot.send_radians([2.9, -0.9, 2.18, 0.17, 0.0, 0.0], 30)
time.sleep(2)
mycobot.send_radians([2.9, 1.3, 1.5, -1.4, 0.0, 0.0], 30)
time.sleep(3)
mycobot.send_radians([2.9, 1.3, 1.5, -1.4, -1.57, 0.0], 90)
def mov1(mc : MyCobot):
init(mc)
time.sleep(10)
mc.send_angle(Angle.J1.value, 90, 30)
time.sleep(5)
mc.send_angle(Angle.J1.value, -90, 30)
time.sleep(5)
mc.send_angle(Angle.J1.value, 0, 30)
time.sleep(5)
mc.send_angle(Angle.J2.value, 90, 30)
time.sleep(5)
mc.send_angle(Angle.J2.value, -90, 30)
time.sleep(5)
mc.send_angle(Angle.J2.value, 0, 30)
time.sleep(5)
mc.send_angle(Angle.J3.value, 90, 30)
time.sleep(5)
mc.send_angle(Angle.J3.value, -90, 30)
time.sleep(5)
mc.send_angle(Angle.J3.value, 0, 30)
time.sleep(5)
mc.send_angle(Angle.J4.value, 90, 30)
time.sleep(5)
mc.send_angle(Angle.J4.value, -90, 30)
time.sleep(5)
mc.send_angle(Angle.J4.value, 0, 30)
time.sleep(5)
mc.send_angle(Angle.J5.value, 90, 30)
time.sleep(5)
mc.send_angle(Angle.J5.value, -90, 30)
time.sleep(5)
mc.send_angle(Angle.J5.value, 0, 30)
time.sleep(5)
mc.send_angle(Angle.J6.value, 90, 30)
time.sleep(5)
mc.send_angle(Angle.J6.value, -90, 30)
time.sleep(5)
mc.send_angle(Angle.J6.value, 0, 30)
time.sleep(5)
class MycobotHwInterface:
def __init__(self):
port_str = rospy.get_param("/hardware_interface/mycobot_port",
"default")
if port_str == "default":
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True)
else:
port = subprocess.check_output(['echo -n ' + port_str], shell=True)
self.mycobot_ = MyCobot(port)
self.mycobot_.power_on()
rospy.init_node('mycobot_hw_interface', anonymous=True)
self.joint_state_msg_pub_ = rospy.Publisher('joint_states_array',
Float32MultiArray,
queue_size=10)
self.joint_cmd_sub = rospy.Subscriber("joint_cmd_array",
Float32MultiArray,
self.jointCommandCallback)
self.rate_ = rospy.Rate(10) # 10hz
self.joint_state_array_ = []
JOINT_NUMBER = 6
for tmp in range(JOINT_NUMBER): # initialize array
tmp = 0.0
self.joint_state_array_.append(tmp)
self.pre_data_list = []
self.first_flag = True
def main_loop(self):
while not rospy.is_shutdown():
self.joint_state_msg_sender()
self.rate_.sleep()
def joint_state_msg_sender(self):
angles = self.mycobot_.get_radians()
for index, value in enumerate(angles):
if index != 2:
value *= -1
self.joint_state_array_[index] = value
# str = "angles: %s" % angles
# rospy.loginfo(str)
joint_state_msg = Float32MultiArray(data=self.joint_state_array_)
self.joint_state_msg_pub_.publish(joint_state_msg)
def jointCommandCallback(self, msg):
data_list = []
for index, value in enumerate(msg.data):
if index != 2:
value *= -1
data_list.append(value)
if self.first_flag:
for value in data_list:
self.pre_data_list.append(value)
self.first_flag = False
if self.pre_data_list != data_list:
rospy.loginfo(rospy.get_caller_id() + "%s", msg.data)
self.mycobot_.send_radians(data_list, 80)
self.pre_data_list = []
for value in data_list:
self.pre_data_list.append(value)
from pymycobot.mycobot import MyCobot
import port
import move
import threading
mc = MyCobot(port.port)
threadMov = threading.Thread(target=move.mov1, args=(mc, ))
threadMov.start()
while True:
print(mc.get_coords())
class MycobotTest(object):
def __init__(self):
self.mycobot = None
self.win = tkinter.Tk()
self.win.title("树莓派版 Mycobot 测试工具")
self.win.geometry(
"918x511+10+10") # 290 160为窗口大小，+10 +10 定义窗口弹出时的默认展示位置
self.port_label = tkinter.Label(self.win, text="选择串口：")
self.port_label.grid(row=0)
self.port_list = ttk.Combobox(
self.win, width=15,
postcommand=self.get_serial_port_list) # #创建下拉菜单
self.get_serial_port_list() # #给下拉菜单设定值
self.port_list.current(0)
self.port_list.grid(row=0, column=1)
self.baud_label = tkinter.Label(self.win, text="选择波特率：")
self.baud_label.grid(row=1)
self.baud_list = ttk.Combobox(self.win, width=15)
self.baud_list["value"] = ("115200", "1000000")
self.baud_list.current(0)
self.baud_list.grid(row=1, column=1)
# Connect
self.connect_label = tkinter.Label(self.win, text="连接mycobot：")
self.connect_label.grid(row=2)
self.connect = tkinter.Button(self.win,
text="连接",
command=self.connect_mycobot)
self.disconnect = tkinter.Button(self.win,
text="断开",
command=self.disconnect_mycobot)
self.connect.grid(row=3)
self.disconnect.grid(row=3, column=1)
# Check servo.
self.check_label = tkinter.Label(self.win, text="检测连接：")
self.check_label.grid(row=4)
self.check_btn = tkinter.Button(self.win,
text="开始检测",
command=self.check_mycobot_servos)
self.check_btn.grid(row=4, column=1)
# Calibration.
self.calibration_num = None
self.calibration_label = tkinter.Label(self.win, text="校准舵机：")
self.calibration_label.grid(row=5)
self.calibration_btn = tkinter.Button(self.win,
text="开始校准",
command=self.calibration_mycobot)
self.calibration_btn.grid(row=5, column=1)
# LED.
self.set_color_label = tkinter.Label(self.win, text="测试Atom灯板：")
self.set_color_label.grid(row=6, columnspan=2)
self.color_red = tkinter.Button(self.win,
text="设置红色",
command=lambda: self.send_color("red"))
self.color_green = tkinter.Button(
self.win, text="设置绿色", command=lambda: self.send_color("green"))
self.color_red.grid(row=7)
self.color_green.grid(row=7, column=1)
# Aging test.
self.aging_stop = False
self.movement_label = tkinter.Label(self.win, text="老化循环动作：")
self.movement_label.grid(row=8)
self.start_btn = tkinter.Button(self.win,
text="开始",
command=self.start_aging_test)
self.start_btn.grid(row=9)
self.stop_btn = tkinter.Button(self.win,
text="停止",
command=self.stop_aging_test)
self.stop_btn.grid(row=9, column=1)
# Release
self.release_btn = tkinter.Button(self.win,
text="放松所有电机",
command=self.release_mycobot)
self.release_btn.grid(row=10)
# rectify
self.rectify_btn = tkinter.Button(self.win,
text="校准pid",
command=self.rectify_mycobot)
self.rectify_btn.grid(row=10, column=1)
# Log output.
self.log_label = tkinter.Label(self.win, text="日志：")
self.log_label.grid(row=0, column=12)
_f = tkinter.Frame(self.win)
_bar = tkinter.Scrollbar(_f, orient=tkinter.VERTICAL)
self.log_data_Text = tkinter.Text(_f,
width=100,
height=35,
yscrollcommand=_bar.set)
_bar.pack(side=tkinter.RIGHT, fill=tkinter.Y)
_bar.config(command=self.log_data_Text.yview)
self.log_data_Text.pack()
# self.log_data_Text.grid(row=1, column=12, rowspan=15, columnspan=10)
_f.grid(row=1, column=12, rowspan=15, columnspan=10)
def run(self):
self.win.mainloop() # run
# ============================================================
# Connect method
# ============================================================
def connect_mycobot(self):
self.prot = port = self.port_list.get()
if not port:
self.write_log_to_Text("请选择串口")
return
self.baud = baud = self.baud_list.get()
if not baud:
self.write_log_to_Text("请选择波特率")
return
baud = int(baud)
try:
# self.mycobot = MyCobot(PI_PORT, PI_BAUD)
self.mycobot = MyCobot(port, baud)
# self.mycobot = MyCobot("/dev/cu.usbserial-0213245D", 115200)
self.write_log_to_Text("连接成功 !")
except Exception as e:
err_log = """\
\r连接失败 !!!
\r=================================================
{}
\r=================================================
""".format(e)
self.write_log_to_Text(err_log)
def disconnect_mycobot(self):
if not self.has_mycobot():
return
try:
del self.mycobot
self.mycobot = None
self.write_log_to_Text("断开连接成功 !")
except AttributeError:
self.write_log_to_Text("还没有连接mycobot！！！")
# ============================================================
# Function method
# ============================================================
def release_mycobot(self):
if not self.has_mycobot():
return
self.mycobot.release_all_servos()
self.write_log_to_Text("Release over.")
def check_mycobot_servos(self):
if not self.has_mycobot():
return
ping_commands = [
[255, 255, 1, 2, 1, 251],
[255, 255, 2, 2, 1, 250],
[255, 255, 3, 2, 1, 249],
[255, 255, 4, 2, 1, 248],
[255, 255, 5, 2, 1, 247],
[255, 255, 6, 2, 1, 246],
[255, 255, 7, 2, 1, 246],
]
res = []
for idx, command in enumerate(ping_commands, start=1):
self.mycobot._write(command)
time.sleep(0.1)
if not self.mycobot._read():
res.append(idx)
time.sleep(0.1)
if res:
self.write_log_to_Text("关节 {} 无法通信！！！".format(res))
else:
self.write_log_to_Text("所有关节连接正常。")
def calibration_mycobot(self):
"""Calibration button click event.
Click to calibrate one motor at a time and calibrate in turn. After all
calibration, resume initialization.
"""
if not self.has_mycobot():
return
if not self.calibration_num:
self.calibration_num = 0
self.calibration_num += 1
self.mycobot.set_servo_calibration(self.calibration_num)
time.sleep(0.1)
self.mycobot.send_angle(self.calibration_num, 0, 0)
time.sleep(0.1)
self.write_log_to_Text("校准电机 %s 结束." % self.calibration_num)
if self.calibration_num == 6:
self.write_log_to_Text("全部校准完成.")
self.calibration_num = None
self.rectify_mycobot()
self._calibration_test()
def send_color(self, color: str):
if not self.has_mycobot():
return
color_dict = {
"red": [255, 0, 0],
"green": [0, 255, 0],
"blue": [0, 0, 255],
}
self.mycobot.set_color(*color_dict[color])
self.write_log_to_Text("发送颜色: {}.".format(color))
def start_aging_test(self):
if not self.has_mycobot():
return
self.aging_stop = False
self.aging = threading.Thread(target=self._aging_test, daemon=True)
self.aging.start()
# self._aging_test()
self.write_log_to_Text("开始循环老化测试 ...")
def stop_aging_test(self):
try:
os.system("sudo systemctl stop aging_test.service")
os.system("sudo rm /home/pi/aging_test.sh")
os.system("sudo rm /home/pi/Desktop/aging_test.py")
os.system("sudo rm /etc/systemd/system/aging_test.service")
os.system("sudo systemctl daemon-reload")
self.write_log_to_Text("结束循环老化测试.")
except:
self.write_log_to_Text("结束老化测试失败 ！！！")
def rectify_mycobot(self):
if not self.has_mycobot():
return
for i in range(1, 7):
self.mycobot.set_servo_data(i, 24, 0)
time.sleep(0.1)
self.mycobot.set_servo_data(i, 26, 3)
time.sleep(0.1)
self.mycobot.set_servo_data(i, 27, 3)
time.sleep(0.1)
time.sleep(0.1)
for i in range(1, 7):
self.write_log_to_Text(
"Read servo {} pid data, 24:{}, 26:{}, 27:{}.".format(
i,
self.mycobot.get_servo_data(i, 24),
self.mycobot.get_servo_data(i, 26),
self.mycobot.get_servo_data(i, 27),
))
time.sleep(0.1)
# ============================================================
# Utils method
# ============================================================
def has_mycobot(self):
"""Check whether it is connected on mycobot"""
if not self.mycobot:
self.write_log_to_Text("还没有连接mycobot！！！")
return False
return True
def _aging_test(self):
"""
Aging test thread target.
By using in `start_aging_test()` and `stop_aging_test()`.
"""
# if socket.gethostname() != "pi":
# self.write_log_to_Text("老化测试支持 Raspberry OS.")
# return
aging_test_content_py = textwrap.dedent("""\
#!/usr/bin/python3
from pymycobot.mycobot import MyCobot
from pymycobot import PI_PORT, PI_BAUD
import time
mycobot = MyCobot('%s', %s)
def aging_test():
# fast
mycobot.set_color(255, 0, 0)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([170, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([-170, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 90, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, -90, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 140, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, -140, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 130, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, -110, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 165, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, -165, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 180], 95)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, -180], 95)
# middle
mycobot.set_color(0, 255, 0)
mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([170, 0, 0, 0, 0, 0], 55)
mycobot.wait(6.5).send_angles([-170, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 90, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, -90, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 140, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, -140, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 130, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, -110, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 165, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, -165, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 180], 55)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, -180], 55)
# slow
mycobot.set_color(0, 0, 255)
mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([170, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([-170, 0, 0, 0, 0, 0], 15)
mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, 90, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, -90, 0, 0, 0, 0], 15)
mycobot.wait(0).send_angles([0, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, 0, 140, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, 0, -140, 0, 0, 0], 15)
mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, 0, 0, 130, 0, 0], 15)
mycobot.wait(7).send_angles([0, 0, 0, -110, 0, 0], 15)
mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, 0, 0, 0, 165, 0], 15)
mycobot.wait(7).send_angles([0, 0, 0, 0, -165, 0], 15)
mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15)
mycobot.wait(7).send_angles([0, 0, 0, 0, 0, 180], 15)
mycobot.wait(7).send_angles([0, 0, 0, 0, 0, -180], 15)
if __name__ == '__main__':
while True:
aging_test()
""" % (self.prot, self.baud))
aging_test_content_sh = textwrap.dedent("""\
#!/bin/bash
/usr/bin/python3 /home/pi/Desktop/aging_test.py
""")
aging_test_content_service = textwrap.dedent("""\
[Unit]
Description=aging-test
[Service]
Type=forking
User=pi
Restart=on-failure
RestartSec=2
ExecStart=/home/pi/aging_test.sh
[Install]
WantedBy=multi-user.target
""")
os.system('echo "' + aging_test_content_py +
'" >> /home/pi/Desktop/aging_test.py')
os.system('echo "' + aging_test_content_sh +
'" >> /home/pi/aging_test.sh')
os.system("sudo chmod +x /home/pi/aging_test.sh")
os.system('echo "' + aging_test_content_service +
'" >> /home/pi/Desktop/aging_test.service')
os.system(
"sudo mv /home/pi/Desktop/aging_test.service /etc/systemd/system/aging_test.service"
)
os.system("sudo systemctl enable aging_test.service")
os.system("sudo systemctl start aging_test.service")
def _calibration_test(self):
self.write_log_to_Text("开始测试校准.")
angles = [0, 0, 0, 0, 0, 0]
test_angle = [-20, 20, 0]
for i in range(6):
for j in range(3):
angles[i] = test_angle[j]
self.mycobot.send_angles(angles, 0)
time.sleep(0.7)
self.write_log_to_Text("测试校准结束.")
def get_serial_port_list(self):
plist = [
str(x).split(" - ")[0].strip()
for x in serial.tools.list_ports.comports()
]
print(plist)
self.port_list["value"] = plist
return plist
def get_current_time(self):
"""Get current time with format."""
current_time = time.strftime("%Y-%m-%d %H:%M:%S",
time.localtime(time.time()))
return current_time
def write_log_to_Text(self, logmsg: str):
global LOG_NUM
current_time = self.get_current_time()
logmsg_in = str(current_time) + " " + str(logmsg) + "\n" # 换行
if LOG_NUM <= 18:
self.log_data_Text.insert(tkinter.END, logmsg_in)
LOG_NUM += len(logmsg_in.split("\n"))
# print(LOG_NUM)
else:
self.log_data_Text.insert(tkinter.END, logmsg_in)
self.log_data_Text.yview("end")
from pymycobot.mycobot import MyCobot from port import port mc = MyCobot(port) mc.release_all_servos()
def talker():
rospy.init_node("display", anonymous=True)
print("Try connect real mycobot...")
port = rospy.get_param("~port", "/dev/ttyAMA0")
baud = rospy.get_param("~baud", 115200)
print("port: {}, baud: {}\n".format(port, baud))
try:
mycobot = MyCobot(port, baud)
except Exception as e:
print(e)
print(
"""\
\rTry connect mycobot failed!
\rPlease check wether connected with mycobot.
\rPlease chckt wether the port or baud is right.
"""
)
exit(1)
mycobot.release_all_servos()
time.sleep(0.1)
print("Rlease all servos over.\n")
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(30) # 30hz
# pub joint state
joint_state_send = JointState()
joint_state_send.header = Header()
joint_state_send.name = [
"joint2_to_joint1",
"joint3_to_joint2",
"joint4_to_joint3",
"joint5_to_joint4",
"joint6_to_joint5",
"joint6output_to_joint6",
]
joint_state_send.velocity = [0]
joint_state_send.effort = []
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "my_namespace"
print("publishing ...")
while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now()
angles = mycobot.get_radians()
data_list = []
for index, value in enumerate(angles):
data_list.append(value)
# rospy.loginfo('{}'.format(data_list))
joint_state_send.position = data_list
pub.publish(joint_state_send)
coords = mycobot.get_coords()
# marker
marker_.header.stamp = rospy.Time.now()
marker_.type = marker_.SPHERE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial
# print(coords)
if not coords:
coords = [0, 0, 0, 0, 0, 0]
rospy.loginfo("error [101]: can not get coord values")
marker_.pose.position.x = coords[1] / 1000 * -1
marker_.pose.position.y = coords[0] / 1000
marker_.pose.position.z = coords[2] / 1000
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
from pymycobot.mycobot import MyCobot from port import port from pymycobot.genre import Angle mc = MyCobot(port) mc.send_angle(Angle.J1.value, 90, 10)
from pymycobot.mycobot import MyCobot # name of device port = "/dev/ttyUSB0" mc = MyCobot(port) # release mycobot # mc.release_all_servos() # calibrate the sixth servo mc.set_servo_calibration(6)
class MycobotBridge:
def __init__(self):
port_str = rospy.get_param("/hardware_interface/mycobot_port",
"default")
if port_str == "default":
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True)
else:
port = subprocess.check_output(['echo -n ' + port_str], shell=True)
self.mycobot_ = MyCobot(port)
self.mycobot_.power_on()
rospy.init_node('mycobot_bridge', anonymous=True)
self.joint_res_pub_ = rospy.Publisher('/joint_res_',
Float32MultiArray,
queue_size=10)
self.joint_cmd_sub = rospy.Subscriber("/joint_cmd_", Float32MultiArray,
self._jointCommandCallback)
self.rate_ = rospy.Rate(10)
self.joint_state_array_ = []
self.pre_data_list = []
self.flag_first = True
NUM_JOINT = 6
for tmp_zero in range(NUM_JOINT):
tmp_zero = 0.0
self.joint_state_array_.append(tmp_zero)
def _joint_state_msg_sender(self):
angles = self.mycobot_.get_radians()
for index, value in enumerate(angles):
if index != 2:
value *= -1
self.joint_state_array_[index] = value
joint_state_msg = Float32MultiArray(data=self.joint_state_array_)
self.joint_res_pub_.publish(joint_state_msg)
def _jointCommandCallback(self, msg):
data_list = []
for index, value in enumerate(msg.data):
if index != 2:
value *= -1
data_list.append(value)
if self.flag_first:
for value in data_list:
self.pre_data_list.append(value)
self.flag_first = False
if self.pre_data_list != data_list:
rospy.loginfo(rospy.get_caller_id() + "%s", msg.data)
self.mycobot_.send_radians(data_list, 90)
self.pre_data_list = []
for value in data_list:
self.pre_data_list.append(value)
def run(self):
while not rospy.is_shutdown():
self._joint_state_msg_sender()
self.rate_.sleep()
