Merge branch 'first_suggestion' into 'development'

Adding the position generator, controller and double integrator

See merge request !1

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

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

my_double_integrator/reference_position_generator.py

+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+
+
+class ReferencePositionGen(Node):
+    def __init__(self):
+        super().__init__('reference_position_gen')
+        self.declare_parameter('rate', 1.0)
+        self.declare_parameter('amplitude', 1.0)
+
+        self.rate = self.get_parameter('rate').value
+        self.amplitude = self.get_parameter('amplitude').value
+
+        self.reference_pub = self.create_publisher(Float64, 'desired_position', 10)
+        self.timer = self.create_timer(1.0 / self.rate, self.publish_reference)
+        self.time = 0.0
+
+    def publish_reference(self):
+        msg = Float64()
+        msg.data = self.amplitude * self.time  # Linearly increasing reference
+        self.reference_pub.publish(msg)
+        self.time += 1.0 / self.rate
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ReferencePositionGen()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()