From 2f93c0a79fcdc210e05b9564a67fda833e75895e Mon Sep 17 00:00:00 2001 From: jerryjerui Date: Tue, 25 May 2021 15:54:53 +0800 Subject: [PATCH] feat: add somt topics. --- CMakeLists.txt | 7 +- msg/MycobotAngles.msg | 6 + msg/MycobotCoords.msg | 6 + msg/{Gripper.msg => MycobotGripperStatus.msg} | 0 msg/MycobotPumpStatus.msg | 1 + msg/MycobotSetAngles.msg | 8 + msg/MycobotSetCoords.msg | 9 ++ scripts/mycobot_topics.py | 137 ++++++++++++++++++ 8 files changed, 173 insertions(+), 1 deletion(-) create mode 100644 msg/MycobotAngles.msg create mode 100644 msg/MycobotCoords.msg rename msg/{Gripper.msg => MycobotGripperStatus.msg} (100%) create mode 100644 msg/MycobotPumpStatus.msg create mode 100644 msg/MycobotSetAngles.msg create mode 100644 msg/MycobotSetCoords.msg create mode 100644 scripts/mycobot_topics.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 6754309..5eec60b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,12 @@ find_package(catkin REQUIRED COMPONENTS ) add_message_files(FILES - Gripper.msg + MycobotAngles.msg + MycobotCoords.msg + MycobotSetAngles.msg + MycobotSetCoords.msg + MycobotGripperStatus.msg + MycobotPumpStatus.msg ) add_service_files(FILES diff --git a/msg/MycobotAngles.msg b/msg/MycobotAngles.msg new file mode 100644 index 0000000..9aa7d97 --- /dev/null +++ b/msg/MycobotAngles.msg @@ -0,0 +1,6 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 diff --git a/msg/MycobotCoords.msg b/msg/MycobotCoords.msg new file mode 100644 index 0000000..08f6dbd --- /dev/null +++ b/msg/MycobotCoords.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz \ No newline at end of file diff --git a/msg/Gripper.msg b/msg/MycobotGripperStatus.msg similarity index 100% rename from msg/Gripper.msg rename to msg/MycobotGripperStatus.msg diff --git a/msg/MycobotPumpStatus.msg b/msg/MycobotPumpStatus.msg new file mode 100644 index 0000000..635a864 --- /dev/null +++ b/msg/MycobotPumpStatus.msg @@ -0,0 +1 @@ +bool Status diff --git a/msg/MycobotSetAngles.msg b/msg/MycobotSetAngles.msg new file mode 100644 index 0000000..3888b50 --- /dev/null +++ b/msg/MycobotSetAngles.msg @@ -0,0 +1,8 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 + +int8 speed diff --git a/msg/MycobotSetCoords.msg b/msg/MycobotSetCoords.msg new file mode 100644 index 0000000..d4c7e54 --- /dev/null +++ b/msg/MycobotSetCoords.msg @@ -0,0 +1,9 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz + +int8 speed +int8 model diff --git a/scripts/mycobot_topics.py b/scripts/mycobot_topics.py new file mode 100644 index 0000000..d9738e5 --- /dev/null +++ b/scripts/mycobot_topics.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python2 +import time +import threading + +import rospy + +from mycobot_ros.msg import (MycobotAngles, MycobotCoords, MycobotSetAngles, MycobotSetCoords, MycobotGripperStatus, MycobotPumpStatus) +from sensor_msgs.msg import JointState + +from pymycobot.mycobot import MyCobot + + +class MycobotTopics(object): + + def __init__(self): + super(MycobotTopics, self).__init__() + + rospy.init_node('mycobot_topics') + rospy.loginfo('start ...') + port = rospy.get_param('~port') + baud = rospy.get_param('~baud') + 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 True: + 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(.25) + + def pub_real_coords(self): + pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5) + ma = MycobotCoords() + + while True: + 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(.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_angles(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() + + +if __name__ == '__main__': + mc_topics = MycobotTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass \ No newline at end of file