diff --git a/GlobalPathCarla2ROS.py b/GlobalPathCarla2ROS.py
index befe378..d80ab7d 100644
--- a/GlobalPathCarla2ROS.py
+++ b/GlobalPathCarla2ROS.py
@@ -1,8 +1,10 @@
import rospy
from std_msgs.msg import String
-from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist
-from nav_msgs.msg import Path
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist, PoseWithCovariance, TwistStamped
+from nav_msgs.msg import Path, Odometry
+from sensor_msgs.msg import PointCloud2, PointField
+from dbw_mkz_msgs.msg import ThrottleCmd,BrakeCmd,SteeringCmd
from tf.transformations import quaternion_from_euler
import carla
@@ -10,6 +12,7 @@
import random
import sys
import threading
+import itertools
import matplotlib.pyplot as plt
import numpy as np
@@ -25,13 +28,18 @@
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()
+ self.player = player
+ 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)
@@ -42,21 +50,39 @@ def __init__(self, world = " ", 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
# 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)
+ 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.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()
+
+
def makePathMessage(self):
@@ -68,7 +94,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
@@ -91,8 +117,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)
@@ -103,6 +129,64 @@ 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/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
+ 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/launch/mkz_goal_pose.launch b/launch/mkz_goal_pose.launch
new file mode 100644
index 0000000..e0edfcd
--- /dev/null
+++ b/launch/mkz_goal_pose.launch
@@ -0,0 +1,75 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/main_carla.py b/main_carla.py
index 3d3ad42..3dc590b 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,13 +160,14 @@ 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)
+ # print("spawn_point...",spawn_point)
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)
@@ -199,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:
@@ -491,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 -------------------------------------------------------------
# ==============================================================================
@@ -633,7 +675,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())
@@ -642,22 +684,29 @@ 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,'carla',snode,dnode)
+ node = globalPathServer(world.world,world.player,'carla',snode,dnode)
# node.plot()
- r = rospy.Rate(10)
- # while not rospy.is_shutdown():
- while True:
- clock = pygame.time.Clock()
- clock.tick_busy_loop(60)
- # # if controller.parse_events(client, world, clock):
- # # return
- # print('render...')
+ # r = rospy.Rate(10)
+ clock = pygame.time.Clock()
+ while not rospy.is_shutdown():
+ # while True:
+
+ clock.tick_busy_loop(20)
+ 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.publish_LiDAR(world.LiDAR_sensor.points)
+ node.apply_control()
world.tick(clock)
world.render(display)
pygame.display.flip()
- r.sleep()
+ # r.sleep()
finally:
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:
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)