def forward(self, throttle, steer_angle):
        """forward generates a forward-moving Control msg with throttle and steering angle

        :param throttle: throttle in range [-1.0, 1.0]
        :param steer_angle: steering angle in range [-1.0, 1.0]

        if throttle < 0.0 then apply brakes

        """
        assert steer_angle >= -1.0 and steer_angle <= 1.0, "Require steer angle to be in range [-1.0, 1.0]: {0}".format(
            steer_angle)
        brake = 0.0
        if throttle < 0.0:
            brake = abs(throttle)
            throttle = 0.0
        assert brake >= 0.0 and brake <= 1.0, "Brake must be in range [0.0, 1.0]: {0}".format(
            brake)
        assert throttle >= 0.0 and throttle <= 1.0, "Throttle must be in range [0.0, 1.0]: {0}".format(
            throttle)
        msg = Control()
        self.populate_header(msg)

        msg.throttle = throttle  # throttle as given
        msg.steer = steer_angle  # steering angle as given
        msg.brake = brake  # braking amount
        msg.shift_gears = 2  # forward gearing

        return msg
def vel_and_angle(data):
    global speed_real  #chamada para a variavel global
    global count
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo atualizado da leitura
    msg.header.frame_id = "base_link"
    msg.steer = data.angle * ANG_TO_RAD  #angulacao desejada para as rodas (0-30) GRAUS

    print "----START-vel_and_angle----------"
    print "vel=", data.velocity
    print "speed_real=", speed_real
    #A variavel data.velocity eh dada em km/h e speed_real eh definida em m/s
    msg.throttle, msg.brake = calc_throttle_brake(
        data.velocity, speed_real)  #funcao que calcul aceleracao e frenagem

    # if count < 100:
    # 	print "count: ",count
    # 	msg.throttle = 1.0
    # 	msg.brake = 0.0
    # else:
    # 	print "count: ",count
    # 	msg.throttle = 0.0
    #	msg.brake = 0.0
    #print "Throttle=",msg.throttle
    #print "brake=",msg.brake
    #print "angle=",data.angle
    #print "----END-vel_and_angle----------"
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
示例#3
0
def handle_incoming_car(msg):

    control = Control()
    control.shift_gears = 2

    print('Receiving command', msg)

    args = msg.split(',')
    speed_level = int(args[7])
    x = float(args[5])
    y = float(args[6])
    deg = xy2deg(x, y)

    delta = 15
    if (45 + delta) <= deg <= (135 - delta):  # forward
        control.throttle = 0.2 * speed_level
    elif (225 + delta) <= deg <= (315 - delta):  #backward
        control.brake = 1.0
    elif (135 - delta) <= deg <= (225 + delta):  #left
        control.throttle = 0.2 * speed_level
        control.steer = 0.2 * speed_level
        if deg >= 180:
            control.shift_gears = 3
    else:  #right
        control.throttle = 0.2 * speed_level
        control.steer = -0.2 * speed_level
        if 270 <= deg <= 360:
            control.shift_gears = 3

    vel_pub.publish(control)
    def __init__(self):

        self.bridge_object = CvBridge()
        self.image_sub = rospy.Subscriber("/prius/front_camera/image_raw",
                                          Image, self.camera_callback)
        self.cmd_vel_pub = rospy.Publisher('prius', Control, queue_size=20)
        self.twist = Control()
示例#5
0
    def drive(self):
        """
        return control object
        """
        if self.sign_detected == (0,0):
            rospy.loginfo("No sign detected, should be normal operation")
            # in the case where there is no sign, set normal values
            self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle= self.normal_throttle,brake= self.normal_brake, steer=0 ,shift_gears=self.normal_gears)

        else:
            rospy.loginfo("sign detected")
            area = self.sign_detected[1]
            matches = self.sign_detected[0]
            if self.control.brake == 1.0 and  
            if matches>0:
                # end all be all
                rospy.loginfo("Many matches, braking hard")
                self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=1.0 , steer=0 ,shift_gears=self.normal_gears )
            else:
                # handle the case where there is a sign
                rospy.loginfo("No matches, but area")
                if area > self.area_threshold_cut_throttle:
                    rospy.loginfo("cutting throttle")
                    self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=0 , steer=0 ,shift_gears=self.normal_gears)
                elif area > self.area_threshold_apply_brakes:
                    rospy.loginfo("applying brake")
                    self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake= 1.0, steer=0 ,shift_gears=self.normal_gears )

        self.prius_control_publisher.publish(self.control)

    def sign_callback(self, msg):
        """
        grab sign positions
        """
        self.sign_detected = (msg.matches, msg.area)

    def stop_the_fucking_car(self):
        rospy.loginfo("stopping the f*****g car")
        self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=1.0 , steer=0 ,shift_gears=self.normal_gears )
        self.prius_control_publisher.publish(self.control)

    def run(self):
        r = rospy.Rate(5)
        while not rospy.is_shutdown():
            self.drive()
            r.sleep()
def cmd_callback(data):
    global wheelbase
    global ackermann_cmd_topic
    global frame_id
    global pub
    global correct_v
    global correct_yaw
    global v
    global yaw
    global last_recorded_vel
    global last_recorded_ang
    global kp
    global ki
    global kd
    global dt
    global integral
    global last_error
    prius_vel = Control()
    target_vel = data.linear.x
    print("Command Velocity: "),
    if (abs(target_vel) < 0.2):
        target_vel = 0
        print("0.0")
    if (target_vel >= 0.2):
        prius_vel.shift_gears = 2
        print("+"),
        print(target_vel)
    if (target_vel <= -0.2):
        prius_vel.shift_gears = 3
        print(target_vel)

    integral = integral + ki * (target_vel - last_recorded_vel)
    integral = min(3, max(-3, integral))
    v = kp * (target_vel - last_recorded_vel) + integral + kd * (
        target_vel - last_recorded_vel - last_error)
    v = min(1, max(-1, v))
    last_error = (target_vel - last_recorded_vel)
    #v = data.linear.x + correct_v

    yaw = data.angular.z + correct_yaw
    steering = convert_trans_rot_vel_to_steering_angle(v, yaw, wheelbase)
    if (target_vel <= -0.2):
        v = v * -1.0
    # msg = AckermannDriveStamped()
    # msg.header.stamp = rospy.Time.now()
    # msg.header.frame_id = frame_id
    # msg.drive.steering_angle = steering
    # msg.drive.speed = v

    prius_vel.throttle = v
    prius_vel.brake = 0
    prius_vel.steer = steering / 3.14
    if (prius_vel.throttle < 0):
        prius_vel.brake = 1
    #pub.publish(msg)
    pub.publish(prius_vel)
