Added controller

my_double_integrator/controller.py

+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()