├── GlobalPathCarla2ROS.py ├── Modifications ├── README.md ├── controller.py ├── get_topology.py ├── main_carla.py ├── main_topology.py ├── navigate.py ├── plot2d.py ├── routes ├── Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz └── Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES │ ├── doc.kml │ └── images │ ├── icon-1.png │ └── icon-2.png ├── sample_ros_control.py └── stash ├── get_path.py ├── main_topology.py └── purepursuit.py /GlobalPathCarla2ROS.py: -------------------------------------------------------------------------------- 1 | 2 | import rospy 3 | from std_msgs.msg import String 4 | from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist 5 | from nav_msgs.msg import Path 6 | from tf.transformations import quaternion_from_euler 7 | 8 | import carla 9 | import time 10 | import random 11 | import sys 12 | import threading 13 | 14 | import matplotlib.pyplot as plt 15 | import numpy as np 16 | import time 17 | 18 | import networkx as nx 19 | import math 20 | 21 | from get_topology import * 22 | 23 | # plt.ion() 24 | # plt.show() 25 | 26 | class globalPathServer(object): 27 | """Global is published everytime there is a request for global path over /get_global_path topic""" 28 | def __init__(self, world = " ", ns = " ",source=0,destination=14): 29 | # super(GlobalPathServer, self).__init__() 30 | 31 | self.ns = ns 32 | # Get topology from the map 33 | _map = world.get_map() 34 | 35 | # Build waypoint graph 36 | topology,waypoints = get_topology(_map) 37 | self.graph,self.id_map = build_graph(topology) 38 | 39 | # get source and destination location from user (invoke while creating the object) 40 | self.source = source 41 | self.destination = destination 42 | 43 | self.p = get_shortest_path(self.graph, self.source, self.destination) 44 | 45 | for i in range(self.p.shape[0]): 46 | wp = carla.Location(self.p[i][0],self.p[i][1],self.p[i][2]) 47 | world.debug.draw_point(wp, size=0.1, color=carla.Color(0, 255, 0), life_time=300.0,persistent_lines=True) 48 | # print(wp) 49 | 50 | 51 | 52 | 53 | 54 | # Because ROS Uses right handed coordinate system 55 | # and carla uses left handed coordinated system 56 | # https://math.stackexchange.com/questions/2626961/how-to-convert-a-right-handed-coordinate-system-to-left-handed 57 | 58 | print "shorted path...",self.p 59 | for i in range(len(self.p)): 60 | self.p[i][1] = -self.p[i][1] 61 | print "shorted path...",self.p 62 | # self.plot() 63 | rospy.init_node('{}_path_server'.format(self.ns), anonymous = True) 64 | rospy.Subscriber('{}/get_global_path'.format(self.ns), String, self.callback_update) 65 | self.path_publisher = rospy.Publisher('{}/global_path'.format(self.ns), Path, queue_size = 10) 66 | 67 | self.path = Path() 68 | 69 | 70 | def makePathMessage(self): 71 | 72 | carla_path = self.p 73 | # posestamped required header to be set... 74 | # Sequence Number - increase every time this function is called 75 | # time stamp - ros current time 76 | # frame_id - global 77 | 78 | poses = [] 79 | for i in range(len(carla_path)-1): 80 | 81 | # posestamped required header to be set... 82 | # Sequence Number - increase every time this function is called 83 | # time stamp - ros current time 84 | # frame_id - global 85 | posestamped = PoseStamped() 86 | 87 | pose = Pose() 88 | pose.position.x = carla_path[i][0] 89 | pose.position.y = carla_path[i][1] 90 | pose.position.z = carla_path[i][1] 91 | pose.orientation = self.directionFromTwoPointsQuaternion(carla_path[i],carla_path[i+1]) 92 | 93 | posestamped.pose = pose 94 | 95 | poses.append(posestamped) 96 | 97 | posestamped = PoseStamped() 98 | 99 | pose = Pose() 100 | pose.position.x = carla_path[-1][0] 101 | pose.position.y = carla_path[-1][1] 102 | pose.position.z = 0; 103 | pose.orientation = self.directionFromTwoPointsQuaternion(carla_path[-1],carla_path[-1]) 104 | posestamped.pose = pose 105 | 106 | poses.append(posestamped) 107 | 108 | self.path.poses = poses 109 | 110 | def callback_update(self, data): 111 | self.makePathMessage() 112 | self.path_publisher.publish(self.path) 113 | 114 | def plot(self): 115 | mapk = self.id_map.keys() 116 | srcind = self.id_map.values().index(self.source) 117 | destind = self.id_map.values().index(self.destination) 118 | source = mapk[srcind] 119 | dest = mapk[destind] 120 | 121 | plt.plot(source[0],source[1],'go--', linewidth=2, markersize=12) 122 | plt.plot(dest[0],dest[1],'ro--', linewidth=2, markersize=12) 123 | 124 | plt.plot(self.p[:,0],self.p[:,1]) 125 | plt.draw() 126 | plt.pause(0.001) 127 | 128 | @staticmethod 129 | def directionFromTwoPointsQuaternion(p1,p2): 130 | 131 | angle_ = np.arctan2(p2[1]-p1[1],p2[0]-p1[0]) 132 | q = quaternion_from_euler(0, 0, angle_) 133 | quat_msg = Quaternion(q[0], q[1], q[2], q[3]) 134 | return quat_msg 135 | 136 | def main(argv): 137 | 138 | client = carla.Client('localhost',2000) 139 | client.set_timeout(2.0) 140 | 141 | world = client.get_world() 142 | 143 | 144 | # namespace - 'carla' 145 | node = globalPathServer(world,'carla') 146 | 147 | while not rospy.is_shutdown(): 148 | rospy.spin() 149 | 150 | if __name__ == '__main__': 151 | try: 152 | main(sys.argv[1:]) 153 | except rospy.ROSInterruptException: 154 | pass -------------------------------------------------------------------------------- /Modifications: -------------------------------------------------------------------------------- 1 | ros-bridge/src/carla_ros_bridge/transforms.py 2 | line 33: -carla_location.y, -> carla_location.y -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pythonapi-test 2 | 3 | This repository is created to learn to use Carla's PythonAPI 4 | 5 | #### Scripts 6 | 7 | ##### GlobalPathCarla2ROS 8 | 9 | This is a ROS node that publishes global path on request. 10 | 11 | 12 | ###### Topics 13 | 14 | **Publisher:** ```//global_path``` 15 | - Message Type : Path (nav_msgs/path) 16 | 17 | **Subscriber:** ```//get_global_path``` 18 | - Message Type : String (publish empty string on this topic to get global path) 19 | 20 | ###### Use 21 | ```python 22 | def main(argv): 23 | 24 | client = carla.Client('localhost',2000) 25 | client.set_timeout(2.0) 26 | 27 | world = client.get_world() 28 | 29 | 30 | # namespace - 'carla' 31 | node = globalPathServer(world,'carla') 32 | 33 | while not rospy.is_shutdown(): 34 | rospy.spin() 35 | ``` 36 | 37 | 38 | ###### Test 39 | 40 | Run `roscore` 41 | 42 | Goto carla source folder and run carla server 43 | 44 | ```bash 45 | ./CarlaUE4.sh -windowed -ResX=320 -ResY=240 -benchmark -fps=10 46 | ``` 47 | 48 | Run carla client, this should spawn an actor 49 | 50 | ```bash 51 | python main_carla.py 52 | ``` 53 | 54 | Launch carla_ros_bridge 55 | 56 | ```bash 57 | roslaunch carla_ros_bridge client.launch 58 | ``` -------------------------------------------------------------------------------- /controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from math import * 4 | import numpy as np 5 | import time 6 | import sys 7 | # import cutils 8 | 9 | import rospy 10 | from std_msgs.msg import String, Float64 11 | from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist 12 | from nav_msgs.msg import Path, Odometry 13 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 14 | from ackermann_msgs.msg import AckermannDrive 15 | 16 | import logging 17 | logging.basicConfig(level=logging.DEBUG, format='%(asctime)s - %(levelname)s - %(message)s') 18 | 19 | global debug 20 | debug = 0 21 | class closestPoint: 22 | def __init__(self, points): 23 | self.points = points 24 | 25 | def distance(self,p1,p2): 26 | return sqrt((p2[0]-p1[0])**2+(p2[1]-p1[1])**2) 27 | 28 | def closest_node(self, node): 29 | nodes = self.points 30 | # print nodes 31 | print node 32 | # if debug: 33 | # print nodes 34 | # logging.debug("Points type: {}".format(type(nodes))) 35 | # logging.debug("Points array {0}\n".format(nodes.size())) 36 | # logging.debug("Node type: {0} content: {1} \n".format(type(node),node)) 37 | try: 38 | dist_2 = np.sum((nodes - node)**2, axis=1) 39 | except TypeError: 40 | dist_2 = np.sum((nodes - (0,0))**2, axis=1) 41 | return np.argmin(dist_2) 42 | 43 | class purePursuit: 44 | 45 | def __init__(self,points): 46 | 47 | rospy.loginfo("initializing pure pursuit node...") 48 | time.sleep(0.5) 49 | # rospy.init_node("purePursuit_node",anonymous=True) 50 | 51 | self.look_ahead_distance = 5 52 | 53 | self.target_speed = 5 54 | 55 | self.k = 0. 56 | self.kp = 0.2 57 | 58 | self.init_xy = False 59 | self.init_heading = False 60 | 61 | self.car_init_x = None 62 | self.car_init_y = None 63 | self.car_init_heading = None 64 | 65 | self.car_current_x = None 66 | self.car_current_y = None 67 | self.car_current_heading = None 68 | self.current_speed = None 69 | 70 | # Get vehicle location from CARLA 71 | 72 | # Initialize vehicle controller message 73 | # data = rospy.wait_for_message("/carla/ego_vehicle/odometry", Odometry) 74 | # print data 75 | rospy.Subscriber("/carla/ego_vehicle/odometry", Odometry, self.callback_odom) 76 | self.cmd_pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=1) 77 | 78 | self.goalxpub = rospy.Publisher('/goalx',Float64, queue_size=10) 79 | self.goalypub = rospy.Publisher('/goaly',Float64, queue_size=10) 80 | 81 | self.points = points 82 | 83 | 84 | def callback_odom(self,data): 85 | 86 | # rospy.loginfo(__func__) 87 | self.car_current_x = data.pose.pose.position.x 88 | self.car_current_y = data.pose.pose.position.y 89 | 90 | current_speed_x = data.twist.twist.linear.x 91 | current_speed_y = data.twist.twist.linear.y 92 | self.current_speed = np.sqrt(current_speed_x**2 + current_speed_y**2) 93 | 94 | quaternion = [data.pose.pose.orientation.x,\ 95 | data.pose.pose.orientation.y,\ 96 | data.pose.pose.orientation.z,\ 97 | data.pose.pose.orientation.w] 98 | euler = euler_from_quaternion(quaternion) 99 | # euler = tf.transformations.euler_from_quaternion(quaternion) 100 | self.car_current_heading = euler[2] 101 | 102 | 103 | def _controller(self): 104 | 105 | ############################## FIX ######################################### 106 | # data = rospy.wait_for_message("/carla/ego_vehicle/odometry", Odometry) 107 | 108 | # # print data 109 | 110 | # self.car_current_x = data.pose.pose.position.x 111 | # self.car_current_y = data.pose.pose.position.y 112 | 113 | # current_speed_x = data.twist.twist.linear.x 114 | # current_speed_y = data.twist.twist.linear.y 115 | # self.current_speed = np.sqrt(current_speed_x**2 + current_speed_y**2) 116 | 117 | # quaternion = [data.pose.pose.orientation.x,\ 118 | # data.pose.pose.orientation.y,\ 119 | # data.pose.pose.orientation.z,\ 120 | # data.pose.pose.orientation.w] 121 | # euler = euler_from_quaternion(quaternion) 122 | # # euler = tf.transformations.euler_from_quaternion(quaternion) 123 | # self.car_current_heading = euler[2] 124 | ############################################################################# 125 | 126 | path = closestPoint(self.points) 127 | ind = path.closest_node((self.car_current_x,self.car_current_y)) 128 | 129 | L = 0 130 | dind = ind 131 | print "current speed {0} m/s (from twist)".format(self.current_speed) 132 | Lf = self.k*self.current_speed + self.look_ahead_distance 133 | while L 3.14159: 175 | # # print "error is above pi" 176 | # error -= 2*3.14159 177 | # else: 178 | # # print "error is in between -pi and pi" 179 | # pass 180 | 181 | L = 2.6 182 | ld = 10 183 | kl = 1 184 | # self.throttle = self._PIDControl(self.target_speed,self.current_speed) 185 | # self.throttle = min(self.throttle,0.2) 186 | if(distance_final < 1): 187 | self.throttle = 0; 188 | self.throttle = 5; 189 | # self.steering = np.arctan(2*L*sin(error)/(kl*self.throttle)) 190 | self.steering_ratio = 1 191 | self.steering = self.steering_ratio * np.arctan2(2*L*sin(error),Lf) 192 | # self.steering = error 193 | 194 | 195 | def _PIDControl(self,target, current): 196 | print "\n" 197 | print "_PIDControl: ", target-current 198 | print "\n" 199 | a = self.kp * (target - current) 200 | return a 201 | 202 | def publish_(self): 203 | 204 | self._controller() 205 | 206 | ai = self.throttle 207 | di = self.steering 208 | 209 | # print "throttle and steering angle :: ",di 210 | print "speed and steering angle :: ",ai,di 211 | print "\n\n" 212 | 213 | ackermann_cmd_msg = AckermannDrive() 214 | ackermann_cmd_msg.steering_angle = di 215 | ackermann_cmd_msg.speed = ai 216 | 217 | self.cmd_pub.publish(ackermann_cmd_msg) 218 | # throttle_msg = make_throttle_msg(ai) 219 | # steering_msg = make_steering_msg(di) 220 | # brake_msg = make_brake_msg() 221 | # self.throttle_pub.publish(throttle_msg) 222 | # self.steering_pub.publish(steering_msg) 223 | # self.brake_pub.publish(brake_msg) 224 | 225 | rospy.on_shutdown(self.stopOnShutdown) 226 | def stopOnShutdown(self): 227 | ai = 0 228 | di = 0 229 | 230 | ackermann_cmd_msg = AckermannDrive() 231 | ackermann_cmd_msg.steering_angle = di 232 | ackermann_cmd_msg.speed = ai 233 | 234 | self.cmd_pub.publish(ackermann_cmd_msg) 235 | 236 | # time.sleep(1) 237 | # reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty) 238 | # reset_world() 239 | # print "exiting..." 240 | rospy.signal_shutdown('Quit') 241 | # print "is shutdown :: ",rospy.is_shutdown() 242 | # return 243 | 244 | def main(): 245 | global points 246 | 247 | controller_object = purePursuit(points) 248 | 249 | r = rospy.Rate(50) 250 | 251 | while not rospy.is_shutdown(): 252 | controller_object.publish_() 253 | r.sleep() 254 | 255 | if __name__ == '__main__': 256 | main() -------------------------------------------------------------------------------- /get_topology.py: -------------------------------------------------------------------------------- 1 | import carla 2 | import time 3 | import random 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import time 8 | 9 | import networkx as nx 10 | import math 11 | 12 | global debug 13 | debug = 0 14 | def unit_vector(point1, point2): 15 | 16 | x1, y1, _ = point1 17 | x2, y2, _ = point2 18 | 19 | vector = (x2 - x1, y2 - y1) 20 | vector_mag = math.sqrt(vector[0]**2 + vector[1]**2) 21 | vector = (vector[0] / vector_mag, vector[1] / vector_mag) 22 | 23 | return vector 24 | def get_topology(map): 25 | _topology = map.get_topology() 26 | topology = [] 27 | xs = [] 28 | ys = [] 29 | zs = [] 30 | 31 | for segment in _topology: 32 | x1 = segment[0].transform.location.x 33 | y1 = segment[0].transform.location.y 34 | z1 = segment[0].transform.location.z 35 | 36 | x2 = segment[1].transform.location.x 37 | y2 = segment[1].transform.location.y 38 | z2 = segment[1].transform.location.z 39 | 40 | seg_dict = dict() 41 | seg_dict['entry'] = (x1, y1, z1) 42 | seg_dict['exit'] = (x2, y2, z2) 43 | seg_dict['path'] = [] 44 | wp1 = segment[0] 45 | wp2 = segment[1] 46 | seg_dict['intersection'] = True if wp1.is_intersection else False 47 | endloc = wp2.transform.location 48 | w = wp1.next(1)[0] 49 | while w.transform.location.distance(endloc) > 1: 50 | x = w.transform.location.x 51 | y = w.transform.location.y 52 | z = w.transform.location.z 53 | seg_dict['path'].append((x, y, z)) 54 | w = w.next(1)[0] 55 | 56 | topology.append(seg_dict) 57 | 58 | for j in segment: 59 | x = j.transform.location.x 60 | y = j.transform.location.y 61 | z = j.transform.location.z 62 | 63 | #logging 64 | # print x,y,z 65 | xs.append(x) 66 | ys.append(y) 67 | zs.append(z) 68 | xs = np.array(xs).reshape((len(xs),1)) 69 | ys = np.array(ys).reshape((len(ys),1)) 70 | zs = np.array(zs).reshape((len(zs),1)) 71 | 72 | # logging 73 | # print ys,zs 74 | 75 | waypoints = np.hstack((xs,ys,zs)) 76 | 77 | return topology,waypoints 78 | 79 | def build_graph(topology): 80 | graph = nx.DiGraph() 81 | # Map with structure {(x,y): id, ... } 82 | id_map = dict() 83 | 84 | for segment in topology: 85 | 86 | entryxy = segment['entry'] 87 | exitxy = segment['exit'] 88 | path = segment['path'] 89 | intersection = segment['intersection'] 90 | for vertex in entryxy, exitxy: 91 | # print('inside build graph...') 92 | # Adding unique nodes and populating id_map 93 | if vertex not in id_map: 94 | new_id = len(id_map) 95 | id_map[vertex] = new_id 96 | graph.add_node(new_id, vertex=vertex) 97 | 98 | n1, n2 = id_map[entryxy], id_map[exitxy] 99 | # print n1,n2 100 | # Adding edge with attributes 101 | graph.add_edge( 102 | n1, n2, 103 | length=len(path) + 1, path=path, 104 | entry = entryxy, 105 | exit = exitxy, 106 | entry_vector=unit_vector( 107 | entryxy, path[0] if len(path) > 0 else exitxy), 108 | exit_vector=unit_vector( 109 | path[-1] if len(path) > 0 else entryxy, exitxy), 110 | net_vector=unit_vector(entryxy, exitxy), 111 | intersection=intersection) 112 | return graph,id_map 113 | 114 | def get_shortest_path(graph, source, destination): 115 | p = nx.shortest_path(graph,source=source,target=destination) 116 | final_path_x = [] 117 | final_path_y = [] 118 | final_path_z = [] 119 | for i in range(len(p)-1): 120 | edge_ = graph.get_edge_data(p[i],p[i+1]) 121 | # waypoint_ = edge_['entry'] 122 | path_ = edge_['path'] 123 | path_ = np.array(path_) 124 | 125 | if debug: 126 | print __file__, "path ",path_ 127 | 128 | try: 129 | xs = path_[:,0] 130 | ys = path_[:,1] 131 | zs = path_[:,2] 132 | final_path_x.extend(list(xs)) 133 | final_path_y.extend(list(ys)) 134 | final_path_z.extend(list(zs)) 135 | except IndexError: 136 | pass 137 | 138 | final_path_x = np.array(final_path_x).reshape(len(final_path_x),1) 139 | final_path_y = np.array(final_path_y).reshape(len(final_path_y),1) 140 | final_path_z = np.array(final_path_z).reshape(len(final_path_z),1) 141 | 142 | return np.hstack((final_path_x, final_path_y, final_path_z)) 143 | 144 | -------------------------------------------------------------------------------- /main_carla.py: -------------------------------------------------------------------------------- 1 | # "manual_control.py" from Carla PythonAPI is modified to work with ROS Messages 2 | 3 | 4 | """ 5 | Welcome to CARLA manual control. 6 | 7 | Use ARROWS or WASD keys for control. 8 | 9 | W : throttle 10 | S : brake 11 | AD : steer 12 | Q : toggle reverse 13 | Space : hand-brake 14 | P : toggle autopilot 15 | M : toggle manual transmission 16 | ,/. : gear up/down 17 | 18 | TAB : change sensor position 19 | ` : next sensor 20 | [1-9] : change to sensor [1-9] 21 | C : change weather (Shift+C reverse) 22 | Backspace : change vehicle 23 | 24 | R : toggle recording images to disk 25 | 26 | CTRL + R : toggle recording of simulation (replacing any previous) 27 | CTRL + P : start replaying last recorded simulation 28 | CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) 29 | CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) 30 | 31 | F1 : toggle HUD 32 | H/? : toggle help 33 | ESC : quit 34 | """ 35 | 36 | 37 | # ============================================================================== 38 | # -- imports ------------------------------------------------------------------- 39 | # ============================================================================== 40 | 41 | from __future__ import print_function 42 | 43 | import rospy 44 | 45 | import carla 46 | from carla import ColorConverter as cc 47 | from GlobalPathCarla2ROS import globalPathServer 48 | import networkx as nx 49 | from get_topology import * 50 | import time 51 | 52 | import argparse 53 | import collections 54 | import datetime 55 | import logging 56 | import math 57 | import random 58 | import re 59 | import weakref 60 | 61 | 62 | try: 63 | import pygame 64 | from pygame.locals import KMOD_CTRL 65 | from pygame.locals import KMOD_SHIFT 66 | from pygame.locals import K_0 67 | from pygame.locals import K_9 68 | from pygame.locals import K_BACKQUOTE 69 | from pygame.locals import K_BACKSPACE 70 | from pygame.locals import K_COMMA 71 | from pygame.locals import K_DOWN 72 | from pygame.locals import K_ESCAPE 73 | from pygame.locals import K_F1 74 | from pygame.locals import K_LEFT 75 | from pygame.locals import K_PERIOD 76 | from pygame.locals import K_RIGHT 77 | from pygame.locals import K_SLASH 78 | from pygame.locals import K_SPACE 79 | from pygame.locals import K_TAB 80 | from pygame.locals import K_UP 81 | from pygame.locals import K_a 82 | from pygame.locals import K_c 83 | from pygame.locals import K_d 84 | from pygame.locals import K_h 85 | from pygame.locals import K_m 86 | from pygame.locals import K_p 87 | from pygame.locals import K_q 88 | from pygame.locals import K_r 89 | from pygame.locals import K_s 90 | from pygame.locals import K_w 91 | from pygame.locals import K_MINUS 92 | from pygame.locals import K_EQUALS 93 | except ImportError: 94 | raise RuntimeError('cannot import pygame, make sure pygame package is installed') 95 | 96 | try: 97 | import numpy as np 98 | except ImportError: 99 | raise RuntimeError('cannot import numpy, make sure numpy package is installed') 100 | 101 | 102 | # ============================================================================== 103 | # -- Global functions ---------------------------------------------------------- 104 | # ============================================================================== 105 | 106 | 107 | def find_weather_presets(): 108 | rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') 109 | name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) 110 | presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] 111 | return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] 112 | 113 | 114 | def get_actor_display_name(actor, truncate=250): 115 | name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) 116 | return (name[:truncate-1] + u'\u2026') if len(name) > truncate else name 117 | 118 | 119 | # ============================================================================== 120 | # -- World --------------------------------------------------------------------- 121 | # ============================================================================== 122 | 123 | 124 | class World(object): 125 | def __init__(self, carla_world, hud, actor_filter): 126 | self.world = carla_world 127 | self.map = self.world.get_map() 128 | self.hud = hud 129 | self.player = None 130 | self.collision_sensor = None 131 | self.lane_invasion_sensor = None 132 | self.gnss_sensor = None 133 | self.camera_manager = None 134 | self._weather_presets = find_weather_presets() 135 | self._weather_index = 0 136 | self._actor_filter = actor_filter 137 | self.restart() 138 | self.world.on_tick(hud.on_world_tick) 139 | self.recording_enabled = False 140 | self.recording_start = 0 141 | 142 | def restart(self): 143 | # Keep same camera config if the camera manager exists. 144 | cam_index = self.camera_manager._index if self.camera_manager is not None else 0 145 | cam_pos_index = self.camera_manager._transform_index if self.camera_manager is not None else 0 146 | # Get a random blueprint. 147 | blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) 148 | blueprint.set_attribute('role_name', 'hero') 149 | if blueprint.has_attribute('color'): 150 | color = random.choice(blueprint.get_attribute('color').recommended_values) 151 | blueprint.set_attribute('color', color) 152 | # Spawn the player. 153 | if self.player is not None: 154 | spawn_point = self.player.get_transform() 155 | spawn_point.location.z += 2.0 156 | spawn_point.rotation.roll = 0.0 157 | spawn_point.rotation.pitch = 0.0 158 | self.destroy() 159 | self.player = self.world.try_spawn_actor(blueprint, spawn_point) 160 | while self.player is None: 161 | spawn_points = self.map.get_spawn_points() 162 | # spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() 163 | spawn_point = carla.Transform(carla.Location(x=97.2789, y=63.1175, z=1.8431), carla.Rotation(pitch=0, yaw=-10.4166, roll=0)) 164 | print("spawn_point...",spawn_point) 165 | 166 | self.player = self.world.try_spawn_actor(blueprint, spawn_point) 167 | # Set up the sensors. 168 | self.collision_sensor = CollisionSensor(self.player, self.hud) 169 | self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) 170 | self.gnss_sensor = GnssSensor(self.player) 171 | self.camera_manager = CameraManager(self.player, self.hud) 172 | self.camera_manager._transform_index = cam_pos_index 173 | self.camera_manager.set_sensor(cam_index, notify=False) 174 | actor_type = get_actor_display_name(self.player) 175 | self.hud.notification(actor_type) 176 | 177 | def next_weather(self, reverse=False): 178 | self._weather_index += -1 if reverse else 1 179 | self._weather_index %= len(self._weather_presets) 180 | preset = self._weather_presets[self._weather_index] 181 | self.hud.notification('Weather: %s' % preset[1]) 182 | self.player.get_world().set_weather(preset[0]) 183 | 184 | def tick(self, clock): 185 | self.hud.tick(self, clock) 186 | 187 | def render(self, display): 188 | self.camera_manager.render(display) 189 | self.hud.render(display) 190 | 191 | def destroySensors(self): 192 | self.camera_manager.sensor.destroy() 193 | self.camera_manager.sensor = None 194 | self.camera_manager._index = None 195 | 196 | def destroy(self): 197 | actors = [ 198 | self.camera_manager.sensor, 199 | self.collision_sensor.sensor, 200 | self.lane_invasion_sensor.sensor, 201 | self.gnss_sensor.sensor, 202 | self.player] 203 | for actor in actors: 204 | if actor is not None: 205 | actor.destroy() 206 | 207 | 208 | # ============================================================================== 209 | # -- HUD ----------------------------------------------------------------------- 210 | # ============================================================================== 211 | 212 | 213 | class HUD(object): 214 | def __init__(self, width, height): 215 | self.dim = (width, height) 216 | font = pygame.font.Font(pygame.font.get_default_font(), 20) 217 | fonts = [x for x in pygame.font.get_fonts() if 'mono' in x] 218 | default_font = 'ubuntumono' 219 | mono = default_font if default_font in fonts else fonts[0] 220 | mono = pygame.font.match_font(mono) 221 | self._font_mono = pygame.font.Font(mono, 14) 222 | self._notifications = FadingText(font, (width, 40), (0, height - 40)) 223 | self.help = HelpText(pygame.font.Font(mono, 24), width, height) 224 | self.server_fps = 0 225 | self.frame_number = 0 226 | self.simulation_time = 0 227 | self._show_info = True 228 | self._info_text = [] 229 | self._server_clock = pygame.time.Clock() 230 | 231 | def on_world_tick(self, timestamp): 232 | self._server_clock.tick() 233 | self.server_fps = self._server_clock.get_fps() 234 | self.frame_number = timestamp.frame_count 235 | self.simulation_time = timestamp.elapsed_seconds 236 | 237 | def tick(self, world, clock): 238 | self._notifications.tick(world, clock) 239 | if not self._show_info: 240 | return 241 | t = world.player.get_transform() 242 | v = world.player.get_velocity() 243 | c = world.player.get_control() 244 | heading = 'N' if abs(t.rotation.yaw) < 89.5 else '' 245 | heading += 'S' if abs(t.rotation.yaw) > 90.5 else '' 246 | heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else '' 247 | heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else '' 248 | colhist = world.collision_sensor.get_collision_history() 249 | collision = [colhist[x + self.frame_number - 200] for x in range(0, 200)] 250 | max_col = max(1.0, max(collision)) 251 | collision = [x / max_col for x in collision] 252 | vehicles = world.world.get_actors().filter('vehicle.lin*') 253 | self._info_text = [ 254 | 'Server: % 16.0f FPS' % self.server_fps, 255 | 'Client: % 16.0f FPS' % clock.get_fps(), 256 | '', 257 | 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), 258 | 'Map: % 20s' % world.map.name, 259 | 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), 260 | '', 261 | 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), 262 | u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading), 263 | 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), 264 | 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), 265 | 'Height: % 18.0f m' % t.location.z, 266 | ''] 267 | if isinstance(c, carla.VehicleControl): 268 | self._info_text += [ 269 | ('Throttle:', c.throttle, 0.0, 1.0), 270 | ('Steer:', c.steer, -1.0, 1.0), 271 | ('Brake:', c.brake, 0.0, 1.0), 272 | ('Reverse:', c.reverse), 273 | ('Hand brake:', c.hand_brake), 274 | ('Manual:', c.manual_gear_shift), 275 | 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] 276 | elif isinstance(c, carla.WalkerControl): 277 | self._info_text += [ 278 | ('Speed:', c.speed, 0.0, 5.556), 279 | ('Jump:', c.jump)] 280 | self._info_text += [ 281 | '', 282 | 'Collision:', 283 | collision, 284 | '', 285 | 'Number of vehicles: % 8d' % len(vehicles)] 286 | if len(vehicles) > 1: 287 | self._info_text += ['Nearby vehicles:'] 288 | distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) 289 | vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] 290 | for d, vehicle in sorted(vehicles): 291 | if d > 200.0: 292 | break 293 | vehicle_type = get_actor_display_name(vehicle, truncate=22) 294 | self._info_text.append('% 4dm %s' % (d, vehicle_type)) 295 | 296 | def toggle_info(self): 297 | self._show_info = not self._show_info 298 | 299 | def notification(self, text, seconds=2.0): 300 | self._notifications.set_text(text, seconds=seconds) 301 | 302 | def error(self, text): 303 | self._notifications.set_text('Error: %s' % text, (255, 0, 0)) 304 | 305 | def render(self, display): 306 | if self._show_info: 307 | info_surface = pygame.Surface((220, self.dim[1])) 308 | info_surface.set_alpha(100) 309 | display.blit(info_surface, (0, 0)) 310 | v_offset = 4 311 | bar_h_offset = 100 312 | bar_width = 106 313 | for item in self._info_text: 314 | if v_offset + 18 > self.dim[1]: 315 | break 316 | if isinstance(item, list): 317 | if len(item) > 1: 318 | points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] 319 | pygame.draw.lines(display, (255, 136, 0), False, points, 2) 320 | item = None 321 | v_offset += 18 322 | elif isinstance(item, tuple): 323 | if isinstance(item[1], bool): 324 | rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) 325 | pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) 326 | else: 327 | rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) 328 | pygame.draw.rect(display, (255, 255, 255), rect_border, 1) 329 | f = (item[1] - item[2]) / (item[3] - item[2]) 330 | if item[2] < 0.0: 331 | rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) 332 | else: 333 | rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) 334 | pygame.draw.rect(display, (255, 255, 255), rect) 335 | item = item[0] 336 | if item: # At this point has to be a str. 337 | surface = self._font_mono.render(item, True, (255, 255, 255)) 338 | display.blit(surface, (8, v_offset)) 339 | v_offset += 18 340 | self._notifications.render(display) 341 | self.help.render(display) 342 | 343 | 344 | # ============================================================================== 345 | # -- FadingText ---------------------------------------------------------------- 346 | # ============================================================================== 347 | 348 | 349 | class FadingText(object): 350 | def __init__(self, font, dim, pos): 351 | self.font = font 352 | self.dim = dim 353 | self.pos = pos 354 | self.seconds_left = 0 355 | self.surface = pygame.Surface(self.dim) 356 | 357 | def set_text(self, text, color=(255, 255, 255), seconds=2.0): 358 | text_texture = self.font.render(text, True, color) 359 | self.surface = pygame.Surface(self.dim) 360 | self.seconds_left = seconds 361 | self.surface.fill((0, 0, 0, 0)) 362 | self.surface.blit(text_texture, (10, 11)) 363 | 364 | def tick(self, _, clock): 365 | delta_seconds = 1e-3 * clock.get_time() 366 | self.seconds_left = max(0.0, self.seconds_left - delta_seconds) 367 | self.surface.set_alpha(500.0 * self.seconds_left) 368 | 369 | def render(self, display): 370 | display.blit(self.surface, self.pos) 371 | 372 | 373 | # ============================================================================== 374 | # -- HelpText ------------------------------------------------------------------ 375 | # ============================================================================== 376 | 377 | 378 | class HelpText(object): 379 | def __init__(self, font, width, height): 380 | lines = __doc__.split('\n') 381 | self.font = font 382 | self.dim = (680, len(lines) * 22 + 12) 383 | self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) 384 | self.seconds_left = 0 385 | self.surface = pygame.Surface(self.dim) 386 | self.surface.fill((0, 0, 0, 0)) 387 | for n, line in enumerate(lines): 388 | text_texture = self.font.render(line, True, (255, 255, 255)) 389 | self.surface.blit(text_texture, (22, n * 22)) 390 | self._render = False 391 | self.surface.set_alpha(220) 392 | 393 | def toggle(self): 394 | self._render = not self._render 395 | 396 | def render(self, display): 397 | if self._render: 398 | display.blit(self.surface, self.pos) 399 | 400 | 401 | # ============================================================================== 402 | # -- CollisionSensor ----------------------------------------------------------- 403 | # ============================================================================== 404 | 405 | 406 | class CollisionSensor(object): 407 | def __init__(self, parent_actor, hud): 408 | self.sensor = None 409 | self._history = [] 410 | self._parent = parent_actor 411 | self._hud = hud 412 | world = self._parent.get_world() 413 | bp = world.get_blueprint_library().find('sensor.other.collision') 414 | self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) 415 | # We need to pass the lambda a weak reference to self to avoid circular 416 | # reference. 417 | weak_self = weakref.ref(self) 418 | self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) 419 | 420 | def get_collision_history(self): 421 | history = collections.defaultdict(int) 422 | for frame, intensity in self._history: 423 | history[frame] += intensity 424 | return history 425 | 426 | @staticmethod 427 | def _on_collision(weak_self, event): 428 | self = weak_self() 429 | if not self: 430 | return 431 | actor_type = get_actor_display_name(event.other_actor) 432 | self._hud.notification('Collision with %r' % actor_type) 433 | impulse = event.normal_impulse 434 | intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) 435 | self._history.append((event.frame_number, intensity)) 436 | if len(self._history) > 4000: 437 | self._history.pop(0) 438 | 439 | 440 | # ============================================================================== 441 | # -- LaneInvasionSensor -------------------------------------------------------- 442 | # ============================================================================== 443 | 444 | 445 | class LaneInvasionSensor(object): 446 | def __init__(self, parent_actor, hud): 447 | self.sensor = None 448 | self._parent = parent_actor 449 | self._hud = hud 450 | world = self._parent.get_world() 451 | bp = world.get_blueprint_library().find('sensor.other.lane_detector') 452 | self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) 453 | # We need to pass the lambda a weak reference to self to avoid circular 454 | # reference. 455 | weak_self = weakref.ref(self) 456 | self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) 457 | 458 | @staticmethod 459 | def _on_invasion(weak_self, event): 460 | self = weak_self() 461 | if not self: 462 | return 463 | text = ['%r' % str(x).split()[-1] for x in set(event.crossed_lane_markings)] 464 | self._hud.notification('Crossed line %s' % ' and '.join(text)) 465 | 466 | # ============================================================================== 467 | # -- GnssSensor -------------------------------------------------------- 468 | # ============================================================================== 469 | 470 | 471 | class GnssSensor(object): 472 | def __init__(self, parent_actor): 473 | self.sensor = None 474 | self._parent = parent_actor 475 | self.lat = 0.0 476 | self.lon = 0.0 477 | world = self._parent.get_world() 478 | bp = world.get_blueprint_library().find('sensor.other.gnss') 479 | self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) 480 | # We need to pass the lambda a weak reference to self to avoid circular 481 | # reference. 482 | weak_self = weakref.ref(self) 483 | self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) 484 | 485 | @staticmethod 486 | def _on_gnss_event(weak_self, event): 487 | self = weak_self() 488 | if not self: 489 | return 490 | self.lat = event.latitude 491 | self.lon = event.longitude 492 | 493 | 494 | # ============================================================================== 495 | # -- CameraManager ------------------------------------------------------------- 496 | # ============================================================================== 497 | 498 | 499 | class CameraManager(object): 500 | def __init__(self, parent_actor, hud): 501 | self.sensor = None 502 | self._surface = None 503 | self._parent = parent_actor 504 | self._hud = hud 505 | self._recording = False 506 | self._camera_transforms = [ 507 | carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), 508 | carla.Transform(carla.Location(x=1.6, z=1.7))] 509 | self._transform_index = 1 510 | self._sensors = [ 511 | ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], 512 | ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], 513 | ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], 514 | ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], 515 | ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], 516 | ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], 517 | ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] 518 | world = self._parent.get_world() 519 | bp_library = world.get_blueprint_library() 520 | for item in self._sensors: 521 | bp = bp_library.find(item[0]) 522 | if item[0].startswith('sensor.camera'): 523 | bp.set_attribute('image_size_x', str(hud.dim[0])) 524 | bp.set_attribute('image_size_y', str(hud.dim[1])) 525 | elif item[0].startswith('sensor.lidar'): 526 | bp.set_attribute('range', '5000') 527 | item.append(bp) 528 | self._index = None 529 | 530 | def toggle_camera(self): 531 | self._transform_index = (self._transform_index + 1) % len(self._camera_transforms) 532 | self.sensor.set_transform(self._camera_transforms[self._transform_index]) 533 | 534 | def set_sensor(self, index, notify=True): 535 | index = index % len(self._sensors) 536 | needs_respawn = True if self._index is None \ 537 | else self._sensors[index][0] != self._sensors[self._index][0] 538 | if needs_respawn: 539 | if self.sensor is not None: 540 | self.sensor.destroy() 541 | self._surface = None 542 | self.sensor = self._parent.get_world().spawn_actor( 543 | self._sensors[index][-1], 544 | self._camera_transforms[self._transform_index], 545 | attach_to=self._parent) 546 | # We need to pass the lambda a weak reference to self to avoid 547 | # circular reference. 548 | weak_self = weakref.ref(self) 549 | self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) 550 | if notify: 551 | self._hud.notification(self._sensors[index][2]) 552 | self._index = index 553 | 554 | def next_sensor(self): 555 | self.set_sensor(self._index + 1) 556 | 557 | def toggle_recording(self): 558 | self._recording = not self._recording 559 | self._hud.notification('Recording %s' % ('On' if self._recording else 'Off')) 560 | 561 | def render(self, display): 562 | if self._surface is not None: 563 | display.blit(self._surface, (0, 0)) 564 | 565 | @staticmethod 566 | def _parse_image(weak_self, image): 567 | self = weak_self() 568 | if not self: 569 | return 570 | if self._sensors[self._index][0].startswith('sensor.lidar'): 571 | points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) 572 | points = np.reshape(points, (int(points.shape[0]/3), 3)) 573 | lidar_data = np.array(points[:, :2]) 574 | lidar_data *= min(self._hud.dim) / 100.0 575 | lidar_data += (0.5 * self._hud.dim[0], 0.5 * self._hud.dim[1]) 576 | lidar_data = np.fabs(lidar_data) 577 | lidar_data = lidar_data.astype(np.int32) 578 | lidar_data = np.reshape(lidar_data, (-1, 2)) 579 | lidar_img_size = (self._hud.dim[0], self._hud.dim[1], 3) 580 | lidar_img = np.zeros(lidar_img_size) 581 | lidar_img[tuple(lidar_data.T)] = (255, 255, 255) 582 | self._surface = pygame.surfarray.make_surface(lidar_img) 583 | else: 584 | image.convert(self._sensors[self._index][1]) 585 | array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) 586 | array = np.reshape(array, (image.height, image.width, 4)) 587 | array = array[:, :, :3] 588 | array = array[:, :, ::-1] 589 | self._surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) 590 | if self._recording: 591 | image.save_to_disk('_out/%08d' % image.frame_number) 592 | 593 | 594 | 595 | class closestPoint: 596 | def __init__(self, points): 597 | self.points = points 598 | 599 | def distance(self,p1,p2): 600 | return sqrt((p2[0]-p1[0])**2+(p2[1]-p1[1])**2) 601 | 602 | def closest_node(self, node): 603 | nodes = self.points 604 | dist_2 = np.sum((nodes - node)**2, axis=1) 605 | return np.argmin(dist_2) 606 | 607 | def main_loop(args): 608 | pygame.init() 609 | pygame.font.init() 610 | world = None 611 | 612 | try: 613 | client = carla.Client(args.host, args.port) 614 | client.set_timeout(2.0) 615 | 616 | display = pygame.display.set_mode( 617 | (args.width, args.height), 618 | pygame.HWSURFACE | pygame.DOUBLEBUF) 619 | 620 | hud = HUD(args.width, args.height) 621 | world = World(client.get_world(), hud, args.filter) 622 | 623 | # source_location = world.player.get_transform() 624 | # print(source_location) 625 | 626 | start_waypoint = world.map.get_waypoint(world.player.get_location()) 627 | swx = start_waypoint.transform.location.x 628 | swy = start_waypoint.transform.location.y 629 | swz = 0.0 630 | initial_location = (swx,swy,swz) 631 | initial_location_ = np.array(initial_location) 632 | # print start_waypoint.transform.location.y 633 | 634 | 635 | topology,waypoints = get_topology(world.map) 636 | print(type(topology)) 637 | graph,id_map = build_graph(topology) 638 | points = np.array(id_map.keys()) 639 | 640 | path = closestPoint(points) 641 | ind = path.closest_node(initial_location_) 642 | snode = id_map[tuple(points[ind])] 643 | dnode = id_map[random.choice(id_map.keys())] 644 | 645 | print(snode,dnode) 646 | 647 | node = globalPathServer(world.world,'carla',snode,dnode) 648 | # node.plot() 649 | r = rospy.Rate(10) 650 | # while not rospy.is_shutdown(): 651 | while True: 652 | clock = pygame.time.Clock() 653 | clock.tick_busy_loop(60) 654 | # # if controller.parse_events(client, world, clock): 655 | # # return 656 | # print('render...') 657 | world.tick(clock) 658 | world.render(display) 659 | pygame.display.flip() 660 | r.sleep() 661 | 662 | finally: 663 | 664 | if (world and world.recording_enabled): 665 | client.stop_recorder() 666 | 667 | if world is not None: 668 | world.destroy() 669 | 670 | pygame.quit() 671 | 672 | 673 | # ============================================================================== 674 | # -- main() -------------------------------------------------------------------- 675 | # ============================================================================== 676 | 677 | 678 | def main(): 679 | # rospy.init_node('main_carla') 680 | argparser = argparse.ArgumentParser( 681 | description='CARLA Manual Control Client') 682 | argparser.add_argument( 683 | '-v', '--verbose', 684 | action='store_true', 685 | dest='debug', 686 | help='print debug information') 687 | argparser.add_argument( 688 | '--host', 689 | metavar='H', 690 | default='127.0.0.1', 691 | help='IP of the host server (default: 127.0.0.1)') 692 | argparser.add_argument( 693 | '-p', '--port', 694 | metavar='P', 695 | default=2000, 696 | type=int, 697 | help='TCP port to listen to (default: 2000)') 698 | argparser.add_argument( 699 | '-a', '--autopilot', 700 | action='store_true', 701 | help='enable autopilot') 702 | argparser.add_argument( 703 | '--res', 704 | metavar='WIDTHxHEIGHT', 705 | default='1280x720', 706 | help='window resolution (default: 1280x720)') 707 | argparser.add_argument( 708 | '--filter', 709 | metavar='PATTERN', 710 | default='vehicle.lin*', 711 | help='actor filter (default: "vehicle.lin*")') 712 | args = argparser.parse_args() 713 | 714 | args.width, args.height = [int(x) for x in args.res.split('x')] 715 | 716 | log_level = logging.DEBUG if args.debug else logging.INFO 717 | logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) 718 | 719 | logging.info('listening to server %s:%s', args.host, args.port) 720 | 721 | # print(__doc__) 722 | 723 | try: 724 | main_loop(args) 725 | except KeyboardInterrupt: 726 | print('\nCancelled by user. Bye!') 727 | 728 | 729 | if __name__ == '__main__': 730 | 731 | main() -------------------------------------------------------------------------------- /main_topology.py: -------------------------------------------------------------------------------- 1 | import carla 2 | import time 3 | import random 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import time 8 | 9 | import networkx as nx 10 | import math 11 | 12 | from get_topology import * 13 | 14 | 15 | def main(): 16 | 17 | client = carla.Client('localhost',2000) 18 | client.set_timeout(2.0) 19 | 20 | world = client.get_world() 21 | 22 | blueprint_library = world.get_blueprint_library() 23 | 24 | 25 | # vehicle 26 | vehicle = blueprint_library.filter('vehicle.lin*') 27 | vehicle = vehicle[0] 28 | # get one of the possible spawning locations 29 | # transform = random.choice(world.get_map().get_spawn_points()) 30 | # print transform 31 | # plt.show() 32 | 33 | 34 | 35 | # Get topology from the map 36 | _map = world.get_map() 37 | 38 | 39 | # Build waypoint graph 40 | topology,waypoints = get_topology(_map) 41 | # print topology[0].keys()#,type(topology[0]) 42 | 43 | 44 | xs = waypoints[:,0] 45 | ys = waypoints[:,1] 46 | graph,id_map = build_graph(topology) 47 | e1 = graph.edges()[0] 48 | e1_data = graph.get_edge_data(e1[0],e1[1]) 49 | 50 | source_location = e1_data['entry'] 51 | waypoint_next_to_source = e1_data['path'][0] 52 | 53 | source_vector = e1_data['entry_vector'] 54 | 55 | 56 | source_yaw = np.degrees(np.arctan2(source_vector[1],source_vector[0])) 57 | 58 | transform = carla.Transform(carla.Location(x=source_location[0], y=source_location[1], z=2), carla.Rotation(yaw=source_yaw)) 59 | vehicle = world.spawn_actor(vehicle,transform) 60 | 61 | 62 | 63 | 64 | # print graph.get_edge_data() 65 | 66 | axes = plt.gca() 67 | axes.set_xlim(-500, 500) 68 | axes.set_ylim(-500, +500) 69 | line, = axes.plot(xs, ys, 'r*') 70 | 71 | p = get_shortest_path(graph, 0, 14) 72 | mapk = id_map.keys() 73 | srcind = id_map.values().index(0) 74 | print srcind 75 | destind = id_map.values().index(14) 76 | source = mapk[srcind] 77 | dest = mapk[destind] 78 | 79 | plt.plot(source[0],source[1],'go--', linewidth=2, markersize=12) 80 | plt.plot(dest[0],dest[1],'ro--', linewidth=2, markersize=12) 81 | 82 | # print len(final_path_x),len(final_path_y) 83 | plt.plot(p[:,0],p[:,1]) 84 | plt.show() 85 | 86 | 87 | 88 | 89 | # plt.show() 90 | 91 | if __name__ == "__main__": 92 | main() -------------------------------------------------------------------------------- /navigate.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import carla 3 | import time 4 | 5 | import rospy 6 | from std_msgs.msg import String 7 | from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist 8 | from nav_msgs.msg import Path,Odometry 9 | 10 | import numpy as np 11 | 12 | import controller as cn 13 | import matplotlib.pyplot as plt 14 | plt.ion() 15 | plt.show() 16 | 17 | class Navigation(object): 18 | """docstring for Navigation""" 19 | def __init__(self,ns = " "): 20 | 21 | self.ns = ns 22 | self.points = [] 23 | 24 | rospy.init_node('{}_navigation_node'.format(self.ns), anonymous = True) 25 | print '{}/global_path'.format(self.ns) 26 | self.global_path_pub = rospy.Publisher('/{}/get_global_path'.format(self.ns),String,queue_size = 10) 27 | time.sleep(0.5) 28 | # rospy.Subscriber('{}/global_path'.format(self.ns), Path, self.callback_gp) 29 | print "Subscriber started..." 30 | 31 | 32 | # self.path_publisher = rospy.Publisher('{}/global_path'.format(self.ns), Path, queue_size = 10) 33 | 34 | # def callback_gp(self,data): 35 | # print "callback_gp called..." 36 | # for i in data.poses: 37 | # self.points.append([i.pose.position.x,i.pose.position.y]) 38 | def get_points(self): 39 | print "waiting for global path..." 40 | data = rospy.wait_for_message('{}/global_path'.format(self.ns), Path) 41 | 42 | for i in data.poses: 43 | self.points.append([i.pose.position.x,i.pose.position.y]) 44 | self.global_path_pub.publish("abc") 45 | return np.array(self.points) 46 | 47 | class DynamicUpdate(): 48 | def __init__(self,points): 49 | global i 50 | self.xdata=[] 51 | self.ydata=[] 52 | self.x = None 53 | self.y = None 54 | self.points = points 55 | 56 | # print self.points 57 | self.fig, self.ax = plt.subplots(1, 1) 58 | self.ax.axis('equal') 59 | self.ax.plot(self.points[:,0],self.points[:,1]) 60 | if(bool(len(self.xdata))): 61 | self.ax.plot(self.xdata[-1],self.ydata[-1],'ro') 62 | self.lines, = self.ax.plot(self.xdata,self.ydata,color='g', linewidth=2.0) 63 | self.ax.set_autoscaley_on(True) 64 | self.ax.set_xlabel('X (m)') 65 | self.ax.set_ylabel('Y (m)') 66 | # self.ax.set_ylim([-40.0, 40.0]) 67 | # self.ax.set_xlim([-40.0, 40.0]) 68 | self.ax.grid() 69 | 70 | def PlotData(self, x, y): 71 | self.xdata.append(x) 72 | self.ydata.append(y) 73 | self.x = x 74 | self.y = -y 75 | self.lines.set_data(self.xdata,self.ydata) 76 | self.ax.relim() 77 | self.ax.autoscale_view() 78 | #We need to draw *and* flush 79 | self.fig.canvas.draw() 80 | self.fig.canvas.flush_events() 81 | 82 | def main(argv): 83 | 84 | # namespace - 'carla' 85 | node = Navigation('carla') 86 | points = node.get_points() 87 | plt.plot(points[:,0],points[:,1]) 88 | 89 | # plt.draw() 90 | # plt.pause(0.001) 91 | # print points 92 | controller_object = cn.purePursuit(points) 93 | plot = DynamicUpdate(points) 94 | r = rospy.Rate(50) 95 | 96 | while not rospy.is_shutdown(): 97 | pos=rospy.wait_for_message("/carla/ego_vehicle/odometry",Odometry) 98 | 99 | x=pos.pose.pose.position.x 100 | y=pos.pose.pose.position.y 101 | plot.PlotData(x,y) 102 | controller_object.publish_() 103 | r.sleep() 104 | 105 | if __name__ == '__main__': 106 | try: 107 | main(sys.argv[1:]) 108 | except rospy.ROSInterruptException: 109 | pass -------------------------------------------------------------------------------- /plot2d.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import matplotlib.pyplot as plt 4 | import mpl_toolkits.mplot3d.axes3d as p3 5 | import rospy 6 | from nav_msgs.msg import Odometry 7 | #from nav_msgs.msg import Odometry 8 | import numpy as np 9 | import rospy 10 | from std_msgs.msg import String, Float64 11 | from geometry_msgs.msg import Twist, Vector3Stamped 12 | 13 | points = [] 14 | 15 | x = 0 16 | while x<=2*np.pi: 17 | # points.append((40*((1-np.sin(x))*np.cos(x)),40*(1-np.sin(x)))) 18 | points.append((20*np.sin(x),20*np.cos(x))) 19 | x += 0.05 20 | 21 | 22 | i=0 23 | points = np.array(points) 24 | plt.ion() 25 | class DynamicUpdate(): 26 | def __init__(self,points): 27 | global i 28 | self.xdata=[] 29 | self.ydata=[] 30 | self.x = None 31 | self.y = None 32 | self.points = points 33 | 34 | # print self.points 35 | self.fig, self.ax = plt.subplots(1, 1) 36 | self.ax.axis('equal') 37 | self.ax.plot(self.points[:,0],self.points[:,1]) 38 | if(bool(len(self.xdata))): 39 | self.ax.plot(self.xdata[-1],self.ydata[-1],'ro') 40 | self.lines, = self.ax.plot(self.xdata,self.ydata,color='g', linewidth=2.0) 41 | self.ax.set_autoscaley_on(True) 42 | self.ax.set_xlabel('X (m)') 43 | self.ax.set_ylabel('Y (m)') 44 | # self.ax.set_ylim([-40.0, 40.0]) 45 | # self.ax.set_xlim([-40.0, 40.0]) 46 | self.ax.grid() 47 | 48 | def PlotData(self, x, y): 49 | global i 50 | i+=1 51 | self.xdata.append(x) 52 | self.ydata.append(y) 53 | self.x = x 54 | self.y = y 55 | self.lines.set_data(self.xdata,self.ydata) 56 | self.ax.relim() 57 | self.ax.autoscale_view() 58 | #We need to draw *and* flush 59 | self.fig.canvas.draw() 60 | self.fig.canvas.flush_events() 61 | 62 | 63 | 64 | rospy.init_node("plot",anonymous=True) 65 | position_vector = rospy.wait_for_message("/vehicle/perfect_gps/utm",Odometry) 66 | self.car_current_x = data.pose.pose.position.x 67 | self.car_current_y = data.pose.pose.position.y 68 | init_x = position_vector.vector.x 69 | init_y = position_vector.vector.y 70 | points = points + np.array([init_x,init_y]) 71 | plot = DynamicUpdate(points) 72 | 73 | 74 | while not rospy.is_shutdown(): 75 | pos=rospy.wait_for_message("/vehicle/perfect_gps/utm",Odometry) 76 | 77 | x=pos.vector.x 78 | y=pos.vector.y 79 | plot.PlotData(x,y) 80 | -------------------------------------------------------------------------------- /routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vvrs/pythonapi-test/624465eb1e953d94c31dde469772e73fbb28f66c/routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz -------------------------------------------------------------------------------- /routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/doc.kml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA 5 | 19 | 33 | 34 | 35 | normal 36 | #icon-ci-1-nodesc-normal 37 | 38 | 39 | highlight 40 | #icon-ci-1-nodesc-highlight 41 | 42 | 43 | 57 | 71 | 72 | 73 | normal 74 | #icon-ci-2-nodesc-normal 75 | 76 | 77 | highlight 78 | #icon-ci-2-nodesc-highlight 79 | 80 | 81 | 90 | 99 | 100 | 101 | normal 102 | #line-1267FF-5000-nodesc-normal 103 | 104 | 105 | highlight 106 | #line-1267FF-5000-nodesc-highlight 107 | 108 | 109 | 110 | Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA 111 | #line-1267FF-5000-nodesc 112 | 113 | 1 114 | 115 | -71.80761,42.27532,0 116 | -71.80758,42.27537,0 117 | -71.80756,42.2754,0 118 | -71.80752,42.27557,0 119 | -71.80746,42.27589,0 120 | -71.80739,42.27601,0 121 | 122 | 123 | 124 | 125 | Atwater Kent Laboratories, Worcester, MA 01609, USA 126 | #icon-ci-1-nodesc 127 | 128 | 129 | -71.8076058,42.275317,0 130 | 131 | 132 | 133 | 134 | 151 Salisbury St, Worcester, MA 01609, USA 135 | #icon-ci-2-nodesc 136 | 137 | 138 | -71.8073916,42.2760079,0 139 | 140 | 141 | 142 | 143 | 144 | -------------------------------------------------------------------------------- /routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/images/icon-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vvrs/pythonapi-test/624465eb1e953d94c31dde469772e73fbb28f66c/routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/images/icon-1.png -------------------------------------------------------------------------------- /routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/images/icon-2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/vvrs/pythonapi-test/624465eb1e953d94c31dde469772e73fbb28f66c/routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/images/icon-2.png -------------------------------------------------------------------------------- /sample_ros_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from ackermann_msgs.msg import AckermannDrive 5 | 6 | def sample_ros_control(): 7 | pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=10) 8 | rospy.init_node('sample_ros_control', anonymous=True) 9 | rate = rospy.Rate(10) # 10hz 10 | while not rospy.is_shutdown(): 11 | 12 | ackermann_cmd = AckermannDrive() 13 | ackermann_cmd.steering_angle = 0.2 14 | ackermann_cmd.speed = 1.0 15 | pub.publish(ackermann_cmd) 16 | rate.sleep() 17 | 18 | if __name__ == '__main__': 19 | try: 20 | sample_ros_control() 21 | except rospy.ROSInterruptException: 22 | pass 23 | -------------------------------------------------------------------------------- /stash/get_path.py: -------------------------------------------------------------------------------- 1 | import sys 2 | try: 3 | sys.path.append('../PythonAPI') 4 | except IndexError: 5 | pass 6 | 7 | import random 8 | 9 | import carla 10 | from agents.navigation.agent import * 11 | from agents.navigation.local_planner import LocalPlanner 12 | from agents.navigation.local_planner import compute_connection, RoadOption 13 | from agents.navigation.global_route_planner import GlobalRoutePlanner 14 | from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO 15 | from agents.tools.misc import vector 16 | 17 | def main(): 18 | 19 | client = carla.Client('localhost',2000) 20 | client.set_timeout(2.0) 21 | 22 | world = client.get_world() 23 | _map = world.get_map() 24 | dao = GlobalRoutePlannerDAO(_map) 25 | 26 | grp = GlobalRoutePlanner(dao) 27 | grp.setup() 28 | 29 | blueprint_library = world.get_blueprint_library() 30 | 31 | # vehicle 32 | blueprint = random.choice(world.get_blueprint_library().filter('vehicle.lin*')) 33 | 34 | spawn_points = world.get_map().get_spawn_points() 35 | spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() 36 | 37 | _vehicle = world.try_spawn_actor(blueprint, spawn_point) 38 | print(_vehicle) 39 | start_waypoint = _map.get_waypoint(_vehicle.get_location()) 40 | end_waypoint = random.choice(spawn_points) if spawn_points else carla.Transform() 41 | 42 | # Obtain route plan 43 | x1 = start_waypoint.transform.location.x 44 | y1 = start_waypoint.transform.location.y 45 | x2 = end_waypoint.location.x 46 | y2 = end_waypoint.location.y 47 | print(x1,x2,y1,y2) 48 | _graph,_id_map = grp.build_graph() 49 | print(_graph) 50 | 51 | if __name__ == '__main__': 52 | main() -------------------------------------------------------------------------------- /stash/main_topology.py: -------------------------------------------------------------------------------- 1 | import carla 2 | import time 3 | import random 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import time 8 | 9 | import networkx as nx 10 | import math 11 | 12 | from get_topology import * 13 | 14 | 15 | def main(): 16 | 17 | client = carla.Client('localhost',2000) 18 | client.set_timeout(2.0) 19 | 20 | world = client.get_world() 21 | 22 | blueprint_library = world.get_blueprint_library() 23 | 24 | 25 | # vehicle 26 | vehicle = blueprint_library.filter('vehicle.lin*') 27 | vehicle = vehicle[0] 28 | # get one of the possible spawning locations 29 | # transform = random.choice(world.get_map().get_spawn_points()) 30 | # print transform 31 | # plt.show() 32 | 33 | 34 | 35 | # Get topology from the map 36 | _map = world.get_map() 37 | 38 | 39 | # Build waypoint graph 40 | topology,waypoints = get_topology(_map) 41 | # print topology[0].keys()#,type(topology[0]) 42 | 43 | 44 | xs = waypoints[:,0] 45 | ys = waypoints[:,1] 46 | graph,id_map = build_graph(topology) 47 | e1 = graph.edges()[0] 48 | e1_data = graph.get_edge_data(e1[0],e1[1]) 49 | 50 | source_location = e1_data['entry'] 51 | waypoint_next_to_source = e1_data['path'][0] 52 | 53 | source_vector = e1_data['entry_vector'] 54 | 55 | 56 | source_yaw = np.degrees(np.arctan2(source_vector[1],source_vector[0])) 57 | 58 | transform = carla.Transform(carla.Location(x=source_location[0], y=source_location[1], z=2), carla.Rotation(yaw=source_yaw)) 59 | print transform.location.x 60 | vehicle = world.spawn_actor(vehicle,transform) 61 | 62 | 63 | 64 | 65 | # print graph.get_edge_data() 66 | 67 | axes = plt.gca() 68 | axes.set_xlim(-500, 500) 69 | axes.set_ylim(-500, +500) 70 | line, = axes.plot(xs, ys, 'r*') 71 | 72 | p = get_shortest_path(graph, 0, 14) 73 | mapk = id_map.keys() 74 | srcind = id_map.values().index(0) 75 | destind = id_map.values().index(14) 76 | source = mapk[srcind] 77 | dest = mapk[destind] 78 | 79 | plt.plot(source[0],source[1],'go--', linewidth=2, markersize=12) 80 | plt.plot(dest[0],dest[1],'ro--', linewidth=2, markersize=12) 81 | 82 | # print len(final_path_x),len(final_path_y) 83 | plt.plot(p[:,0],p[:,1]) 84 | plt.show() 85 | 86 | 87 | 88 | 89 | # plt.show() 90 | 91 | if __name__ == "__main__": 92 | main() -------------------------------------------------------------------------------- /stash/purepursuit.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2018 Intel Labs. 4 | # authors: German Ros (german.ros@intel.com) 5 | # 6 | # This work is licensed under the terms of the MIT license. 7 | # For a copy, see . 8 | 9 | """ This module contains PID controllers to perform lateral and longitudinal control. """ 10 | 11 | from collections import deque 12 | import math 13 | 14 | import numpy as np 15 | 16 | import carla 17 | from agents.tools.misc import distance_vehicle, get_speed 18 | 19 | class VehiclePIDController(): 20 | """ 21 | VehiclePIDController is the combination of two PID controllers (lateral and longitudinal) to perform the 22 | low level control a vehicle from client side 23 | """ 24 | 25 | def __init__(self, vehicle, 26 | args_lateral={'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}, 27 | args_longitudinal={'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}): 28 | """ 29 | :param vehicle: actor to apply to local planner logic onto 30 | :param args_lateral: dictionary of arguments to set the lateral PID controller using the following semantics: 31 | K_P -- Proportional term 32 | K_D -- Differential term 33 | K_I -- Integral term 34 | :param args_longitudinal: dictionary of arguments to set the longitudinal PID controller using the following 35 | semantics: 36 | K_P -- Proportional term 37 | K_D -- Differential term 38 | K_I -- Integral term 39 | """ 40 | self._vehicle = vehicle 41 | self._world = self._vehicle.get_world() 42 | self._lon_controller = PIDLongitudinalController( 43 | self._vehicle, **args_longitudinal) 44 | self._lat_controller = PIDLateralController( 45 | self._vehicle, **args_lateral) 46 | 47 | def run_step(self, target_speed, waypoint): 48 | """ 49 | Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint 50 | at a given target_speed. 51 | 52 | :param target_speed: desired vehicle speed 53 | :param waypoint: target location encoded as a waypoint 54 | :return: distance (in meters) to the waypoint 55 | """ 56 | throttle = self._lon_controller.run_step(target_speed) 57 | steering = self._lat_controller.run_step(waypoint) 58 | 59 | control = carla.VehicleControl() 60 | control.steer = steering 61 | control.throttle = throttle 62 | control.brake = 0.0 63 | control.hand_brake = False 64 | control.manual_gear_shift = False 65 | 66 | return control 67 | 68 | 69 | class PIDLongitudinalController(): 70 | """ 71 | PIDLongitudinalController implements longitudinal control using a PID. 72 | """ 73 | 74 | def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): 75 | """ 76 | :param vehicle: actor to apply to local planner logic onto 77 | :param K_P: Proportional term 78 | :param K_D: Differential term 79 | :param K_I: Integral term 80 | :param dt: time differential in seconds 81 | """ 82 | self._vehicle = vehicle 83 | self._K_P = K_P 84 | self._K_D = K_D 85 | self._K_I = K_I 86 | self._dt = dt 87 | self._e_buffer = deque(maxlen=30) 88 | 89 | def run_step(self, target_speed, debug=False): 90 | """ 91 | Execute one step of longitudinal control to reach a given target speed. 92 | 93 | :param target_speed: target speed in Km/h 94 | :return: throttle control in the range [0, 1] 95 | """ 96 | current_speed = get_speed(self._vehicle) 97 | 98 | if debug: 99 | print('Current speed = {}'.format(current_speed)) 100 | 101 | return self._pid_control(target_speed, current_speed) 102 | 103 | def _pid_control(self, target_speed, current_speed): 104 | """ 105 | Estimate the throttle of the vehicle based on the PID equations 106 | 107 | :param target_speed: target speed in Km/h 108 | :param current_speed: current speed of the vehicle in Km/h 109 | :return: throttle control in the range [0, 1] 110 | """ 111 | _e = (target_speed - current_speed) 112 | self._e_buffer.append(_e) 113 | 114 | if len(self._e_buffer) >= 2: 115 | _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt 116 | _ie = sum(self._e_buffer) * self._dt 117 | else: 118 | _de = 0.0 119 | _ie = 0.0 120 | 121 | return np.clip((self._K_P * _e) + (self._K_D * _de / self._dt) + (self._K_I * _ie * self._dt), 0.0, 1.0) 122 | 123 | 124 | class PIDLateralController(): 125 | """ 126 | PIDLateralController implements lateral control using a PID. 127 | """ 128 | 129 | def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): 130 | """ 131 | :param vehicle: actor to apply to local planner logic onto 132 | :param K_P: Proportional term 133 | :param K_D: Differential term 134 | :param K_I: Integral term 135 | :param dt: time differential in seconds 136 | """ 137 | self._vehicle = vehicle 138 | self._K_P = K_P 139 | self._K_D = K_D 140 | self._K_I = K_I 141 | self._dt = dt 142 | self._e_buffer = deque(maxlen=10) 143 | 144 | def run_step(self, waypoint): 145 | """ 146 | Execute one step of lateral control to steer the vehicle towards a certain waypoin. 147 | 148 | :param waypoint: target waypoint 149 | :return: steering control in the range [-1, 1] where: 150 | -1 represent maximum steering to left 151 | +1 maximum steering to right 152 | """ 153 | return self._pid_control(waypoint, self._vehicle.get_transform()) 154 | 155 | def _pid_control(self, waypoint, vehicle_transform): 156 | """ 157 | Estimate the steering angle of the vehicle based on the PID equations 158 | 159 | :param waypoint: target waypoint 160 | :param vehicle_transform: current transform of the vehicle 161 | :return: steering control in the range [-1, 1] 162 | """ 163 | v_begin = vehicle_transform.location 164 | v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), 165 | y=math.sin(math.radians(vehicle_transform.rotation.yaw))) 166 | 167 | v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) 168 | w_vec = np.array([waypoint.transform.location.x - 169 | v_begin.x, waypoint.transform.location.y - 170 | v_begin.y, 0.0]) 171 | _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / 172 | (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) 173 | 174 | _cross = np.cross(v_vec, w_vec) 175 | if _cross[2] < 0: 176 | _dot *= -1.0 177 | 178 | self._e_buffer.append(_dot) 179 | if len(self._e_buffer) >= 2: 180 | _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt 181 | _ie = sum(self._e_buffer) * self._dt 182 | else: 183 | _de = 0.0 184 | _ie = 0.0 185 | 186 | return np.clip((self._K_P * _dot) + (self._K_D * _de / 187 | self._dt) + (self._K_I * _ie * self._dt), -1.0, 1.0) 188 | --------------------------------------------------------------------------------