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') time.sleep(1.5) print('::send_angle()') mycobot.send_angle(Angle.J2.value, 10, 50) print('==> angle: joint2, degree: 10, speed: 50\n') time.sleep(1) print('::get_coords()') print('==> coords {}\n'.format(mycobot.get_coords())) time.sleep(0.5) print('::send_coords()') coord_list = [160, 160, 160, 0, 0, 0] mycobot.send_coords(coord_list, 70, 0)
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 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)
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()