示例#7
0
def vehicle_general_control():
    #TV data
    global longitude_TV
    global latitude_TV
    global heading_TV
    global speed_TV  #chamada para a variavel global
    global steering_TV  #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_TV  #percentual do acelerador (0 - 1)
    global brake_TV  #percentual do freio (0 - 1)

    #SV data
    global longitude_SV
    global latitude_SV
    global heading_SV
    global speed_SV  #chamada para a variavel global
    global steering_SV  #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_SV  #percentual do acelerador (0 - 1)
    global brake_SV  #percentual do freio (0 - 1)

    global publication_topic

    global erro_vel

    pub = rospy.Publisher(publication_topic, Control,
                          queue_size=10)  #topico de controle do carro
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo SVizado da leitura
    msg.header.frame_id = "base_link"
    msg.steer = 0 * ANG_TO_RAD  #angulacao desejada para as rodas (0-30) GRAUS

    #calcula distancia
    distance = ((longitude_TV - longitude_SV)**2 +
                (latitude_TV - latitude_SV)**2)**0.5
    #print "Distance", distance
    maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME
    distance_error = distance - maximum_distance
    print "------------------------------------------------------"
    #ajusta velocidade e aceleracao
    if (speed_TV >= EDGE_LOW_SPEED and distance_error >= 0.0):
        print "TV moving"
        #acao_velocidade = controla_velocidade (speed_TV, speed_SV)         #m/s
        acao_velocidade = distance_control(speed_TV, speed_SV, distance_error)
        msg.steer = direction_control(heading_TV, heading_SV)
        msg.throttle, msg.brake = calc_throttle_brake(acao_velocidade,
                                                      speed_TV)

    else:
        print "TV stopped"
        msg.throttle = 0.0
        msg.brake = 1.0
    #print "throttle", msg.throttle
    #print "brake", msg.brake
    #Atualiza topico carro
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
示例#8
0
def controla_veiculo():
    #dados do lider
    global longitude_leader
    global latitude_leader
    global heading_leader
    global speed_leader  #chamada para a variavel global
    global steering_leader  #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_leader  #percentual do acelerador (0 - 1)
    global brake_leader  #percentual do freio (0 - 1)

    #dados do veiculo
    global longitude_atual
    global latitude_atual
    global heading_atual
    global speed_atual  #chamada para a variavel global
    global steering_atual  #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_atual  #percentual do acelerador (0 - 1)
    global brake_atual  #percentual do freio (0 - 1)

    global publication_topic

    global erro_vel

    pub = rospy.Publisher(publication_topic, Control,
                          queue_size=10)  #topico de controle do carro
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo atualizado da leitura
    msg.header.frame_id = "base_link"
    msg.steer = 0 * ANG_TO_RAD  #angulacao desejada para as rodas (0-30) GRAUS

    #calcula distancia
    distance = ((longitude_leader - longitude_atual)**2 +
                (latitude_leader - latitude_atual)**2)**0.5
    #print "Distance", distance
    maximum_distance = MINIMUM_DISTANCE + speed_leader * 2
    #ajusta velocidade e aceleracao
    if (speed_leader >= EDGE_LOW_SPEED and distance >= MINIMUM_DISTANCE):
        print "Leader moving"
        acao_velocidade = controla_velocidade(speed_leader, speed_atual)  #m/s
        msg.throttle, msg.brake = calc_throttle_brake(acao_velocidade,
                                                      speed_leader)
    else:
        print "Leader stopped"
        msg.throttle = 0.0
        msg.brake = 1.0
    print "throttle", msg.throttle
    print "brake", msg.brake
    #ajusta heading
    #atualiza topico carro
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
def vel_and_angle(data):
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo atualizado da leitura
    msg.header.frame_id = "base_link"
    msg.throttle = data.velocity  #aceleracao desejada 	(0-1)
    #msg.brake = data.brake									#frenagem desejada 		(0-1)
    msg.steer = data.angle  #angulacao desejada para as rodas (0-30) GRAUS OU RADIANOS?
    if data.velocity == 0:
        msg.brake = 1
    print "vel=", data.velocity
    print "angle=", data.angle
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
def vel_and_angle(data):
    global speed_real  #chamada para a variavel global
    global count
    global car_name_TV

    var_print = OFF
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo atualizado da leitura
    msg.header.frame_id = "base_link"
    msg.steer = data.angle * ANG_TO_RAD  #angulacao desejada para as rodas (0-30) GRAUS

    if var_print:
        print "----START-vel_and_angle----------"
        print "vel: ", data.velocity
        print "speed_real: ", speed_real
        print "data_angle: ", data.angle
        print "steer: ", data.angle * ANG_TO_RAD

    if (data.angle * ANG_TO_RAD) > 0.05 and REDUCED_SPEED:
        #A variavel data.velocity eh dada em km/h e speed_real eh definida em m/s
        msg.throttle, msg.brake = calc_throttle_brake(
            VEL_FRENAGEM, speed_real)  #funcao que calcul aceleracao e frenagem
    else:
        #A variavel data.velocity eh dada em km/h e speed_real eh definida em m/s
        msg.throttle, msg.brake = calc_throttle_brake(
            data.velocity,
            speed_real)  #funcao que calcul aceleracao e frenagem

    # if count < 100:
    # 	print "count: ",count
    # 	msg.throttle = 1.0
    # 	msg.brake = 0.0
    # else:
    # 	print "count: ",count
    # 	msg.throttle = 0.0
    #	msg.brake = 0.0
    #print "Throttle=",msg.throttle
    #print "brake=",msg.brake
    #print "angle=",data.angle
    #print "----END-vel_and_angle----------"
    pub = rospy.Publisher(car_name_TV + 'prius', Control,
                          queue_size=10)  #topico de controle do carro
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
示例#11
0
    def sendCtrlCmdsToGazebo(self):
        """
        Send control commands to Gazebo, which updates the robot state according to its inertial model.
        """
        command_msg = Control()
        command_msg.header.stamp = rospy.get_rostime()
        command_msg.throttle = self.throttle
        command_msg.brake = 0.0
        command_msg.shift_gears = Control.FORWARD
        steer = self.steering
        command_msg.steer = steer
        self.control_pub.publish(command_msg)

        position_msg = PointStamped()
        position_msg.header.stamp = rospy.get_rostime()
        position_msg.point.x = self.lin_position
        self.linear_position_pub.publish(position_msg)
        return
