From baf9d1d9482191cc4dfee42b0eba8a51c84d2080 Mon Sep 17 00:00:00 2001
From: Lukas Krauss <uoypb@student.kit.edu>
Date: Wed, 4 Dec 2024 13:01:23 +0100
Subject: [PATCH] Added controller
---
my_double_integrator/controller.py | 43 ++++++++++++++++++++++++++++++
1 file changed, 43 insertions(+)
create mode 100644 my_double_integrator/controller.py
diff --git a/my_double_integrator/controller.py b/my_double_integrator/controller.py
new file mode 100644
index 0000000..5aba9f5
--- /dev/null
+++ b/my_double_integrator/controller.py
@@ -0,0 +1,43 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Vector3
+
+
+class Controller(Node):
+ def __init__(self):
+ super().__init__('controller')
+ self.declare_parameter('kp', 1.0)
+ self.kp = self.get_parameter('kp').value
+
+ self.desired_position = 0.0
+ self.current_position = 0.0
+
+ self.desired_pos_sub = self.create_subscription(Float64, 'desired_position', self.desired_position_callback, 10)
+ self.state_sub = self.create_subscription(Vector3, 'state', self.state_callback, 10)
+ self.control_pub = self.create_publisher(Float64, 'control_input', 10)
+
+ self.timer = self.create_timer(0.02, self.compute_control)
+
+ def desired_position_callback(self, msg):
+ self.desired_position = msg.data
+
+ def state_callback(self, msg):
+ self.current_position = msg.x
+
+ def compute_control(self):
+ error = self.desired_position - self.current_position
+ control_signal = self.kp * error
+ self.control_pub.publish(Float64(data=control_signal))
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = Controller()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
--
GitLab