# 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()))
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()))
time.sleep(3)
print('::send_radians')
mycobot.send_radians([1, 1, 1, 1, 1, 1], 70)
print('==> set raidans [1,1,1,1,1,1], speed 70\n')
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()
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()))
time.sleep(3)
print("::send_radians")
mycobot.send_radians([1, 1, 1, 1, 1, 1], 70)
print("==> set raidans [1,1,1,1,1,1], speed 70\n")