示例#12
0
def control_test():
    pub = rospy.Publisher("/prius", Control, queue_size=1)
    rospy.init_node('sim_control', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    command = Control()
    command.header.stamp = rospy.Time.now()
    command.throttle = 0.2
    command.brake = 0
    command.steer = 0.0
    command.shift_gears = Control.NO_COMMAND   
    
   # while not rospy.is_shutdown():
    for i in range(10):
        pub.publish(command)
        rate.sleep()
    
    for i in range(10):
        command.brake = 0.5
        pub.publish(command)
        rate.sleep()
示例#13
0
def vel_and_angle(data):
	global speed_real										#chamada para a variavel global
	msg = Control()											#Define msg como um dado do tipo Control -> comando de controle do veiculo
	msg.header.stamp = rospy.Time.now();					#tempo atualizado da leitura
	msg.header.frame_id = "base_link";						
	#msg.throttle = data.velocity							#aceleracao desejada 	(0-1)
	#msg.brake = data.brake									#frenagem desejada 		(0-1)
	msg.steer = data.angle									#angulacao desejada para as rodas (0-30) GRAUS OU RADIANOS?
	
	print "------------------------------------------"
	print "vel=", data.velocity
	print "speed_real=", speed_real
	
	"""
	erro_vel = data.velocity/3.6 - speed_real
	print "erro_vel=", erro_vel
	erro_vel_norm = (erro_vel - MIN_SPEED) / (MAX_SPEED - MIN_SPEED)
	print "erro_vel_norm=", erro_vel_norm
	accel_control = min(erro_vel_norm,1.0)
	print "accel_control=", accel_control
	accel_control = max(accel_control,-1.0)
	print "accel_control=", accel_control

	if(abs(accel_control) > 0.05):
		msg.throttle = max(0.0,accel_control)
		msg.brake = max(0.0,-accel_control)
	elif (data.velocity == 0.0):
		msg.throttle = 0.0
		msg.brake = 1.0
	"""
	msg.throttle, msg.brake = calc_throttle_brake (data.velocity, speed_real)
	
	#if data.velocity == 0:
	#	msg.brake = 1
	
	print "Throttle=",msg.throttle
	print "brake=",msg.brake 
	#print "angle=",data.angle
	pub.publish(msg)										#publica a mensagem desejada no topico 'car1/prius'
示例#14
0
def prius_pub(data):
    '''
	publishes the velocity and steering angle
	published on topic : ackermann_cmd_topic
	'''
    global prius_vel
    prius_vel = Control()

    if (data.linear.x > 0):
        prius_vel.throttle = data.linear.x / 100
        prius_vel.brake = 0
        print("acc")
        print(prius_vel.throttle)

    if (data.linear.x < 0):
        prius_vel.brake = -data.linear.x / 100
        prius_vel.throttle = 0
        print("brake")
        print(prius_vel.brake)

    prius_vel.steer = data.angular.z / 30

    pub.publish(prius_vel)
    def callback(self, message):
        rospy.logdebug("joy_translater received axes %s", message.axes)
        command = Control()
        command.header = message.header
        if message.axes[THROTTLE_AXIS] >= 0:
            command.throttle = message.axes[THROTTLE_AXIS]
            command.brake = 0.0
        else:
            command.brake = message.axes[THROTTLE_AXIS] * -1
            command.throttle = 0.0

        if message.buttons[3]:
            command.shift_gears = Control.FORWARD
        elif message.buttons[1]:
            command.shift_gears = Control.NEUTRAL
        elif message.buttons[0]:
            command.shift_gears = Control.REVERSE
        else:
            command.shift_gears = Control.NO_COMMAND

        command.steer = message.axes[STEERING_AXIS]
        self.last_published = message
        self.pub.publish(command)
示例#16
0
    def callback(self, message):
        throttle_value = message.linear.x
        gears_value = message.linear.z
        steering_value = message.angular.z

        rospy.logdebug("joy_translater received Throttle value " +
                       str(throttle_value))
        rospy.logdebug("joy_translater received Gears value " +
                       str(gears_value))
        rospy.logdebug("joy_translater received Steering value " +
                       str(steering_value))
        command = Control()

        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()

        command.header = header
        if throttle_value >= 0:
            command.throttle = throttle_value
            command.brake = 0.0
        else:
            command.brake = throttle_value * -1
            command.throttle = 0.0

        if gears_value > 0.0:
            command.shift_gears = Control.FORWARD
        elif gears_value == 0.0:
            command.shift_gears = Control.NEUTRAL
        elif gears_value < 0.0:
            command.shift_gears = Control.REVERSE
        else:
            command.shift_gears = Control.NO_COMMAND

        command.steer = steering_value
        self.last_published = message
        self.pub.publish(command)
示例#17
0
def callback_path(data):
    '''
    calculates target path point and the steering angle
    :param data [Path]
    :param ep [float]
    :param cp [int]
    '''
    global ep  # min distance
    global cp  # index of closest point
    global ep_max
    global ep_sum
    global ep_avg
    global n
    global cp1
    global path_length

    cross_err = Twist()
    x_p = data
    # calculate minimum distance
    calc_path_length(x_p)

    distances = []
    for i in range(len(x_p.poses)):
        a = x_p.poses[i]
        distances += [dist(a, x_bot, y_bot)]
    ep = min(distances)
    ep1 = ep

    if (ep > ep_max):
        ep_max = ep

    n = n + 1
    ep_sum = ep_sum + ep
    ep_avg = ep_sum / n

    cp = distances.index(ep)
    cp1 = cp
    cross2 = [(x_bot - data.poses[cp1].pose.position.x),
              (y_bot - data.poses[cp1].pose.position.y)]
    cross = [math.cos(yaw), math.sin(yaw)]
    cross_prod = cross[0] * cross2[1] - cross[1] * cross2[0]
    if (cross_prod > 0):
        ep1 = -ep1

    print 'ep_sum: ', ep_sum
    print 'ep_avg: ', ep_avg
    cross_err.linear.x = ep1
    cross_err.angular.x = ep_max
    cross_err.angular.y = ep_avg

    print 'old index:', cp
    # calculate index of target point on path
    cmd = Twist()
    cmd1 = Twist()
    prius_vel = Control()
    L = 0
    Lf = k * max_vel + d_lookahead

    while Lf > L and (cp + 1) < len(x_p.poses):
        dx = data.poses[cp + 1].pose.position.x - \
            data.poses[cp].pose.position.x
        dy = data.poses[cp + 1].pose.position.y - \
            data.poses[cp].pose.position.y
        L += math.sqrt(dx ** 2 + dy ** 2)
        cp = cp + 1
    print len(x_p.poses)
    print 'new index is:', cp

    goal_point = [x_p.poses[cp].pose.position.x,
                  x_p.poses[cp].pose.position.y]
    print 'current goal is:', goal_point
    error = [goal_point[0] - x_bot, goal_point[1] - y_bot]
    print error
    steer_angle = pure_pursuit(goal_point)

    siny = +2.0 * (x_p.poses[cp].pose.orientation.w *
                   x_p.poses[cp].pose.orientation.z +
                   x_p.poses[cp].pose.orientation.x *
                   x_p.poses[cp].pose.orientation.y)

    cosy = +1.0 - 2.0 * (x_p.poses[cp].pose.orientation.y *
                         x_p.poses[cp].pose.orientation.y +
                         x_p.poses[cp].pose.orientation.z *
                         x_p.poses[cp].pose.orientation.z)

    steer_path = math.atan2(siny, cosy)
    steer_err = (yaw - steer_path)
    cross_err.linear.y = (-1) * (yaw - steer_path)

    print "steer_angle :", steer_angle * 180 / math.pi
    cmd.angular.z = min(30, max(-30, steer_angle * 180 / math.pi))
    cmd.linear.y = math.sqrt(error[0]**2 + error[1]**2)
    print 'omega:', cmd.angular.z
    cross_err.linear.z = path_length[cp]

    #r = rospy.Rate(100)
    # while not rospy.is_shutdown():
    pub1.publish(cmd)
    pub2.publish(cross_err)
    #	r.sleep()

    print "cmd published"

    # print (ep)
    print x_p.poses[cp].pose.orientation
示例#18
0
def vehicle_general_control():
    #TV data
    global longitude_TV
    global latitude_TV
    global heading_TV
    global speed_TV							                #chamada para a variavel global
    global steering_TV                                       #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_TV                                       #percentual do acelerador (0 - 1)
    global brake_TV                                          #percentual do freio (0 - 1)
    global speed_TV_reference
    #SV data
    global longitude_SV
    global latitude_SV
    global heading_SV
    global speed_SV							                #chamada para a variavel global
    global steering_SV                                       #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
    global throttle_SV                                       #percentual do acelerador (0 - 1)
    global brake_SV                                          #percentual do freio (0 - 1)

    global theta_error

    global publication_error_topic
    global publication_topic
    global erro_vel
    global TV_position_vector                               #vector to store TV positions to be followed
    global mode

    global incremental_controller
    global TV_error_vector
    global TV_PID_error
    global TV_THETA_error

    debug = ON

    pub = rospy.Publisher(publication_topic, Control, queue_size=10)				#topico de controle do carro
    msg = Control()											                        #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now();					                        #tempo atualizado da leitura
    msg.header.frame_id = "base_link";						
    msg.steer = 0 * ANG_TO_RAD						                                #angulacao desejada para as rodas (0-20) GRAUS

    #print "------------------------------------------------------"
    #calcula distancia
    distance = ((longitude_TV - longitude_SV)**2 + (latitude_TV - latitude_SV)**2)**0.5                                 #real distance between TV and SV
    if abs(speed_TV - speed_TV_reference) > 1:
        speed_TV_reference = speed_TV
    maximum_distance = MINIMUM_DISTANCE + speed_TV_reference * SAFETY_TIME                                              #desisred distance
    #maximum_distance = 10
    distance_error = distance - maximum_distance                                                                        #error between desired ditance and real distance
    if abs(distance_error) < 2.0:
        distance_error = 0.0
    elif distance_error > 0:
        distance_error = distance_error - 2.0
    else:
        distance_error = distance_error + 2.0
    pid_error = distance_error

    if debug:
        #print "TV 1: ", longitude_TV, latitude_TV, heading_TV, speed_TV
        #print "SV: ", longitude_SV, latitude_SV, heading_SV, speed_SV
        print "Distance", distance
        print "maximum_distance: ", maximum_distance
        print "Distance Error: ", distance_error
    if (TV_position_vector[0,0]<=0.1 and TV_position_vector[0,1]<=0.1):                                                 #cleaning the first position of the vector
        TV_position_vector = np.append(TV_position_vector,[[latitude_TV, longitude_TV, heading_TV, speed_TV]],axis=0)   #Numpy vector que armazena posicoes de TV
        TV_position_vector = TV_position_vector[1:,:]                                                                   #delete the first line with empty values
        if (incremental_controller):
            TV_error_vector = np.append(TV_error_vector,[[TV_PID_error, TV_THETA_error]],axis=0)
            TV_error_vector = TV_error_vector[1:,:]
    else:
        TV_position_vector = np.append(TV_position_vector,[[latitude_TV, longitude_TV, heading_TV, speed_TV]],axis=0)   #Numpy vector que armazena posicoes de TV
        if (incremental_controller):
            TV_error_vector = np.append(TV_error_vector,[[TV_PID_error, TV_THETA_error]],axis=0)
        
    #if debug:
        #print "TV_position_vector shape: ", TV_position_vector.shape
        #print "Speed_TV: ", speed_TV
        #print TV_position_vector
    safety_dist = ((speed_SV*3.6)*(speed_SV*3.6))/(250*0.75)           #determine the safety distance considering a Mi = 1.0(coeficiente de atrito)
    if debug:
        print "Safety Distance: ", safety_dist

    #ajusta velocidade e aceleracao
    if brake_TV == 1.0:
        msg.throttle, msg.brake = 0.0 , 0.1                        #determine throttle and brake
        msg.steer = steering_SV
        if debug:
            print "TV Brake "
    #elif ((distance < safety_dist and speed_SV > 0.5) or distance < MINIMUM_DISTANCE):
    elif (distance < MINIMUM_DISTANCE):
        msg.throttle, msg.brake = 0.0 , 0.7                        #determine throttle and brake
        msg.steer = steering_SV
        if debug:
            print "Safety Distance: ", safety_dist
            print "Distance Error: ", distance_error
            #print "maximun distance: ", maximum_distance
    elif (speed_TV >= EDGE_LOW_SPEED and distance >= MINIMUM_DISTANCE):               #TV is moving and the distance is significative
        if (distance < safety_dist and speed_SV > 0.5):
            distance_error = distance - safety_dist
        if incremental_controller:
            PID_dist = distance_control (distance_error + TV_PID_error)                             #determines de control action for the distance
        else:
            PID_dist = distance_control (distance_error)                                                #determines de control action for the distance
        lat_compare, long_compare, heading_compare, speed_compare, TV_dist_acum, TV_head_acum = compare_lost_position()         #compares the SV_position with the vector of positions
        new_speed_SV = speed_compare + PID_dist                                                     #defines the new speed for the SV
        # if debug:
        #     print "TV Moving!"
        #     print "speed_compare: ", speed_compare
        #     print "new Speed_SV: ", new_speed_SV
        msg.throttle, msg.brake = calc_throttle_brake(new_speed_SV,speed_SV)                        #determine throttle and brake
        msg.steer = direction_control (lat_compare, long_compare, heading_compare + TV_head_acum, heading_SV)

        # if (new_speed_SV > speed_TV * 1.5) and distance < maximum_distance:
        #     new_speed_SV = max(50.0/3.6,speed_TV)
        #     print "REDUCED SPEED"
        #     print "new Speed_SV: ", new_speed_SV
        #     msg.throttle, msg.brake = calc_throttle_brake(new_speed_SV,speed_SV)                        #determine throttle and brake

    else:
        msg.throttle, msg.brake = 0.0 , 0.5                        #determine throttle and brake
        msg.steer = steering_SV
        if debug:
            if speed_TV < EDGE_LOW_SPEED:
                print "Speed < EDGE_LOW_SPEED"
            elif distance <= MINIMUM_DISTANCE:
                print "Distance < MINIMUM_distance"
            else:
                print "ERROR!!!" 

    # print "throttle: ", msg.throttle
    # print "brake: ", msg.brake
    # print "steer: ", msg.steer
    if debug:
        print "------------------------------"
    #Atualiza topico carro
    pub.publish(msg)										#publica a mensagem desejada no topico 'car1/prius'
    
    #publishing error messages
    pub_error = rospy.Publisher(publication_error_topic, error_control, queue_size=1) 	#parametros de erro para controle e impressao
    msg_error = error_control() 
    msg_error.pid_error_value = pid_error
    msg_error.theta_error_value = theta_error
    pub_error.publish(msg_error)
示例#19
0
left = 0

stdscr = curses.initscr()
curses.cbreak()
stdscr.keypad(1)
rospy.init_node('keyboard_talker', anonymous=True)
self.pub = rospy.Publisher('car_1/prius', Control, queue_size=1)

stdscr.refresh()

key = ''
while key != ord('q'):
    key = stdscr.getch()
    stdscr.refresh()

    command = Control()

    # fill in the conditions to increment/decrement throttle/steer

    if key == curses.KEY_UP:
        command.throttle = message.axes[THROTTLE_AXIS]
        command.brake = 0.0
        command.shift_gears = Control.FORWARD
    elif key == curses.KEY_DOWN:
        forward = forward - 1
    elif key == curses.KEY_LEFT:
        left = left + 1
    elif key == curses.KEY_RIGHT:
        left = left - 1
    elif key == ord(' '):
        # this key will center the steer and throttle
示例#20
0
 def stop_the_fucking_car(self):
     rospy.loginfo("stopping the f*****g car")
     self.control = Control(header=Header(stamp=rospy.Time.now(),frame_id=''), throttle=0 ,brake=1.0 , steer=0 ,shift_gears=self.normal_gears )
     self.prius_control_publisher.publish(self.control)
示例#21
0
def callback_feedback(data):
    '''
	Assigns the position of the robot to global variables from odometry and
	calculates target path point and the steering angle
	:param x_bot [float]
	:param y_bot [float]
	:param yaw [float]
	:param vel [float]
	:param data [Path]
	:param ep [float]
	:param cp [int]
	'''
    global x_bot
    global y_bot
    global yaw
    global vel
    global ep  # min distance
    global cp  # index of closest point
    global ep_max
    global ep_sum
    global ep_avg
    global n
    global cp1
    global path_length
    global x_p

    x_bot = data.pose.pose.position.x
    y_bot = data.pose.pose.position.y
    # quarternion to euler conversion
    siny = +2.0 * (
        data.pose.pose.orientation.w * data.pose.pose.orientation.z +
        data.pose.pose.orientation.x * data.pose.pose.orientation.y)
    cosy = +1.0 - 2.0 * (
        data.pose.pose.orientation.y * data.pose.pose.orientation.y +
        data.pose.pose.orientation.z * data.pose.pose.orientation.z)
    yaw = math.atan2(siny, cosy)  # yaw in radians

    # printing the odometry readings
    print("x of car:", str(x_bot))
    print('y of car:', str(y_bot))
    print('angle of car:', str(yaw))
    print('vel of car:', str(data.twist.twist.linear.x),
          str(data.twist.twist.linear.y), str(data.twist.twist.linear.z))
    print('c')

    cross_err = Twist()
    calc_path_length(x_p)
    data1 = x_p

    distances = []
    for i in range(len(x_p.poses)):
        a = x_p.poses[i]
        distances += [dist(a, x_bot, y_bot)]
    ep = min(distances)
    ep1 = ep

    if (ep > ep_max):
        ep_max = ep

    n = n + 1
    ep_sum = ep_sum + ep
    ep_avg = ep_sum / n

    cp = distances.index(ep)
    cp1 = cp
    cross2 = [(x_bot - data1.poses[cp1].pose.position.x),
              (y_bot - data1.poses[cp1].pose.position.y)]
    cross = [math.cos(yaw), math.sin(yaw)]
    cross_prod = cross[0] * cross2[1] - cross[1] * cross2[0]
    if (cross_prod > 0):
        ep1 = -ep1

    print('ep_sum: ', str(ep_sum))
    print('ep_avg: ', str(ep_avg))
    cross_err.linear.x = ep1
    cross_err.angular.x = ep_max
    cross_err.angular.y = ep_avg

    print('old index:', str(cp))
    # calculate index of target point on path
    cmd = Twist()
    cmd1 = Twist()
    prius_vel = Control()
    L = 0
    Lf = k * max_vel + d_lookahead

    while Lf > L and (cp + 1) < len(x_p.poses):
        dx = data1.poses[cp + 1].pose.position.x - \
         data1.poses[cp].pose.position.x
        dy = data1.poses[cp + 1].pose.position.y - \
         data1.poses[cp].pose.position.y
        L += math.sqrt(dx**2 + dy**2)
        cp = cp + 1
    print(len(x_p.poses))
    print('new index is:', str(cp))

    goal_point = [x_p.poses[cp].pose.position.x, x_p.poses[cp].pose.position.y]
    print('current goal is:', str(goal_point))
    error = [goal_point[0] - x_bot, goal_point[1] - y_bot]
    print(error)
    steer_angle = pure_pursuit(goal_point)

    siny = +2.0 * (
        x_p.poses[cp].pose.orientation.w * x_p.poses[cp].pose.orientation.z +
        x_p.poses[cp].pose.orientation.x * x_p.poses[cp].pose.orientation.y)

    cosy = +1.0 - 2.0 * (
        x_p.poses[cp].pose.orientation.y * x_p.poses[cp].pose.orientation.y +
        x_p.poses[cp].pose.orientation.z * x_p.poses[cp].pose.orientation.z)

    steer_path = math.atan2(siny, cosy)
    steer_err = (yaw - steer_path)
    cross_err.linear.y = (-1) * (yaw - steer_path)

    print("steer_angle :", str(steer_angle * 180 / math.pi))
    cmd.angular.z = min(30, max(-30, steer_angle * 180 / math.pi))
    cmd.linear.y = math.sqrt(error[0]**2 + error[1]**2)
    print('omega:', str(cmd.angular.z))
    cross_err.linear.z = path_length[cp]

    pub1.publish(cmd)
    pub2.publish(cross_err)

    print("cmd published")

    # print (ep)
    print(x_p.poses[cp].pose.orientation)
示例#22
0
def vehicle_general_control():
    #TV data
    global longitude_TV
    global latitude_TV
    global heading_TV
    global speed_TV  #chamada para a variavel global
    global steering_TV  #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_TV  #percentual do acelerador (0 - 1)
    global brake_TV  #percentual do freio (0 - 1)

    #SV data
    global longitude_SV
    global latitude_SV
    global heading_SV
    global speed_SV  #chamada para a variavel global
    global steering_SV  #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
    global throttle_SV  #percentual do acelerador (0 - 1)
    global brake_SV  #percentual do freio (0 - 1)

    global publication_topic
    global erro_vel
    global TV_position_vector

    var_print = 1

    pub = rospy.Publisher(publication_topic, Control,
                          queue_size=10)  #topico de controle do carro
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo SVizado da leitura
    msg.header.frame_id = "base_link"
    msg.steer = 0 * ANG_TO_RAD  #angulacao desejada para as rodas (0-30) GRAUS

    print "------------------------------------------------------"
    #calcula distancia
    distance = (
        (longitude_TV - longitude_SV)**2 +
        (latitude_TV - latitude_SV)**2)**0.5  #real distance between TV and SV
    maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME
    distance_error = distance - maximum_distance

    if var_print:
        print "Distance", distance
        print "maximum_distance: ", maximum_distance
        print "Distance Error: ", distance_error

    TV_position_vector = np.append(
        TV_position_vector,
        [[latitude_TV, longitude_TV, heading_TV, speed_TV]],
        axis=0)  #Numpy vector que armazena posicoes de TV
    if var_print:
        print "TV_position_vector shape: ", TV_position_vector.shape
    #print TV_position_vector

    #ajusta velocidade e aceleracao
    if (speed_TV >= EDGE_LOW_SPEED and distance >= MINIMUM_DISTANCE):
        if var_print:
            print "TV Moving!"
        PID_dist = distance_control(
            distance_error)  #determines de control action for the distance
        heading_compare, speed_compare = compare_position(
        )  #compares the SV_position with the vector of positions
        new_speed_SV = speed_compare + PID_dist  #defines the new speed for the SV
        msg.throttle, msg.brake = calc_throttle_brake(
            new_speed_SV, speed_SV)  #determine throttle and brake

        if (heading_compare <> heading_SV):
            msg.steer = direction_control(heading_compare, heading_SV)

    #print "throttle", msg.throttle
    #print "brake", msg.brake
    #Atualiza topico carro
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
示例#23
0
                        data.pose.pose.orientation.z)
    yaw = math.atan2(siny, cosy)
    last_recorded_vel = data.twist.twist.linear.x * \
        math.cos(yaw) + data.twist.twist.linear.y * math.sin(yaw)
    act_vel_can = last_recorded_vel
    #print("Real Vel %f" %act_vel_can)


