class Controller(object): def __init__(self, *args, **kwargs): ... def control(self, *args, **kwargs): ... def reset(self): ... def pid_control(self, *args, **kwargs): ...
def control(self, twist_cmd, current_velocity, dbw_enabled): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = current_velocity.twist.linear.x vel_error = twist_cmd.twist.linear.x - current_vel throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if twist_cmd.twist.linear.x == 0. and current_vel < 0.1: throttle = 0 brake = 700 # N*m - to hold the car in place if we are stopped at a light. Acceleration ~ 1m/s^2 return throttle, brake, steeringThis is an example of the control method that takes in the velocity commands, the current velocity of the car, and whether the car is in drive-by-wire (dbw) mode. It first checks if the car is in dbw mode, if not, it resets the internal state and returns 0 for all commands. If the car is in dbw mode, it calculates the velocity error and uses the throttle PID controller to get the throttle command. It also calculates the brake command if necessary, and returns the steering, throttle, and brake commands. The twist_controller module is a package library in ROS.