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()
