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

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 ...")

mycobot_280/mycobot_280jn/scripts/slider_control_gripper.py

@@ -38,7 +38,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)
@@ -46,6 +45,8 @@ def listener():
     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退出，直到该节点停止

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 ...")

mycobot_280/mycobot_280jn_gripper_moveit/scripts/sync_plan.py

@@ -38,7 +38,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)
@@ -46,6 +45,8 @@ def listener():
     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退出，直到该节点停止

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)