def pid_callback(data):
	'''
	for subscribing to topic "pid_output"
	sets velocity using geometry_msgs:Twist
	'''
    global prius_vel
    global act_vel_can
    prius_vel = Control()

    if(data.linear.x > 0):
        prius_vel.throttle = data.linear.x / 100
        prius_vel.brake = 0
        print ("acc")
        print (prius_vel.throttle)

    if(data.linear.x < 0):
        prius_vel.brake = -data.linear.x / 100
        prius_vel.throttle = 0
        print ("brake")
        print (prius_vel.brake)

    prius_vel.steer = data.angular.z / 30
    print "steering:", prius_vel.steer
示例#24
0
def vehicle_general_control():
    #TV data
    global longitude_TV
    global latitude_TV
    global heading_TV
    global speed_TV  #chamada para a variavel global
    global steering_TV  #steering (angulacao das rodas do veiculo em relacao ao veiculo)
    global throttle_TV  #percentual do acelerador (0 - 1)
    global brake_TV  #percentual do freio (0 - 1)

    #SV data
    global longitude_SV
    global latitude_SV
    global heading_SV
    global speed_SV  #chamada para a variavel global
    global steering_SV  #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
    global throttle_SV  #percentual do acelerador (0 - 1)
    global brake_SV  #percentual do freio (0 - 1)

    global publication_topic
    global erro_vel
    global TV_position_vector

    var_print = 1

    pub = rospy.Publisher(publication_topic, Control,
                          queue_size=10)  #topico de controle do carro
    msg = Control(
    )  #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now()
    #tempo atualizado da leitura
    msg.header.frame_id = "base_link"
    msg.steer = 0 * ANG_TO_RAD  #angulacao desejada para as rodas (0-20) GRAUS

    print "------------------------------------------------------"
    #calcula distancia
    distance = (
        (longitude_TV - longitude_SV)**2 +
        (latitude_TV - latitude_SV)**2)**0.5  #real distance between TV and SV
    maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME  #desisred distance
    distance_error = distance - maximum_distance  #error between desired ditance and real distance

    if var_print:
        print "Distance", distance
        print "maximum_distance: ", maximum_distance
        print "Distance Error: ", distance_error
    if (TV_position_vector[0, 0] <= 0.1 and TV_position_vector[0, 1] <=
            0.1):  #cleaning the first position of the vector
        TV_position_vector = np.append(
            TV_position_vector,
            [[latitude_TV, longitude_TV, heading_TV, speed_TV]],
            axis=0)  #Numpy vector que armazena posicoes de TV
        TV_position_vector = TV_position_vector[
            1:, :]  #delete the first line with empty values
    else:
        TV_position_vector = np.append(
            TV_position_vector,
            [[latitude_TV, longitude_TV, heading_TV, speed_TV]],
            axis=0)  #Numpy vector que armazena posicoes de TV

    if var_print:
        print "TV_position_vector shape: ", TV_position_vector.shape
    #print TV_position_vector

    #ajusta velocidade e aceleracao
    if (speed_TV >= EDGE_LOW_SPEED and distance >=
            MINIMUM_DISTANCE):  #TV is moving and the distance is significative
        if var_print:
            print "TV Moving!"
        PID_dist = distance_control(
            distance_error)  #determines de control action for the distance
        heading_compare, speed_compare = compare_position(
        )  #compares the SV_position with the vector of positions
        if mode == NORMAL_MODE:  #NORMAL MODE (platoon)
            if var_print:
                "NORMAL MODE"
            new_speed_SV = speed_compare + PID_dist  #defines the new speed for the SV
            if var_print:
                print "new_speed_SV: ", new_speed_SV
            msg.throttle, msg.brake = calc_throttle_brake(
                new_speed_SV, speed_SV)  #determine throttle and brake
            if (heading_compare <> heading_SV):
                msg.steer = direction_control(heading_compare, heading_SV)
        else:  #LOST MODE (OUT OF REACH)
            if var_print:
                "LOST MODE ON!!!"
            heading_compare, speed_compare = compare_lost_position(
            )  #compares the SV_position with the TV_position
            new_speed_SV = speed_compare + PID_dist
            if var_print:
                print "new_speed_SV: ", new_speed_SV
            msg.throttle, msg.brake = calc_throttle_brake(
                new_speed_SV, speed_SV)
            if (heading_compare <> heading_SV):
                msg.steer = direction_control(heading_compare, heading_SV)
    else:
        print "Distance < MINIMUM_distance"

    #TODO in order to test, force the steer to be the same as the TV
    #msg.steer = steering_TV

    print "throttle: ", msg.throttle
    print "brake: ", msg.brake
    print "steer: ", msg.steer
    #Atualiza topico carro
    pub.publish(msg)  #publica a mensagem desejada no topico 'car1/prius'
