Add double integrator

my_double_integrator/double_integrator.py

+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Vector3
+
+class DoubleIntegratorSim(Node):
+    def __init__(self):
+        super().__init__('double_integrator_sim')
+        self.declare_parameter('update_rate', 50.0)
+        self.update_rate = self.get_parameter('update_rate').value
+        self.state = Vector3(x=0.0, y=0.0, z=0.0)  # x=position, y=velocity, z=unused
+
+        self.control_sub = self.create_subscription(Float64, 'control_input', self.control_callback, 10)
+        self.state_pub = self.create_publisher(Vector3, 'state', 10)
+
+        self.control_input = 0.0  # Initialize control input
+
+        self.timer = self.create_timer(1.0 / self.update_rate, self.update_state)
+
+    def control_callback(self, msg):
+        self.control_input = msg.data
+
+    def update_state(self):
+        dt = 1.0 / self.update_rate
+        self.state.y += self.control_input * dt  # Update velocity
+        self.state.x += self.state.y * dt       # Update position
+        self.state_pub.publish(self.state)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DoubleIntegratorSim()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()