Carla vehicle won't Follow waypoints when i apply PID controller

[ImtiazUlHassan]

Hi guys i have recently started working on Carla. I m trying to drive my vehicle through custom waypoints which i have saved in a csv file. But when i apply vehicle control using pid to follow those steps my vehicle is unable to follow those way-points.

for i in range(len(wp)-1):

    desiredSpeed = 25
    waypoint=wp[i]
    control = PID.run_step(desiredSpeed,waypoint)
    vehicle.apply_control(control)
where `wp` is a list which consist of waypoints through which i wanna drive.

I have plotted the waypoints on the map through which i want to drive.

while these are my PID arguments

args_lateral_dict = {
            'K_P': 1,
            'K_D': 0.8,
            'K_I': 0.8

            ,'dt': 1.0 / 10.0
            }

args_long_dict = {
            'K_P': 1,
            'K_D': 0.8,
            'K_I': 0.8
            ,'dt': 1.0 / 10.0
            }```

[jackbart94]

I've had some experience with this, so here's my suggestions:

• Use synchronous_mode, especially if you're drawing points each tick with the debug helper.
• Start tuning the PID with (0,0,0) and then slowly change the values as explained in this link.
• You can also start with the default ones from the agents (I'm sure you're familiar with them) but those will also need tuning:

          args_lateral_dict = {
          'K_P': 1.95,
          'K_D': 0.2,
          'K_I': 0.07,
          }
      args_longitudinal_dict = {
          'K_P': 1.0,
          'K_D': 0,
          'K_I': 0.05,
          }

  If you decide to use them, set dt to 0.05, since they are tuned for that specific delta time step.

• Decide on a specific vehicle. All of them have different configurations, and only one set of PID values won't be sufficient. You'll probably need a lateral/longitudinal dict for slow velocities, and another one for higher velocities.

Finally, if you do achieve some decent results, make sure to share the values! It would be very valuable for us.

[ImtiazUlHassan]

I've had some experience with this, so here's my suggestions:

* Use `synchronous_mode`, especially if you're drawing points each tick with the debug helper.

* Start tuning the `PID` with (0,0,0) and then slowly change the values [as explained in this link.](https://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops)

* You can also start with the default ones from the agents (I'm sure you're familiar with them) but those will also need tuning:

            args_lateral_dict = {
            'K_P': 1.95,
            'K_D': 0.2,
            'K_I': 0.07,
            }
        args_longitudinal_dict = {
            'K_P': 1.0,
            'K_D': 0,
            'K_I': 0.05,
            }

If you decide to use them, set dt to 0.05, since they are tuned for that specific delta time step.

* Decide on a specific vehicle. All of them have different configurations, and only one set of PID values won't be sufficient. You'll probably need a lateral/longitudinal dict for slow velocities, and another one for higher velocities.

Finally, if you do achieve some decent results, make sure to share the values! It would be very valuable for us.

So far no luck. When i use the following it works fine. As the vehicle roams randomly all over the town.

waypoints = world.get_map().get_waypoint(vehicle.get_location()) #get way point for current location
            waypoint = np.random.choice(waypoints.next(3)) #find waypoint in the radius of the given value (0.3) the approximate distance where to get the next waypoints. 
            desiredSpeed = 25
            control = PID.run_step(desiredSpeed,waypoint) 
            vehicle.apply_control(control) # apply pid control 
            world.debug.draw_point(waypoint.transform.location,size=0.3)

But for Custom waypoints which i have saved it doesn't work. And when i run the simulator in synchronous mode. It freezes .

[ImtiazUlHassan]

The issue has been resolved. The problem was that i was getting next waypoint before making it sure that i have reached near target way-point. so i have made the following necessary changes.

while True:

    vehicle_loc= vehicle.get_location()

    dist = math.sqrt( (way.transform.location.x - vehicle_loc.x)**2 + (way.transform.location.y - vehicle_loc.y)**2 )
    desiredSpeed = 25

    print ("Distrance befor the loop is ", dist)
    control = PID.run_step(desiredSpeed,way) 
    vehicle.apply_control(control)

    if i==(len(wp)-1):
        print("last waypoint reached")
        break

    if (dist<2.5):
        print ("The distance is less than  ", dist)  
        #Get next way point only when the distance between our vehicle and the current                                
        #waypoint is less than 2.5    meters 

        desiredSpeed = 25
        control = PID.run_step(desiredSpeed,way) 
        vehicle.apply_control(control)
        i=i+1
        way=wp[i]

[Zhuweilong123]

I have the same question. Can you share your complete code. Beside, is there a method can let a vehicle run in our predefined route in self-driving mode? Best Wish! @ImtiazUlHassan

[MankaranSingh]

thanks @ImtiazUlHassan. nice catch !!

[ImtiazUlHassan]

I have the same question. Can you share your complete code. Beside, is there a method can let a vehicle run in our predefined route in self-driving mode? Best Wish! @ImtiazUlHassan

import carla

from global_route_planner import  GlobalRoutePlanner
from global_route_planner_dao import GlobalRoutePlannerDAO 

import time
import math
import numpy as np
import controller

def spawn_vehicle(spawnPoint=carla.Transform(carla.Location(x=-6.446170, y=-79.055023, z=0.275307 ),carla.Rotation(pitch=0.0, yaw=0.0, roll=0.000000))):

    """

    This function spawn vehicles in the given spawn points. If no spawn 
    point is provided it spawns vehicle in this 
    position x=27.607,y=3.68402,z=0.02
    """

    spawnPoint=spawnPoint
    world = client.get_world()
    blueprint_library = world.get_blueprint_library()
    bp = blueprint_library.filter('vehicle.*')[7]
    vehicle = world.spawn_actor(bp, spawnPoint)
    return vehicle

def drive_through_plan(planned_route,vehicle,speed,PID):
    """
    This function drives throught the planned_route with the speed passed in the argument

    """

    i=0
    target=planned_route[0]
    while True:
        vehicle_loc= vehicle.get_location()
        distance_v =find_dist_veh(vehicle_loc,target)
        control = PID.run_step(speed,target)
        vehicle.apply_control(control)

        if i==(len(planned_route)-1):
            print("last waypoint reached")
            break 

        if (distance_v<3.5):
            control = PID.run_step(speed,target)
            vehicle.apply_control(control)
            i=i+1
            target=planned_route[i]

    control = PID.run_step(0,planned_route[len(planned_route)-1])
    vehicle.apply_control(control)

def find_dist(start ,end ):
    dist = math.sqrt( (start.transform.location.x - end.transform.location.x)**2 + (start.transform.location.y - end.transform.location.y)**2 )

    return dist

def find_dist_veh(vehicle_loc,target):
    dist = math.sqrt( (target.transform.location.x - vehicle_loc.x)**2 + (target.transform.location.y - vehicle_loc.y)**2 )

    return dist

def setup_PID(vehicle):

    """
    This function creates a PID controller for the vehicle passed to it 
    """

    args_lateral_dict = {
            'K_P': 1.95,
            'K_D': 0.2,
            'K_I': 0.07

            ,'dt': 1.0 / 10.0
            }

    args_long_dict = {
            'K_P': 1,
            'K_D': 0.0,
            'K_I': 0.75
            ,'dt': 1.0 / 10.0
            }

    PID=controller.VehiclePIDController(vehicle,args_lateral=args_lateral_dict,args_longitudinal=args_long_dict)

    return PID

client = carla.Client("localhost", 2000)
client.set_timeout(10)
world = client.load_world('Town03')

amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
a = carla.Location(spawn_points[0].location)
b = carla.Location(spawn_points[100].location)
w1 = grp.trace_route(a, b) 

world.debug.draw_point(a,color=carla.Color(r=255, g=0, b=0),size=1.6 ,life_time=120.0)
world.debug.draw_point(b,color=carla.Color(r=255, g=0, b=0),size=1.6 ,life_time=120.0)

wps=[]

for i in range(len(w1)):
    wps.append(w1[i][0])
    world.debug.draw_point(w1[i][0].transform.location,color=carla.Color(r=255, g=0, b=0),size=0.4 ,life_time=120.0)

vehicle=spawn_vehicle()
PID=setup_PID(vehicle)

speed=30
drive_through_plan(wps,vehicle,speed,PID)

[Tanman1234]

I have the same question. Can you share your complete code. Beside, is there a method can let a vehicle run in our predefined route in self-driving mode? Best Wish! @ImtiazUlHassan

import carla

from global_route_planner import  GlobalRoutePlanner
from global_route_planner_dao import GlobalRoutePlannerDAO 

import time
import math
import numpy as np
import controller

def spawn_vehicle(spawnPoint=carla.Transform(carla.Location(x=-6.446170, y=-79.055023, z=0.275307 ),carla.Rotation(pitch=0.0, yaw=0.0, roll=0.000000))):

    """

    This function spawn vehicles in the given spawn points. If no spawn 
    point is provided it spawns vehicle in this 
    position x=27.607,y=3.68402,z=0.02
    """

    spawnPoint=spawnPoint
    world = client.get_world()
    blueprint_library = world.get_blueprint_library()
    bp = blueprint_library.filter('vehicle.*')[7]
    vehicle = world.spawn_actor(bp, spawnPoint)
    return vehicle

def drive_through_plan(planned_route,vehicle,speed,PID):
    """
    This function drives throught the planned_route with the speed passed in the argument

    """

    i=0
    target=planned_route[0]
    while True:
        vehicle_loc= vehicle.get_location()
        distance_v =find_dist_veh(vehicle_loc,target)
        control = PID.run_step(speed,target)
        vehicle.apply_control(control)

        if i==(len(planned_route)-1):
            print("last waypoint reached")
            break 

        if (distance_v<3.5):
            control = PID.run_step(speed,target)
            vehicle.apply_control(control)
            i=i+1
            target=planned_route[i]

    control = PID.run_step(0,planned_route[len(planned_route)-1])
    vehicle.apply_control(control)

def find_dist(start ,end ):
    dist = math.sqrt( (start.transform.location.x - end.transform.location.x)**2 + (start.transform.location.y - end.transform.location.y)**2 )

    return dist

def find_dist_veh(vehicle_loc,target):
    dist = math.sqrt( (target.transform.location.x - vehicle_loc.x)**2 + (target.transform.location.y - vehicle_loc.y)**2 )

    return dist

def setup_PID(vehicle):

    """
    This function creates a PID controller for the vehicle passed to it 
    """

    args_lateral_dict = {
            'K_P': 1.95,
            'K_D': 0.2,
            'K_I': 0.07

            ,'dt': 1.0 / 10.0
            }

    args_long_dict = {
            'K_P': 1,
            'K_D': 0.0,
            'K_I': 0.75
            ,'dt': 1.0 / 10.0
            }

    PID=controller.VehiclePIDController(vehicle,args_lateral=args_lateral_dict,args_longitudinal=args_long_dict)

    return PID

client = carla.Client("localhost", 2000)
client.set_timeout(10)
world = client.load_world('Town03')

amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
a = carla.Location(spawn_points[0].location)
b = carla.Location(spawn_points[100].location)
w1 = grp.trace_route(a, b) 

world.debug.draw_point(a,color=carla.Color(r=255, g=0, b=0),size=1.6 ,life_time=120.0)
world.debug.draw_point(b,color=carla.Color(r=255, g=0, b=0),size=1.6 ,life_time=120.0)

wps=[]

for i in range(len(w1)):
    wps.append(w1[i][0])
    world.debug.draw_point(w1[i][0].transform.location,color=carla.Color(r=255, g=0, b=0),size=0.4 ,life_time=120.0)

vehicle=spawn_vehicle()
PID=setup_PID(vehicle)

speed=30
drive_through_plan(wps,vehicle,speed,PID)

for this also my vehicle is not following waypoint. any suggestion ?

[ImtiazUlHassan]

@Tanman1234 try changing the 3.5 value. And lemme know if it works.

            control = PID.run_step(speed,target)
            vehicle.apply_control(control)
            i=i+1
            target=planned_route[i]

[Tanman1234]

@Tanman1234 try changing the 3.5 value. And lemme know if it works.

            control = PID.run_step(speed,target)
            vehicle.apply_control(control)
            i=i+1
            target=planned_route[i]

so i tried different values but its not working

[ImtiazUlHassan]

@Tanman1234 can you share any video or screenshot? Is the vehicle not moving at all or its going out of the track ?

[Tanman1234]

@Tanman1234 can you share any video or screenshot? Is the vehicle not moving at all or its going out of the track ? yes its going out of track

[Tanman1234]

@Tanman1234 can you share any video or screenshot? Is the vehicle not moving at all or its going out of the track ?

basically when i run the code its not following the path which is printed on carla simulator , its going offtrack and crashing to walls and other stuff

[Kin-Zhang]

@Tanman1234 can you share any video or screenshot? Is the vehicle not moving at all or its going out of the track ?

basically when i run the code its not following the path which is printed on carla simulator , its going offtrack and crashing to walls and other stuff

I run the code and it works fine for me, here is the gif:

And here is running gif:

and the output plot:

And I slightly change the speed limit with the road speed limit not constant like the output plot shown.

Make sure that you load the world right, or copy the whole code (remember to change the import controller since the path maybe different.

[rns0869]

@ImtiazUlHassan @Kin-Zhang @Zhuweilong123 I'm facing an error in the distance function, it says that 'Location' object has no attribute 'transform'. Do you have any idea why am I facing this error?

The complete Error is below:

AttributeError Traceback (most recent call last)

in 1 speed = 30 ----> 2 drive_through_route(waypoint_list, vehicle, speed, PID) in drive_through_route(planned_route, vehicle, speed, PID) 4 while True: 5 vehicle_location = vehicle.get_location() ----> 6 distance_v = find_distance_vehicle(vehicle_location, target) 7 control = PID.run_step(speed, target) 8 vehicle.apply_control(control) in find_distance_vehicle(vehicle_location, target) 1 def find_distance_vehicle(vehicle_location, target): ----> 2 distance = np.sqrt((target.transform.location.x - vehicle_location.transform.location.x)**2 + (target.transform.location.y - vehicle_location.transform.location.y)**2) 3 return distane AttributeError: 'Location' object has no attribute 'transform'

[Zhuweilong123]

@rohansingh080699

vehicle_location = vehicle.get_location()   #  this method get a location type object
# location type is the attribute of transform type.
# location type has three attribute: x, y, z
# therefore, you only modify your formulation:
distance = np.sqrt((target.transform.location.x - vehicle_location.x)**2 + (target.transform.location.y - vehicle_location.y)**2)

[Saddy21]

Hi I wanted to know where "GlobalRoutePlannerDAO " is located.I cant find this module anywhere . Is it because of different version or something else.My version is 0.9.13. Please help. @Zhuweilong123 @Kin-Zhang @rohansingh080699

[Zhuweilong123]

Hi @Saddy21 "GlobalRoutePlannerDAO" was renamed in 0.9.13.

import carla import sys sys.path.append('../') from agents.navigation.global_route_planner import GlobalRoutePlanner

[kadavparth]

Hi, I am starting to learn controls and wanted to do something as simple as designing a waypoint following script in CARLA that gets the next 20 waypoints and follows them with a set speed of 10 mph. I wanted to use a PID controller for the same. Could anyone help me with the same!

It would be much appreciated

[nahes11]

Hi @Saddy21 "GlobalRoutePlannerDAO" was renamed in 0.9.13.

import carla import sys sys.path.append('../') from agents.navigation.global_route_planner import GlobalRoutePlanner

hi what's the name? thanks

[friendship1]

Hi @Saddy21 "GlobalRoutePlannerDAO" was renamed in 0.9.13.

import carla import sys sys.path.append('../') from agents.navigation.global_route_planner import GlobalRoutePlanner

hi what's the name? thanks

@nahes11 I had the same problem and I tried to find out what caused the GlobalRoutePlannerDAO to disappear.

As far as I know, GlobalRoutePlannerDAO has been merged into GlobalRoutePlanner.

Since 0.9.11, you don't need to call GlobalRoutePlannerDAO before calling GlobalRoutePlanner, you can use GlobalRoutePlanner directly.

Note the two links below: https://github.com/carla-simulator/carla/commit/e930958297c448dc6f4d00eb32bcdba6d5011be9 https://github.com/carla-simulator/carla/pull/4362

[AOOOOOA]

@Tanman1234 can you share any video or screenshot? Is the vehicle not moving at all or its going out of the track ?

basically when i run the code its not following the path which is printed on carla simulator , its going offtrack and crashing to walls and other stuff

I run the code and it works fine for me, here is the gif:

And here is running gif:

and the output plot:

And I slightly change the speed limit with the road speed limit not constant like the output plot shown.

Make sure that you load the world right, or copy the whole code (remember to change the import controller since the path maybe different.

Hi, may I ask how you get the output plot? Is it drawn by yourself based on the logged vehicle status data or generated by the PythonAPI provided by Carla? Thanks!

[SExpert12]

/ #3883

[knowaiser]

Hello,

I've encountered challenges similar to those discussed here, particularly with my vehicle following waypoints. Despite my efforts, the vehicle consistently drives in circles, with both steering and throttle commands maxing out within the constraints set by the code. My approach integrates the PID code shared in this thread by @ImtiazUlHassan , and I've also utilized the VehiclePIDController class provided by the CARLA simulator. The primary deviation in my setup lies in the waypoint generation logic.

Despite experimenting with various Kp, Ki, and Kd values, I've observed minimal improvements in performance. I would greatly appreciate any insights or suggestions you might offer to help address these issues. Your support in this matter would be invaluable.

Thank you!

[Ramri-as]

Hello,

I have a question regarding the implementation of PID in CARLA. Why is it that If I set the target yaw to -90 and the vehicle moving in reverse, no matter what I did the vehicle always circled around in the end, but if the vehicle moving forward it was able to correct its yaw?