示例#25
0
    global longitude_SV
    global latitude_SV
    global heading_SV
    global speed_SV							                #chamada para a variavel global
    global steering_SV                                       #steering (angulacao das rodas do veiculo em relacao ao veiculo), speed_TV
    global throttle_SV                                       #percentual do acelerador (0 - 1)
    global brake_SV                                          #percentual do freio (0 - 1)

    global publication_topic
    global erro_vel
    global TV_position_vector                               #vector to store TV positions to be followed

    debug = ON

    pub = rospy.Publisher(publication_topic, Control, queue_size=10)				#topico de controle do carro
    msg = Control()											                        #Define msg como um dado do tipo Control -> comando de controle do veiculo
    msg.header.stamp = rospy.Time.now();					                        #tempo atualizado da leitura
    msg.header.frame_id = "base_link";						
    msg.steer = 0 * ANG_TO_RAD						                                #angulacao desejada para as rodas (0-20) GRAUS

    print "------------------------------------------------------"
    #calcula distancia
    distance = ((longitude_TV - longitude_SV)**2 + (latitude_TV - latitude_SV)**2)**0.5                                 #real distance between TV and SV
    maximum_distance = MINIMUM_DISTANCE + speed_TV * SAFETY_TIME                                                        #desisred distance
    distance_error = distance - maximum_distance                                                                        #error between desired ditance and real distance

    if debug:
    #    print "Distance", distance
    #    print "maximum_distance: ", maximum_distance
        print "Distance Error: ", distance_error
    if (TV_position_vector[0,0]<=0.1 and TV_position_vector[0,1]<=0.1):                                                 #cleaning the first position of the vector
