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()
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
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())
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)
print('==> send coords [160,160,160,0,0,0], speed 70, mode 0\n')
time.sleep(3.0)
print(mycobot.is_in_position(coord_list, 1))
time.sleep(1)
print('::send_coord()')
mycobot.send_coord(Coord.X.value, -40, 70)
print('==> send coord id: X, coord value: -40, speed: 70\n')
time.sleep(2)
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)
print("==> send coords [160,160,160,0,0,0], speed 70, mode 0\n")
time.sleep(3.0)
print(mycobot.is_in_position(coord_list, 1))
time.sleep(1)
print("::send_coord()")
mycobot.send_coord(Coord.X.value, -40, 70)
print("==> send coord id: X, coord value: -40, speed: 70\n")
time.sleep(2)