def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
## MPC
self.mpc = MPC()
## PIDs
self.steering_pid = PID(P=0.34611, I=0.0370736, D=3.5349)
self.steering_pid.setSampleTime = 0.033
self.throttle_brake_pid = PID(P=7.0, I=1.0, D=1.026185)
self.throttle_brake_pid.setSampleTime = 0.033
## Pure Pursuit
self.pp = PP(L=4.5, k_pp=1.3)
self.stanley = STANLEY(L=4.5, k=4, k_soft=1)
def __init__(self):
rospy.init_node('dbw_node')
self.twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.vars = cutils.CUtils()
self._current_x = 0 #data.pose_x
self._current_y = 0 #data.pose_y
self._current_yaw = 0 #data.yaw
self._current_speed = 0 #data.speed
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = True
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = [] #way_list
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
# TODO: Create `TwistController` object
# TODO: Subscribe to all the topics you need to
#rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/final_waypoints', PoseArray,
self.update_waypoints_cb)
rospy.Subscriber('/imu_topic', Imu, self.current_pose_cb)
#rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
# go into event loop
self.loop()
def __init__(self, waypoints, control_method):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
self._kP = 0.8
self._kI = 0.5
self._kD = 0.0
self._Kpp = 4.5
self._Kvf = 2.5
self._Kcte = 0.5
self._Kmpc = 0.3
self._eps_lookahead = 10e-3
self._closest_distance = 0
self._wheelbase = 3.0
self._control_method = control_method
self._steering_diff = np.linspace(-10, 10, 21, endpoint=True)
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._lookahead_distance = 1.2
self._pure_pursuit_gain = 0.15
self._kps = 2.5
self._kds = 0.03
self._kis = 0.3
self._wheelbase = 3
self._uk1s = 0.0
self._er_1s = 0.0
self._er_2s = 0.0
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self.minidx = 0
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
# Definición de errores para el PID LONGITUDINAL
self.err2 = 0
self.err1 = 0
self.err = 0
self.u1 = 0
self.u = 0
# Definitions for the Stanley controller
self.cterr = 0
self.herr = 0
self.l = 2.3
self.steerout = 0
self.rx = waypoints[0][0]
self.ry = waypoints[0][1]
def __init__(self, waypoints, controller_type="MPC"):
self.vars = cutils.CUtils()
self._lookahead_distance = 3.0
self._lookahead_time = 1.0
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self.longitudinal_controller = LongitudinalPID(self._current_speed,
Kp=1.0,
Kd=0.1,
Ki=0.1,
integrator_max=10.0,
integrator_min=-10.0)
if controller_type == "PurePursuit":
from NavigationLibrary.controllers.PurePursuit import PurePursuit
self.lateral_controller = PurePursuit(self._current_x, self._current_y,
self._current_yaw, self._current_speed,
K=1.5)
elif controller_type == "StanleyController":
from NavigationLibrary.controllers.StanleyController import StanleyController
self.lateral_controller = StanleyController(self._current_x, self._current_y,
self._current_yaw, self._current_speed,
K=1.0)
elif controller_type == "MPC":
from NavigationLibrary.controllers.MPC import MPC
Q = np.eye(4)
R = 0.01*np.eye(2)
Qf = 5*np.eye(4)
Rd = np.eye(2)
self.controller = MPC(x=self._current_x, y=self._current_y, yaw=self._current_yaw,
v=self._current_speed, delta=0,
L=2, Q=Q, R=R, Qf=Qf, Rd=Rd,
len_horizon=10)
self.controller_type = controller_type
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
self._lot_controller = PID(Kp=60.0, Ki=0.6, Kd=0.001)
self._lot_controller.output_limits = (-100.0 ,100.0)
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
self.crosstrack_error = 0
self.k_e = 0.8
self.k_p = 1.908852977263873
self.k_i = 0.6316572093202273
self.k_d = 0.0001
self.total_v_error = 0
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._previous_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._previous_waypoint = waypoints[0]
self._current_waypoint = waypoints[1]
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
self._kp = 0.2
self._kd = 0.05
self._ki = 0.01
self._stanleygain = 2.5