示例#26
0
    def joystick_callback(self, message):
        command = Control()

        # Set message header
        command.header = message.header

        # Get steering value
        command.steer = message.axes[self._STEERING_AXIS]
        # Set GUI steering value
        self._steering_scrollbar_signal.emit(
            int(message.axes[self._STEERING_AXIS] * -100))

        # Get cruise control state
        if message.buttons[self._CIRCLE] and self._circle_flag:
            self._circle_flag = False
            self._cruise_control_state = not self._cruise_control_state
            self._cruise_radiobutton_signal.emit(self._cruise_control_state)
        elif not message.buttons[self._CIRCLE]:
            self._circle_flag = True

        # Increment cruise control speed
        if message.buttons[self._R1] and self._R1_flag:
            self._R1_flag = False
            if self._cruise_control_speed < self._MAX_SPEED:
                self._cruise_control_speed += 1
                self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
        elif not message.buttons[self._R1]:
            self._R1_flag = True

        # Decrement cruise control speed
        if message.buttons[self._L1] and self._L1_flag:
            self._L1_flag = False
            if self._cruise_control_speed > 0:
                self._cruise_control_speed -= 1
                self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
        elif not message.buttons[self._L1]:
            self._L1_flag = True

        # If cruise control was on then
        if self._cruise_control_state:
            command.shift_gears = Control.FORWARD
            self._gears_lineedit_signal.emit('D')

            # Calculate the safe distance which prevents collision
            safe_velocity = np.sqrt(
                2 * self._AMAX * self._closest_distance) * 3.6
            # Get the minimum of the safe distance or the speed desired by the driver
            desired_velocity = min(self._cruise_control_speed, safe_velocity)

            # PID loop
            t_current = time.time()

            dt = t_current - self._t_previous

            v_current_error = desired_velocity - self._current_velocity
            self._v_total_error += v_current_error * dt
            v_error_rate = (v_current_error - self._v_previous_error) / dt

            p_throttle = self._KP * v_current_error
            i_throttle = self._KI * self._v_total_error
            d_throttle = self._KD * v_error_rate

            longitudinal_output = p_throttle + i_throttle + d_throttle

            if longitudinal_output >= 0:
                command.throttle = np.fmax(np.fmin(longitudinal_output, 1.0),
                                           0.0)
                command.brake = 0.0
                self._throttle_scrollbar_signal.emit(
                    int(command.throttle * -100))
            else:
                command.throttle = 0.0
                command.brake = np.fmax(np.fmin(-longitudinal_output, 1.0),
                                        0.0)
                self._throttle_scrollbar_signal.emit(int(command.brake * 100))

            self._v_previous_error = v_current_error
            self._t_previous = t_current
        else:
            # Reset variables
            self._v_total_error = 0
            self._v_previous_error = 0
            self._t_previous = 0

            self._closest_distance = float('inf')

            # Get throttle/breaking value
            if message.axes[self._THROTTLE_AXIS] >= 0:
                command.throttle = message.axes[self._THROTTLE_AXIS]
                command.brake = 0.0
            else:
                command.brake = message.axes[self._THROTTLE_AXIS] * -1
                command.throttle = 0.0

            # Set GUI throttle value
            self._throttle_scrollbar_signal.emit(
                int(message.axes[self._THROTTLE_AXIS] * -100))

            # Get gears value
            if message.buttons[self._TRIANGLE]:
                command.shift_gears = Control.FORWARD
                self._gears_lineedit_signal.emit('D')
            elif message.buttons[self._CROSS]:
                command.shift_gears = Control.NEUTRAL
                self._gears_lineedit_signal.emit('N')
            elif message.buttons[self._SQUARE]:
                command.shift_gears = Control.REVERSE
                self._gears_lineedit_signal.emit('R')
            else:
                command.shift_gears = Control.NO_COMMAND

        self._t_previous = time.time()

        # Send control message
        try:
            self._prius_publisher.publish(command)
            rospy.loginfo("Published commands")
        except Exceptionrospy.ROSInterruptException as e:
            pass

        self._last_published = message
