From 052fa569091c3b0860ec420c0f293abade7d86c4 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Fri, 29 Mar 2019 21:05:05 -0400 Subject: [PATCH 01/10] Fix 0 FPS issue on client On branch yecheng Changes to be committed: modified: main_carla.py --- main_carla.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/main_carla.py b/main_carla.py index 3d3ad42..32a2a56 100644 --- a/main_carla.py +++ b/main_carla.py @@ -646,10 +646,11 @@ def main_loop(args): node = globalPathServer(world.world,'carla',snode,dnode) # node.plot() - r = rospy.Rate(10) - # while not rospy.is_shutdown(): - while True: - clock = pygame.time.Clock() + # r = rospy.Rate(10) + clock = pygame.time.Clock() + while not rospy.is_shutdown(): + # while True: + clock.tick_busy_loop(60) # # if controller.parse_events(client, world, clock): # # return @@ -657,7 +658,7 @@ def main_loop(args): world.tick(clock) world.render(display) pygame.display.flip() - r.sleep() + # r.sleep() finally: From 2c45f1722327ddc07ffe1023ce346d363a65cc68 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Sat, 30 Mar 2019 11:03:45 -0400 Subject: [PATCH 02/10] Support ROS message control via PythonAPI --- GlobalPathCarla2ROS.py | 31 +++++++++++++++++++++++++++++++ main_carla.py | 9 ++++++--- 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py index befe378..5418fbf 100644 --- a/GlobalPathCarla2ROS.py +++ b/GlobalPathCarla2ROS.py @@ -3,6 +3,7 @@ from std_msgs.msg import String from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist from nav_msgs.msg import Path +from dbw_mkz_msgs.msg import ThrottleCmd,BrakeCmd,SteeringCmd from tf.transformations import quaternion_from_euler import carla @@ -32,6 +33,13 @@ def __init__(self, world = " ", ns = " ",source=0,destination=14): # Get topology from the map _map = world.get_map() + actor_list=world.get_actors() + for actor in actor_list.filter('vehicle.lincoln.mkz2017'): + self.player = actor + self.throttle = 0 + self.brake = 0 + self.steering = 0 + # Build waypoint graph topology,waypoints = get_topology(_map) self.graph,self.id_map = build_graph(topology) @@ -54,11 +62,17 @@ def __init__(self, world = " ", ns = " ",source=0,destination=14): # self.plot() rospy.init_node('{}_path_server'.format(self.ns), anonymous = True) rospy.Subscriber('{}/get_global_path'.format(self.ns), String, self.callback_update) + rospy.Subscriber('carla/ThrottleCmd', ThrottleCmd, self.callback_throttle) + rospy.Subscriber('carla/BrakeCmd', BrakeCmd, self.callback_brake) + rospy.Subscriber('carla/SteeringCmd', SteeringCmd, self.callback_steering) + self.path_publisher = rospy.Publisher('{}/global_path'.format(self.ns), Path, queue_size = 10) self.path = Path() + + def makePathMessage(self): carla_path = self.p @@ -103,6 +117,23 @@ def callback_update(self, data): self.makePathMessage() self.path_publisher.publish(self.path) + def callback_throttle(self, msg): + self.throttle=msg.pedal_cmd + if(self.throttle>0): + self.brake = 0 + + def callback_brake(self, msg): + self.brake = msg.pedal_cmd + if(self.brake>0): + self.throttle = 0 + + def callback_steering(self, msg): + self.steering = msg.steering_wheel_angle_cmd + + def apply_control(self): + control_cmd = carla.VehicleControl(throttle=self.throttle,brake=self.brake,steer=self.steering) + self.player.apply_control(control_cmd) + def plot(self): mapk = self.id_map.keys() srcind = self.id_map.values().index(self.source) diff --git a/main_carla.py b/main_carla.py index 32a2a56..392a2f8 100644 --- a/main_carla.py +++ b/main_carla.py @@ -59,6 +59,7 @@ import weakref + try: import pygame from pygame.locals import KMOD_CTRL @@ -147,7 +148,8 @@ def restart(self): blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) blueprint.set_attribute('role_name', 'hero') if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) + # color = random.choice(blueprint.get_attribute('color').recommended_values) + color = '50,50,50' blueprint.set_attribute('color', color) # Spawn the player. if self.player is not None: @@ -158,7 +160,7 @@ def restart(self): self.destroy() self.player = self.world.try_spawn_actor(blueprint, spawn_point) while self.player is None: - spawn_points = self.map.get_spawn_points() + # spawn_points = self.map.get_spawn_points() # spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() spawn_point = carla.Transform(carla.Location(x=97.2789, y=63.1175, z=1.8431), carla.Rotation(pitch=0, yaw=-10.4166, roll=0)) print("spawn_point...",spawn_point) @@ -651,10 +653,11 @@ def main_loop(args): while not rospy.is_shutdown(): # while True: - clock.tick_busy_loop(60) + clock.tick_busy_loop(20) # # if controller.parse_events(client, world, clock): # # return # print('render...') + node.apply_control() world.tick(clock) world.render(display) pygame.display.flip() From 145f7a23455771fea3ed9b5129ba04a9a8197713 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Sat, 30 Mar 2019 11:59:13 -0400 Subject: [PATCH 03/10] pass player from main code to globalPathServer --- GlobalPathCarla2ROS.py | 6 ++---- main_carla.py | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py index 5418fbf..7fa65a6 100644 --- a/GlobalPathCarla2ROS.py +++ b/GlobalPathCarla2ROS.py @@ -26,16 +26,14 @@ class globalPathServer(object): """Global is published everytime there is a request for global path over /get_global_path topic""" - def __init__(self, world = " ", ns = " ",source=0,destination=14): + def __init__(self, world = " ", player = " ", ns = " ",source=0,destination=14): # super(GlobalPathServer, self).__init__() self.ns = ns # Get topology from the map _map = world.get_map() - actor_list=world.get_actors() - for actor in actor_list.filter('vehicle.lincoln.mkz2017'): - self.player = actor + self.player = player self.throttle = 0 self.brake = 0 self.steering = 0 diff --git a/main_carla.py b/main_carla.py index 392a2f8..816062e 100644 --- a/main_carla.py +++ b/main_carla.py @@ -646,7 +646,7 @@ def main_loop(args): print(snode,dnode) - node = globalPathServer(world.world,'carla',snode,dnode) + node = globalPathServer(world.world,world.player,'carla',snode,dnode) # node.plot() # r = rospy.Rate(10) clock = pygame.time.Clock() From 07352c3c538e9db198fc0d97fafa52582bf10749 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Sat, 30 Mar 2019 20:36:30 -0400 Subject: [PATCH 04/10] Control Carla ego-vehicle using onboard planner and controller --- GlobalPathCarla2ROS.py | 43 +++++++++++++++++++++++----- launch/mkz_goal_pose.launch | 57 +++++++++++++++++++++++++++++++++++++ main_carla.py | 16 +++++++---- navigate.py | 4 +-- 4 files changed, 105 insertions(+), 15 deletions(-) create mode 100644 launch/mkz_goal_pose.launch diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py index 7fa65a6..c94bda0 100644 --- a/GlobalPathCarla2ROS.py +++ b/GlobalPathCarla2ROS.py @@ -1,9 +1,11 @@ import rospy from std_msgs.msg import String -from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist, PoseWithCovariance, TwistStamped from nav_msgs.msg import Path from dbw_mkz_msgs.msg import ThrottleCmd,BrakeCmd,SteeringCmd +from nav_msgs.msg import Odometry + from tf.transformations import quaternion_from_euler import carla @@ -53,10 +55,10 @@ def __init__(self, world = " ", player = " ", ns = " ",source=0,destination=14): # and carla uses left handed coordinated system # https://math.stackexchange.com/questions/2626961/how-to-convert-a-right-handed-coordinate-system-to-left-handed - print "shorted path...",self.p + # print "shorted path...",self.p for i in range(len(self.p)): self.p[i][1] = -self.p[i][1] - print "shorted path...",self.p + # print "shorted path...",self.p # self.plot() rospy.init_node('{}_path_server'.format(self.ns), anonymous = True) rospy.Subscriber('{}/get_global_path'.format(self.ns), String, self.callback_update) @@ -65,8 +67,11 @@ def __init__(self, world = " ", player = " ", ns = " ",source=0,destination=14): rospy.Subscriber('carla/SteeringCmd', SteeringCmd, self.callback_steering) self.path_publisher = rospy.Publisher('{}/global_path'.format(self.ns), Path, queue_size = 10) + self.odom_publisher = rospy.Publisher('{}/odom'.format(self.ns), Odometry, queue_size = 10) + self.speed_publisher = rospy.Publisher('{}/speed'.format(self.ns), TwistStamped, queue_size = 10) self.path = Path() + self.makePathMessage() @@ -80,7 +85,7 @@ def makePathMessage(self): # frame_id - global poses = [] - for i in range(len(carla_path)-1): + for i in range(min(len(carla_path)-1,9000)): # posestamped required header to be set... # Sequence Number - increase every time this function is called @@ -103,8 +108,8 @@ def makePathMessage(self): pose = Pose() pose.position.x = carla_path[-1][0] pose.position.y = carla_path[-1][1] - pose.position.z = 0; - pose.orientation = self.directionFromTwoPointsQuaternion(carla_path[-1],carla_path[-1]) + pose.position.z = 0 + pose.orientation = self.directionFromTwoPointsQuaternion(carla_path[-2],carla_path[-1]) posestamped.pose = pose poses.append(posestamped) @@ -126,12 +131,36 @@ def callback_brake(self, msg): self.throttle = 0 def callback_steering(self, msg): - self.steering = msg.steering_wheel_angle_cmd + self.steering = - msg.steering_wheel_angle_cmd/8.2 def apply_control(self): control_cmd = carla.VehicleControl(throttle=self.throttle,brake=self.brake,steer=self.steering) self.player.apply_control(control_cmd) + def publish_odom(self,odom): + out_odom = Odometry() + out_odom.pose.pose.position.x = odom.location.x + out_odom.pose.pose.position.y = -odom.location.y + out_odom.pose.pose.position.z = odom.location.z + + quat = quaternion_from_euler(odom.rotation.roll/180*np.pi,odom.rotation.pitch/180*np.pi,-odom.rotation.yaw/180*np.pi) + # quat = quaternion_from_euler(0,0,-odom.rotation.yaw/180*np.pi) + # print(odom.rotation.yaw/180*np.pi) + out_odom.pose.pose.orientation.x = quat[0] + out_odom.pose.pose.orientation.y = quat[1] + out_odom.pose.pose.orientation.z = quat[2] + out_odom.pose.pose.orientation.w = quat[3] + + self.odom_publisher.publish(out_odom) + + def publish_path(self): + self.path_publisher.publish(self.path) + + def publish_speed(self,speed): + twist = TwistStamped() + twist.twist.linear.x = speed/3.6 + self.speed_publisher.publish(twist) + def plot(self): mapk = self.id_map.keys() srcind = self.id_map.values().index(self.source) diff --git a/launch/mkz_goal_pose.launch b/launch/mkz_goal_pose.launch new file mode 100644 index 0000000..37fe71d --- /dev/null +++ b/launch/mkz_goal_pose.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/main_carla.py b/main_carla.py index 816062e..79f59b1 100644 --- a/main_carla.py +++ b/main_carla.py @@ -163,7 +163,7 @@ def restart(self): # spawn_points = self.map.get_spawn_points() # spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() spawn_point = carla.Transform(carla.Location(x=97.2789, y=63.1175, z=1.8431), carla.Rotation(pitch=0, yaw=-10.4166, roll=0)) - print("spawn_point...",spawn_point) + # print("spawn_point...",spawn_point) self.player = self.world.try_spawn_actor(blueprint, spawn_point) # Set up the sensors. @@ -635,7 +635,7 @@ def main_loop(args): topology,waypoints = get_topology(world.map) - print(type(topology)) + # print(type(topology)) graph,id_map = build_graph(topology) points = np.array(id_map.keys()) @@ -644,7 +644,7 @@ def main_loop(args): snode = id_map[tuple(points[ind])] dnode = id_map[random.choice(id_map.keys())] - print(snode,dnode) + # print(snode,dnode) node = globalPathServer(world.world,world.player,'carla',snode,dnode) # node.plot() @@ -654,9 +654,13 @@ def main_loop(args): # while True: clock.tick_busy_loop(20) - # # if controller.parse_events(client, world, clock): - # # return - # print('render...') + odom = world.player.get_transform() + # print(odom.location) + velocity = world.player.get_velocity() + speed = np.linalg.norm([velocity.x,velocity.y,velocity.z]) + node.publish_odom(odom) + node.publish_speed(speed) + node.publish_path() node.apply_control() world.tick(clock) world.render(display) diff --git a/navigate.py b/navigate.py index 304874b..7c6ef05 100644 --- a/navigate.py +++ b/navigate.py @@ -22,7 +22,7 @@ def __init__(self,ns = " "): self.points = [] rospy.init_node('{}_navigation_node'.format(self.ns), anonymous = True) - print '{}/global_path'.format(self.ns) + print("carla/global_path") self.global_path_pub = rospy.Publisher('/{}/get_global_path'.format(self.ns),String,queue_size = 10) time.sleep(0.5) # rospy.Subscriber('{}/global_path'.format(self.ns), Path, self.callback_gp) @@ -36,7 +36,7 @@ def __init__(self,ns = " "): # for i in data.poses: # self.points.append([i.pose.position.x,i.pose.position.y]) def get_points(self): - print "waiting for global path..." + print("waiting for global path...") data = rospy.wait_for_message('{}/global_path'.format(self.ns), Path) for i in data.poses: From 2b88b712682e8828b65f33aadad68c576b40b616 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Sun, 31 Mar 2019 17:46:11 -0400 Subject: [PATCH 05/10] bug fix --- GlobalPathCarla2ROS.py | 2 +- launch/mkz_goal_pose.launch | 75 +++++++++------ stash/get_path.py | 52 ---------- stash/main_topology.py | 92 ------------------ stash/purepursuit.py | 187 ------------------------------------ 5 files changed, 47 insertions(+), 361 deletions(-) delete mode 100644 stash/get_path.py delete mode 100644 stash/main_topology.py delete mode 100644 stash/purepursuit.py diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py index c94bda0..b602cb9 100644 --- a/GlobalPathCarla2ROS.py +++ b/GlobalPathCarla2ROS.py @@ -158,7 +158,7 @@ def publish_path(self): def publish_speed(self,speed): twist = TwistStamped() - twist.twist.linear.x = speed/3.6 + twist.twist.linear.x = speed self.speed_publisher.publish(twist) def plot(self): diff --git a/launch/mkz_goal_pose.launch b/launch/mkz_goal_pose.launch index 37fe71d..905e7a9 100644 --- a/launch/mkz_goal_pose.launch +++ b/launch/mkz_goal_pose.launch @@ -1,38 +1,55 @@ - + - - - + + + - - - - - - - - - - - + + + + + + + + + + + - - - - - + + + + + + + + - + + + + + + + + + + + + + @@ -43,15 +60,15 @@ - + - - + + - + - + diff --git a/stash/get_path.py b/stash/get_path.py deleted file mode 100644 index a50f47f..0000000 --- a/stash/get_path.py +++ /dev/null @@ -1,52 +0,0 @@ -import sys -try: - sys.path.append('../PythonAPI') -except IndexError: - pass - -import random - -import carla -from agents.navigation.agent import * -from agents.navigation.local_planner import LocalPlanner -from agents.navigation.local_planner import compute_connection, RoadOption -from agents.navigation.global_route_planner import GlobalRoutePlanner -from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO -from agents.tools.misc import vector - -def main(): - - client = carla.Client('localhost',2000) - client.set_timeout(2.0) - - world = client.get_world() - _map = world.get_map() - dao = GlobalRoutePlannerDAO(_map) - - grp = GlobalRoutePlanner(dao) - grp.setup() - - blueprint_library = world.get_blueprint_library() - - # vehicle - blueprint = random.choice(world.get_blueprint_library().filter('vehicle.lin*')) - - spawn_points = world.get_map().get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - - _vehicle = world.try_spawn_actor(blueprint, spawn_point) - print(_vehicle) - start_waypoint = _map.get_waypoint(_vehicle.get_location()) - end_waypoint = random.choice(spawn_points) if spawn_points else carla.Transform() - - # Obtain route plan - x1 = start_waypoint.transform.location.x - y1 = start_waypoint.transform.location.y - x2 = end_waypoint.location.x - y2 = end_waypoint.location.y - print(x1,x2,y1,y2) - _graph,_id_map = grp.build_graph() - print(_graph) - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/stash/main_topology.py b/stash/main_topology.py deleted file mode 100644 index 2838894..0000000 --- a/stash/main_topology.py +++ /dev/null @@ -1,92 +0,0 @@ -import carla -import time -import random - -import matplotlib.pyplot as plt -import numpy as np -import time - -import networkx as nx -import math - -from get_topology import * - - -def main(): - - client = carla.Client('localhost',2000) - client.set_timeout(2.0) - - world = client.get_world() - - blueprint_library = world.get_blueprint_library() - - - # vehicle - vehicle = blueprint_library.filter('vehicle.lin*') - vehicle = vehicle[0] - # get one of the possible spawning locations - # transform = random.choice(world.get_map().get_spawn_points()) - # print transform - # plt.show() - - - - # Get topology from the map - _map = world.get_map() - - - # Build waypoint graph - topology,waypoints = get_topology(_map) - # print topology[0].keys()#,type(topology[0]) - - - xs = waypoints[:,0] - ys = waypoints[:,1] - graph,id_map = build_graph(topology) - e1 = graph.edges()[0] - e1_data = graph.get_edge_data(e1[0],e1[1]) - - source_location = e1_data['entry'] - waypoint_next_to_source = e1_data['path'][0] - - source_vector = e1_data['entry_vector'] - - - source_yaw = np.degrees(np.arctan2(source_vector[1],source_vector[0])) - - transform = carla.Transform(carla.Location(x=source_location[0], y=source_location[1], z=2), carla.Rotation(yaw=source_yaw)) - print transform.location.x - vehicle = world.spawn_actor(vehicle,transform) - - - - - # print graph.get_edge_data() - - axes = plt.gca() - axes.set_xlim(-500, 500) - axes.set_ylim(-500, +500) - line, = axes.plot(xs, ys, 'r*') - - p = get_shortest_path(graph, 0, 14) - mapk = id_map.keys() - srcind = id_map.values().index(0) - destind = id_map.values().index(14) - source = mapk[srcind] - dest = mapk[destind] - - plt.plot(source[0],source[1],'go--', linewidth=2, markersize=12) - plt.plot(dest[0],dest[1],'ro--', linewidth=2, markersize=12) - - # print len(final_path_x),len(final_path_y) - plt.plot(p[:,0],p[:,1]) - plt.show() - - - - - # plt.show() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/stash/purepursuit.py b/stash/purepursuit.py deleted file mode 100644 index eefe5aa..0000000 --- a/stash/purepursuit.py +++ /dev/null @@ -1,187 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2018 Intel Labs. -# authors: German Ros (german.ros@intel.com) -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" This module contains PID controllers to perform lateral and longitudinal control. """ - -from collections import deque -import math - -import numpy as np - -import carla -from agents.tools.misc import distance_vehicle, get_speed - -class VehiclePIDController(): - """ - VehiclePIDController is the combination of two PID controllers (lateral and longitudinal) to perform the - low level control a vehicle from client side - """ - - def __init__(self, vehicle, - args_lateral={'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}, - args_longitudinal={'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}): - """ - :param vehicle: actor to apply to local planner logic onto - :param args_lateral: dictionary of arguments to set the lateral PID controller using the following semantics: - K_P -- Proportional term - K_D -- Differential term - K_I -- Integral term - :param args_longitudinal: dictionary of arguments to set the longitudinal PID controller using the following - semantics: - K_P -- Proportional term - K_D -- Differential term - K_I -- Integral term - """ - self._vehicle = vehicle - self._world = self._vehicle.get_world() - self._lon_controller = PIDLongitudinalController( - self._vehicle, **args_longitudinal) - self._lat_controller = PIDLateralController( - self._vehicle, **args_lateral) - - def run_step(self, target_speed, waypoint): - """ - Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint - at a given target_speed. - - :param target_speed: desired vehicle speed - :param waypoint: target location encoded as a waypoint - :return: distance (in meters) to the waypoint - """ - throttle = self._lon_controller.run_step(target_speed) - steering = self._lat_controller.run_step(waypoint) - - control = carla.VehicleControl() - control.steer = steering - control.throttle = throttle - control.brake = 0.0 - control.hand_brake = False - control.manual_gear_shift = False - - return control - - -class PIDLongitudinalController(): - """ - PIDLongitudinalController implements longitudinal control using a PID. - """ - - def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): - """ - :param vehicle: actor to apply to local planner logic onto - :param K_P: Proportional term - :param K_D: Differential term - :param K_I: Integral term - :param dt: time differential in seconds - """ - self._vehicle = vehicle - self._K_P = K_P - self._K_D = K_D - self._K_I = K_I - self._dt = dt - self._e_buffer = deque(maxlen=30) - - def run_step(self, target_speed, debug=False): - """ - Execute one step of longitudinal control to reach a given target speed. - - :param target_speed: target speed in Km/h - :return: throttle control in the range [0, 1] - """ - current_speed = get_speed(self._vehicle) - - if debug: - print('Current speed = {}'.format(current_speed)) - - return self._pid_control(target_speed, current_speed) - - def _pid_control(self, target_speed, current_speed): - """ - Estimate the throttle of the vehicle based on the PID equations - - :param target_speed: target speed in Km/h - :param current_speed: current speed of the vehicle in Km/h - :return: throttle control in the range [0, 1] - """ - _e = (target_speed - current_speed) - self._e_buffer.append(_e) - - if len(self._e_buffer) >= 2: - _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt - _ie = sum(self._e_buffer) * self._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip((self._K_P * _e) + (self._K_D * _de / self._dt) + (self._K_I * _ie * self._dt), 0.0, 1.0) - - -class PIDLateralController(): - """ - PIDLateralController implements lateral control using a PID. - """ - - def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): - """ - :param vehicle: actor to apply to local planner logic onto - :param K_P: Proportional term - :param K_D: Differential term - :param K_I: Integral term - :param dt: time differential in seconds - """ - self._vehicle = vehicle - self._K_P = K_P - self._K_D = K_D - self._K_I = K_I - self._dt = dt - self._e_buffer = deque(maxlen=10) - - def run_step(self, waypoint): - """ - Execute one step of lateral control to steer the vehicle towards a certain waypoin. - - :param waypoint: target waypoint - :return: steering control in the range [-1, 1] where: - -1 represent maximum steering to left - +1 maximum steering to right - """ - return self._pid_control(waypoint, self._vehicle.get_transform()) - - def _pid_control(self, waypoint, vehicle_transform): - """ - Estimate the steering angle of the vehicle based on the PID equations - - :param waypoint: target waypoint - :param vehicle_transform: current transform of the vehicle - :return: steering control in the range [-1, 1] - """ - v_begin = vehicle_transform.location - v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), - y=math.sin(math.radians(vehicle_transform.rotation.yaw))) - - v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([waypoint.transform.location.x - - v_begin.x, waypoint.transform.location.y - - v_begin.y, 0.0]) - _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / - (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) - - _cross = np.cross(v_vec, w_vec) - if _cross[2] < 0: - _dot *= -1.0 - - self._e_buffer.append(_dot) - if len(self._e_buffer) >= 2: - _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt - _ie = sum(self._e_buffer) * self._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip((self._K_P * _dot) + (self._K_D * _de / - self._dt) + (self._K_I * _ie * self._dt), -1.0, 1.0) From 6a6c72b29b1b6ee1d761dd096b94a76322ccf300 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Mon, 1 Apr 2019 09:21:54 -0400 Subject: [PATCH 06/10] Add waypoint marks to Carla --- GlobalPathCarla2ROS.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py index b602cb9..5d49540 100644 --- a/GlobalPathCarla2ROS.py +++ b/GlobalPathCarla2ROS.py @@ -50,6 +50,14 @@ def __init__(self, world = " ", player = " ", ns = " ",source=0,destination=14): self.p = get_shortest_path(self.graph, self.source, self.destination) + for i in range(self.p.shape[0]): + wp = carla.Location(self.p[i][0],self.p[i][1],self.p[i][2]) + world.debug.draw_point(wp, size=0.1, color=carla.Color(0, 255, 0), life_time=300.0,persistent_lines=True) + # print(wp) + + + + # Because ROS Uses right handed coordinate system # and carla uses left handed coordinated system From eed849f7faeca696e565027ef90031c79430f3e8 Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Mon, 1 Apr 2019 13:35:17 -0400 Subject: [PATCH 07/10] Tuning parameters Changes to be committed: modified: launch/mkz_goal_pose.launch --- launch/mkz_goal_pose.launch | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/launch/mkz_goal_pose.launch b/launch/mkz_goal_pose.launch index 905e7a9..6fb0dd0 100644 --- a/launch/mkz_goal_pose.launch +++ b/launch/mkz_goal_pose.launch @@ -43,8 +43,8 @@ - - + + @@ -62,10 +62,11 @@ - - + + + - + From 54afcf5e2b48a451ab6d5332a7c480f76282f2cd Mon Sep 17 00:00:00 2001 From: "yechenglyu@gmail.com" Date: Mon, 1 Apr 2019 19:20:13 -0400 Subject: [PATCH 08/10] Add VLP-16 LiDAR support --- GlobalPathCarla2ROS.py | 24 +++++++++++++++++++++--- main_carla.py | 41 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+), 3 deletions(-) diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py index 5d49540..d80ab7d 100644 --- a/GlobalPathCarla2ROS.py +++ b/GlobalPathCarla2ROS.py @@ -2,10 +2,9 @@ import rospy from std_msgs.msg import String from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist, PoseWithCovariance, TwistStamped -from nav_msgs.msg import Path +from nav_msgs.msg import Path, Odometry +from sensor_msgs.msg import PointCloud2, PointField from dbw_mkz_msgs.msg import ThrottleCmd,BrakeCmd,SteeringCmd -from nav_msgs.msg import Odometry - from tf.transformations import quaternion_from_euler import carla @@ -13,6 +12,7 @@ import random import sys import threading +import itertools import matplotlib.pyplot as plt import numpy as np @@ -77,6 +77,7 @@ def __init__(self, world = " ", player = " ", ns = " ",source=0,destination=14): self.path_publisher = rospy.Publisher('{}/global_path'.format(self.ns), Path, queue_size = 10) self.odom_publisher = rospy.Publisher('{}/odom'.format(self.ns), Odometry, queue_size = 10) self.speed_publisher = rospy.Publisher('{}/speed'.format(self.ns), TwistStamped, queue_size = 10) + self.LiDAR_publisher = rospy.Publisher('{}/LiDAR'.format(self.ns), PointCloud2, queue_size = 10) self.path = Path() self.makePathMessage() @@ -169,6 +170,23 @@ def publish_speed(self,speed): twist.twist.linear.x = speed self.speed_publisher.publish(twist) + def publish_LiDAR(self,points): + msg = PointCloud2() + msg.height = 1 + msg.width = len(points) + msg.fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] + msg.is_bigendian = False + msg.point_step = 12 + msg.row_step = msg.point_step * len(points) + pc = np.zeros([len(points),3]).astype(np.float32) + for row, pt in itertools.izip(pc, points): + row[:] = [pt.x,-pt.y,-pt.z] + msg.data = pc.tostring() + msg.header.frame_id = 'map' + self.LiDAR_publisher.publish(msg) + def plot(self): mapk = self.id_map.keys() srcind = self.id_map.values().index(self.source) diff --git a/main_carla.py b/main_carla.py index 79f59b1..3dc590b 100644 --- a/main_carla.py +++ b/main_carla.py @@ -167,6 +167,7 @@ def restart(self): self.player = self.world.try_spawn_actor(blueprint, spawn_point) # Set up the sensors. + self.LiDAR_sensor = LiDAR_Sensor(self.player) self.collision_sensor = CollisionSensor(self.player, self.hud) self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) self.gnss_sensor = GnssSensor(self.player) @@ -201,6 +202,7 @@ def destroy(self): self.collision_sensor.sensor, self.lane_invasion_sensor.sensor, self.gnss_sensor.sensor, + self.LiDAR_sensor.sensor, self.player] for actor in actors: if actor is not None: @@ -493,6 +495,44 @@ def _on_gnss_event(weak_self, event): self.lon = event.longitude +# ============================================================================== +# -- LiDAR Sensor -------------------------------------------------------- +# ============================================================================== + + +class LiDAR_Sensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.points = None + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.lidar.ray_cast') + bp.set_attribute('channels','16') + bp.set_attribute('range','8000') + bp.set_attribute('upper_fov','15.0') + bp.set_attribute('lower_fov','-15.0') + bp.set_attribute('sensor_tick','0.1') + # bp.set_attribute('points_per_second','36000') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1, z=1.7)), attach_to=self._parent) + # print(self.position) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LiDAR_Sensor._on_obst_event(weak_self, event)) + + @staticmethod + def _on_obst_event(weak_self, lidar_measurement): + self = weak_self() + if not self: + return + # print(lidar_measurement[0].x) + if len(lidar_measurement): + self.points = lidar_measurement + else: + self.points = None + # self.lat = event.latitude + # self.lon = event.longitude + # ============================================================================== # -- CameraManager ------------------------------------------------------------- # ============================================================================== @@ -661,6 +701,7 @@ def main_loop(args): node.publish_odom(odom) node.publish_speed(speed) node.publish_path() + node.publish_LiDAR(world.LiDAR_sensor.points) node.apply_control() world.tick(clock) world.render(display) From 78d529735128a249bbed46b14b9785f3c3d54d0b Mon Sep 17 00:00:00 2001 From: "yechenglyu@gmail.com" Date: Tue, 2 Apr 2019 16:47:28 -0400 Subject: [PATCH 09/10] bug fix: rename topic name in onboard system Changes to be committed: modified: launch/mkz_goal_pose.launch --- launch/mkz_goal_pose.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/mkz_goal_pose.launch b/launch/mkz_goal_pose.launch index 6fb0dd0..adf5db7 100644 --- a/launch/mkz_goal_pose.launch +++ b/launch/mkz_goal_pose.launch @@ -34,7 +34,7 @@ - + From 5b56706e95f8f2744bdb44992035bc79779e2d0d Mon Sep 17 00:00:00 2001 From: Yecheng Lyu Date: Thu, 4 Apr 2019 12:36:37 -0400 Subject: [PATCH 10/10] Update mkz_goal_pose.launch --- launch/mkz_goal_pose.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/mkz_goal_pose.launch b/launch/mkz_goal_pose.launch index adf5db7..e0edfcd 100644 --- a/launch/mkz_goal_pose.launch +++ b/launch/mkz_goal_pose.launch @@ -8,7 +8,7 @@ - + @@ -44,7 +44,7 @@ - +