From feaa1b5a10f0c0b86f92394858cd26eb3a795bda Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Lukas=20Krau=C3=9F?= <lukas.krauss@kabelbw.de>
Date: Wed, 4 Dec 2024 12:41:01 +0100
Subject: [PATCH] Add double integrator
---
my_double_integrator/double_integrator.py | 39 +++++++++++++++++++++++
1 file changed, 39 insertions(+)
diff --git a/my_double_integrator/double_integrator.py b/my_double_integrator/double_integrator.py
index e69de29..8fe4c9c 100644
--- a/my_double_integrator/double_integrator.py
+++ b/my_double_integrator/double_integrator.py
@@ -0,0 +1,39 @@
+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()
--
GitLab