diff --git a/launch.py b/launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..7cd3a6f24b9dc42ea49f99df227c9e72b800ff49
--- /dev/null
+++ b/launch.py
@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='my_double_integrator',
+            executable='double_integrator',
+            name='double_integrator_sim',
+            output='screen',
+            parameters=[{'update_rate': 50.0}]
+        ),
+        Node(
+            package='my_double_integrator',
+            executable='controller',
+            name='controller',
+            output='screen',
+            parameters=[{'kp': 1.0}]
+        ),
+        Node(
+            package='my_double_integrator',
+            executable='reference_position_generator',
+            name='reference_position_gen',
+            output='screen',
+            parameters=[{'rate': 1.0, 'amplitude': 1.0}]
+        ),
+    ])
diff --git a/my_double_integrator/controller.py b/my_double_integrator/controller.py
new file mode 100644
index 0000000000000000000000000000000000000000..5aba9f528bd48592bbe4fbc1066ae57cf26fd13c
--- /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()
diff --git a/my_double_integrator/double_integrator.py b/my_double_integrator/double_integrator.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..8fe4c9cb9e9180b81a7708ac785ad13b64668cd5 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()
diff --git a/my_double_integrator/reference_position_generator.py b/my_double_integrator/reference_position_generator.py
new file mode 100644
index 0000000000000000000000000000000000000000..6a438e9f7f71306ca12da0e9aba7b3c5f6b12c10
--- /dev/null
+++ b/my_double_integrator/reference_position_generator.py
@@ -0,0 +1,35 @@
+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()
diff --git a/setup.py b/setup.py
index 481b0baba03ed518b04f44d5d69c80c2fe1e2cf9..497beb30c725a8c89cc025da986a28e6d0bc5409 100644
--- a/setup.py
+++ b/setup.py
@@ -9,17 +9,20 @@ setup(
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['package.xml', 'launch.py']),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
     maintainer='silas',
     maintainer_email='silas@todo.todo',
-    description='TODO: Package description',
+    description='A double integrator simulation with ROS2 nodes',
     license='Apache-2.0',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'double_integrator = my_double_integrator.double_integrator:main',
+            'controller = my_double_integrator.controller:main',
+            'reference_position_generator = my_double_integrator.reference_position_generator:main',
         ],
     },
 )