diff --git a/mycobot_280/mycobot_280/CMakeLists.txt b/mycobot_280/mycobot_280/CMakeLists.txt
index c6ea4f94..1d247587 100644
--- a/mycobot_280/mycobot_280/CMakeLists.txt
+++ b/mycobot_280/mycobot_280/CMakeLists.txt
@@ -32,6 +32,7 @@ catkin_install_python(PROGRAMS
scripts/simple_gui.py
scripts/follow_display_gripper.py
scripts/slider_control_gripper.py
+ scripts/listen_real_gripper.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
diff --git a/mycobot_280/mycobot_280/launch/simple_gui_gripper.launch b/mycobot_280/mycobot_280/launch/simple_gui_gripper.launch
index 32fa28a3..b85a1383 100644
--- a/mycobot_280/mycobot_280/launch/simple_gui_gripper.launch
+++ b/mycobot_280/mycobot_280/launch/simple_gui_gripper.launch
@@ -20,6 +20,6 @@
-
+
diff --git a/mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch b/mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch
index 322b7a5a..7c8b72b1 100644
--- a/mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch
+++ b/mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch
@@ -21,5 +21,5 @@
-
+
diff --git a/mycobot_280/mycobot_280/scripts/listen_real_gripper.py b/mycobot_280/mycobot_280/scripts/listen_real_gripper.py
new file mode 100755
index 00000000..1d6ed867
--- /dev/null
+++ b/mycobot_280/mycobot_280/scripts/listen_real_gripper.py
@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# license removed for brevity
+from distutils.log import error
+import time
+import math
+import os
+import fcntl
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from mycobot_communication.srv import GetAngles
+from pymycobot.mycobot import MyCobot
+from rospy import ServiceException
+
+mc = None
+
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+ open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+ fd = os.open(lock_file, open_mode)
+
+ pid = os.getpid()
+ lock_file_fd = None
+
+ timeout = 50.0
+ start_time = current_time = time.time()
+ while current_time < start_time + timeout:
+ try:
+ # The LOCK_EX means that only one process can hold the lock
+ # The LOCK_NB means that the fcntl.flock() is not blocking
+ # and we are able to implement termination of while loop,
+ # when timeout is reached.
+ fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+ except (IOError, OSError):
+ pass
+ else:
+ lock_file_fd = fd
+ break
+
+ # print('pid waiting for lock:%d'% pid)
+
+
+ time.sleep(1.0)
+ current_time = time.time()
+ if lock_file_fd is None:
+ os.close(fd)
+ return lock_file_fd
+
+
+def release(lock_file_fd):
+ # Do not remove the lockfile:
+ fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+ os.close(lock_file_fd)
+ return None
+
+def talker():
+ rospy.loginfo("start ...")
+
+ rospy.init_node("real_listener_gripper", anonymous=True)
+ pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+ port = rospy.get_param("~port", "/dev/ttyUSB0")
+ baud = rospy.get_param("~baud", 115200)
+ mc = MyCobot(port, baud)
+ 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",
+ "gripper_controller",
+ ]
+ joint_state_send.velocity = [0]
+ joint_state_send.effort = []
+
+ # waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
+ rospy.loginfo("wait service")
+ rospy.wait_for_service("get_joint_angles")
+
+ while True:
+ try:
+ func = rospy.ServiceProxy("get_joint_angles", GetAngles)
+ break
+ except ServiceException as e:
+ # pass
+ # print(f'error:{e}')
+ print("--------------error",e)
+
+ rospy.loginfo("start loop ...")
+ while not rospy.is_shutdown():
+ # get real angles from server.从服务器获得真实的角度。
+ res = func()
+ if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
+ continue
+ if mc:
+ lock = acquire("/tmp/mycobot_lock")
+ gripper_value = mc.get_gripper_value()
+ release(lock)
+ if gripper_value != -1:
+ gripper_value = -0.78 + round(gripper_value / 117.0, 2)
+ # print(gripper_value)
+ radians_list = [
+ res.joint_1 * (math.pi / 180),
+ res.joint_2 * (math.pi / 180),
+ res.joint_3 * (math.pi / 180),
+ res.joint_4 * (math.pi / 180),
+ res.joint_5 * (math.pi / 180),
+ res.joint_6 * (math.pi / 180),
+ ]
+ radians_list.append(gripper_value)
+ rospy.loginfo("res: {}".format(radians_list))
+
+ # publish angles.发布角度
+ joint_state_send.header.stamp = rospy.Time.now()
+ joint_state_send.position = radians_list
+ pub.publish(joint_state_send)
+ rate.sleep()
+
+
+if __name__ == "__main__":
+ try:
+ talker()
+ except rospy.ROSInterruptException:
+ pass
\ No newline at end of file