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()
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')
time.sleep(1.5)
print('::send_angle()')
mycobot.send_angle(Angle.J2.value, 10, 50)
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()
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)
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")
time.sleep(1.5)
print("::send_angle()")
mycobot.send_angle(Angle.J2.value, 10, 50)