示例#27
0
    def __init__(self):
        # Initiate variables
        self.control_type = rospy.get_param(
            '~lat_control', 'stanley')  # Default is stanley controller
        rospy.loginfo("Setting up Control Node")
        rospy.loginfo("Lateral Control Type: %s", self.control_type)
        rospy.sleep(1)
        self.goalMarker = Marker()
        self.freq = 1 / 30.0  # 30 hz
        self.velocity = 0
        self.angular_velocity = 0
        self.xpos = 0
        self.ypos = 0
        self.roll = 0
        self.yaw = 0
        self.prev_the = 0
        self.prev_e_lat = 0
        self.pitch = 0
        self.steer_cmd = 0.0
        self.waypoint_path = Path()
        self.waypoint_path_cubic_spline = Path()
        self.path_changed = True
        self.prev_cmd = 0.0

        # Publishers
        self.goalMarkerPub = rospy.Publisher("~goal", Marker, queue_size=10)
        self.cmdPub = rospy.Publisher(
            "/prius",
            Control,
            latch=True,
        )

        # Subscribers
        rospy.Subscriber("/base_pose_ground_truth", Odometry,
                         self.odomCallback)
        rospy.Subscriber("/waypoints", Path, self.waypointCallback)
        rospy.sleep(1)

        # Setup Controller
        lat_control = purePursuit.PurePursuit(path=self.waypoint_path)
        speed_control = SpeedPID.SpeedPID(path=self.waypoint_path,
                                          target_speed=3)

        r = rospy.Rate(1 / self.freq)
        # Control Loop
        while not rospy.is_shutdown():

            lat_control.update(self)  # Update Control
            speed_control.update(self, target_speed=3)

            # Build Command
            self.prius_command = Control()
            self.prius_command.steer = np.clip(lat_control.getSteeringCmd(),
                                               -1, 1)
            self.prius_command.throttle = np.clip(
                speed_control.get_pedal_cmds()[0], 0, 1)
            self.prius_command.brake = np.clip(
                speed_control.get_pedal_cmds()[1], 0, 1)

            # Update Throttle
            self.prius_command.shift_gears = 2

            self.cmdPub.publish(self.prius_command)

            rospy.loginfo("Steer CMD %f, Throttle CMD %f, Brake CMD %f",
                          self.prius_command.steer,
                          self.prius_command.throttle,
                          self.prius_command.brake)
            gx, gy = lat_control.getGoalPoint()
            self.genGoalMarker(gx, gy)
            self.goalMarkerPub.publish(self.goalMarker)

            r.sleep()