修复国产280JN系统使用ROS1报错问题

elephantrobotics · Oct 31, 2024 · 02e2544 · 02e2544
1 parent 29b0567
commit 02e2544
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 5 deletions.
diff --git a/mycobot_280/mycobot_280jn/scripts/slider_control.py b/mycobot_280/mycobot_280jn/scripts/slider_control.py
@@ -34,8 +34,7 @@ def callback(data):
 def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
-
-    rospy.Subscriber("joint_states", JointState, callback)
+
     port = rospy.get_param("~port", "/dev/ttyTHS1")
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
@@ -44,6 +43,8 @@ def listener():
     mc.set_fresh_mode(1)
     time.sleep(0.05)
 
+    rospy.Subscriber("joint_states", JointState, callback) 
+
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")

diff --git a/mycobot_280/mycobot_280jn/scripts/slider_control_gripper.py b/mycobot_280/mycobot_280jn/scripts/slider_control_gripper.py
@@ -38,14 +38,15 @@ def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
 
-    rospy.Subscriber("joint_states", JointState, callback)
     port = rospy.get_param("~port", "/dev/ttyTHS1")
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
     mc = MyCobot(port, baud)
     time.sleep(0.05)
     mc.set_fresh_mode(1)
     time.sleep(0.05)
+
+    rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

diff --git a/mycobot_280/mycobot_280jn/scripts/slider_control_parallel_gripper.py b/mycobot_280/mycobot_280jn/scripts/slider_control_parallel_gripper.py
@@ -39,7 +39,6 @@ def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
 
-    rospy.Subscriber("joint_states", JointState, callback)
     port = rospy.get_param("~port", "/dev/ttyTHS1")
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
@@ -48,6 +47,8 @@ def listener():
     mc.set_fresh_mode(1)
     time.sleep(0.05)
 
+    rospy.Subscriber("joint_states", JointState, callback)
+
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")

diff --git a/mycobot_280/mycobot_280jn_gripper_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280jn_gripper_moveit/scripts/sync_plan.py
@@ -38,14 +38,15 @@ def listener():
     global mc
     rospy.init_node("control_slider", anonymous=True)
 
-    rospy.Subscriber("joint_states", JointState, callback)
     port = rospy.get_param("~port", "/dev/ttyTHS1")
     baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
     mc = MyCobot(port, baud)
     time.sleep(0.05)
     mc.set_fresh_mode(1)
     time.sleep(0.05)
+
+    rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止

diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py
@@ -97,6 +97,9 @@ def get_angles(req):
         lock = acquire("/tmp/mycobot_lock")
         angles = mc.get_angles()
         release(lock)
+        if angles is None:
+            rospy.logwarn('angles is None, no angle data')
+            return GetAnglesResponse()
         return GetAnglesResponse(*angles)
 
 
@@ -125,6 +128,9 @@ def get_coords(req):
         lock = acquire("/tmp/mycobot_lock")
         coords = mc.get_coords()
         release(lock)
+        if coords is None:
+            rospy.logwarn('coords is None, no coord data')
+            return GetCoordsResponse()
         return GetCoordsResponse(*coords)