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