Verzögerung eingebaut

ExoMy_Software-master/src/delay_node.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import collections
+import rospy
+from sensor_msgs.msg import Joy
+
+queue = collections.deque()
+pub = None
+
+
+def callback(msg):
+    delay = rospy.get_param('/delay_seconds', 0.0)
+    if delay <= 0:
+        pub.publish(msg)
+        return
+    queue.append((rospy.Time.now().to_sec(), msg))
+
+
+def spin():
+    rate = rospy.Rate(50)
+    while not rospy.is_shutdown():
+        delay = rospy.get_param('/delay_seconds', 0.0)
+        now = rospy.Time.now().to_sec()
+        if delay <= 0:
+            while queue:
+                pub.publish(queue.popleft()[1])
+        else:
+            while queue and (now - queue[0][0]) >= delay:
+                pub.publish(queue.popleft()[1])
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    rospy.init_node('delay_node')
+    rospy.loginfo('delay_node gestartet')
+    pub = rospy.Publisher('/joy_delayed', Joy, queue_size=50)
+    rospy.Subscriber('/joy', Joy, callback, queue_size=50)
+    spin()

ExoMy_Software-master/src/joystick_parser_node.py

@@ -193,7 +193,7 @@ if __name__ == '__main__':
     rospy.init_node('joystick_parser_node')
     rospy.loginfo('joystick_parser_node started')
 
-    sub = rospy.Subscriber("/joy", Joy, callback, queue_size=1)
+    sub = rospy.Subscriber("/joy_delayed", Joy, callback, queue_size=1)
     pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
 
     rospy.spin()