Path tracking working

TODO:

Trajectory generation
Throttle command introduction
Location from GPS points

Author: vvrs

GlobalPathCarla2ROS.py

@@ -20,8 +20,8 @@
 
 from get_topology import *
 
-plt.ion()
-plt.show()
+# plt.ion()
+# plt.show()
 
 class globalPathServer(object):
 	"""Global is published everytime there is a request for global path over /get_global_path topic"""
@@ -40,8 +40,18 @@ def __init__(self, world = " ", ns = " ",source=0,destination=14):
 		self.source = source
 		self.destination = destination
 
-		self.p = get_shortest_path(self.graph, self.source, self.destination) 
-		self.plot()
+		self.p = get_shortest_path(self.graph, self.source, self.destination)
+
+
+		# 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 
+		for i in range(len(self.p)):
+			self.p[i][1] = -self.p[i][1]
+		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)
 		self.path_publisher = rospy.Publisher('{}/global_path'.format(self.ns), Path, queue_size = 10)
@@ -109,7 +119,8 @@ def plot(self):
 
 	@staticmethod
 	def  directionFromTwoPointsQuaternion(p1,p2):
-		angle_ = np.arctan2(p2[1]-p1[1],p2[0]-p1[0])
+		
+		angle_ = np.arctan2(p2[1]-p1[1],p2[0]-p1[0]) 
 		q = quaternion_from_euler(0, 0, angle_)
 		quat_msg = Quaternion(q[0], q[1], q[2], q[3])
 		return quat_msg

Modifications

@@ -0,0 +1,2 @@
+ros-bridge/src/carla_ros_bridge/transforms.py
+	line 33: -carla_location.y, -> carla_location.y

controller.py

@@ -4,10 +4,10 @@
 import numpy as np
 import time
 import sys
-
+# import cutils
 
 import rospy
-from std_msgs.msg import String
+from std_msgs.msg import String, Float64
 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, Twist
 from nav_msgs.msg import Path, Odometry
 from tf.transformations import quaternion_from_euler, euler_from_quaternion
@@ -17,7 +17,7 @@
 logging.basicConfig(level=logging.DEBUG, format='%(asctime)s - %(levelname)s - %(message)s')
 
 global debug
-debug = 1
+debug = 0
 class closestPoint:
 	def __init__(self, points):
 		self.points = points
@@ -48,12 +48,12 @@ def __init__(self,points):
 		time.sleep(0.5)
 		# rospy.init_node("purePursuit_node",anonymous=True)
 
-		self.look_ahead_distance = 10
+		self.look_ahead_distance = 5
 
-		self.target_speed = 1
+		self.target_speed = 5
 
-		self.k = 0.1
-		self.kp = 1
+		self.k = 0.
+		self.kp = 0.2
 
 		self.init_xy = False
 		self.init_heading = False
@@ -72,19 +72,24 @@ def __init__(self,points):
 		# Initialize vehicle controller message
 		# data = rospy.wait_for_message("/carla/ego_vehicle/odometry", Odometry)
 		# print data
-		# rospy.Subscriber("/carla/ego_vehicle/odometry", Odometry, self.callback_odom)
+		rospy.Subscriber("/carla/ego_vehicle/odometry", Odometry, self.callback_odom)
 		self.cmd_pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=1)
 
+		self.goalxpub = rospy.Publisher('/goalx',Float64, queue_size=10)
+		self.goalypub = rospy.Publisher('/goaly',Float64, queue_size=10)
+
 		self.points = points
 
 
 	def callback_odom(self,data):
 
-		rospy.loginfo(__func__)
+		# rospy.loginfo(__func__)
 		self.car_current_x = data.pose.pose.position.x
 		self.car_current_y = data.pose.pose.position.y
 
-		self.current_speed = data.twist.twist.linear.x
+		current_speed_x = data.twist.twist.linear.x
+		current_speed_y = data.twist.twist.linear.y
+		self.current_speed = np.sqrt(current_speed_x**2 + current_speed_y**2)
 
 		quaternion = [data.pose.pose.orientation.x,\
 					   data.pose.pose.orientation.y,\
@@ -98,22 +103,24 @@ def callback_odom(self,data):
 	def _controller(self):
 
 		############################## FIX #########################################
-		data = rospy.wait_for_message("/carla/ego_vehicle/odometry", Odometry)
+		# data = rospy.wait_for_message("/carla/ego_vehicle/odometry", Odometry)
 
-		# print data
+		# # print data
 
-		self.car_current_x = data.pose.pose.position.x
-		self.car_current_y = data.pose.pose.position.y
+		# self.car_current_x = data.pose.pose.position.x
+		# self.car_current_y = data.pose.pose.position.y
 
-		self.current_speed = data.twist.twist.linear.x
+		# current_speed_x = data.twist.twist.linear.x
+		# current_speed_y = data.twist.twist.linear.y
+		# self.current_speed = np.sqrt(current_speed_x**2 + current_speed_y**2)
 
-		quaternion = [data.pose.pose.orientation.x,\
-					   data.pose.pose.orientation.y,\
-					   data.pose.pose.orientation.z,\
-					   data.pose.pose.orientation.w]
-		euler = euler_from_quaternion(quaternion)
-		# euler = tf.transformations.euler_from_quaternion(quaternion)
-		self.car_current_heading = euler[2]
+		# quaternion = [data.pose.pose.orientation.x,\
+		# 			   data.pose.pose.orientation.y,\
+		# 			   data.pose.pose.orientation.z,\
+		# 			   data.pose.pose.orientation.w]
+		# euler = euler_from_quaternion(quaternion)
+		# # euler = tf.transformations.euler_from_quaternion(quaternion)
+		# self.car_current_heading = euler[2]
 		#############################################################################
 
 		path = closestPoint(self.points)
@@ -139,25 +146,27 @@ def _controller(self):
 		near_x = self.points[(ind)%len(self.points)][0]
 		near_y = self.points[(ind)%len(self.points)][1]
 
-		# self.goalxpub.publish(goal_x)
-		# self.goalypub.publish(goal_y)
+		self.goalxpub.publish(goal_x)
+		self.goalypub.publish(goal_y)
 
 		distance = sqrt((self.car_current_x-goal_x)**2 + (self.car_current_y-goal_y)**2)
+		distance_final = sqrt((self.car_current_x-self.points[-1][0])**2 + (self.car_current_y-self.points[-1][1])**2)
 		distance2 = sqrt((near_x-goal_x)**2 + (near_y-goal_y)**2)
-
-		print "Car current position :: ",self.car_current_x,self.car_current_y
-		print "Goal position :: ",goal_x,goal_y
-		print "Nearest point on the path:: ",near_x,near_y
-		print "Distance to goal :: ",distance
-		print "Look ahead distance:: ",distance2
+		print "distance_final : ",distance_final
 
 		desired_pose = atan2((goal_y-self.car_current_y),(goal_x-self.car_current_x))
 
-		print "desired_pose:: {0} ({1}) ".format(desired_pose,degrees(desired_pose))
-		print "current heading:: {0} ({1}) ".format(self.car_current_heading,degrees(self.car_current_heading))
 		error = (desired_pose - self.car_current_heading)
 
-		print "heading error:: {0} ({1}) ".format(error,degrees(error))
+		if debug:
+			print "Car current position :: ",self.car_current_x,self.car_current_y
+			print "Goal position :: ",goal_x,goal_y
+			print "Nearest point on the path:: ",near_x,near_y
+			print "Distance to goal :: ",distance
+			print "Look ahead distance:: ",distance2
+			print "desired_pose:: {0} ({1}) ".format(desired_pose,degrees(desired_pose))
+			print "current heading:: {0} ({1}) ".format(self.car_current_heading,degrees(self.car_current_heading))
+			print "heading error:: {0} ({1}) ".format(error,degrees(error))
 
 		# if error < -3.14159:
 		# 	# print "error is below -pi"
@@ -172,15 +181,21 @@ def _controller(self):
 		L = 2.6
 		ld = 10
 		kl = 1
-		self.throttle = self._PIDControl(self.target_speed,self.current_speed)
+		# self.throttle = self._PIDControl(self.target_speed,self.current_speed)
 		# self.throttle = min(self.throttle,0.2)
+		if(distance_final < 1):
+			self.throttle = 0;
+		self.throttle = 5;
 		# self.steering = np.arctan(2*L*sin(error)/(kl*self.throttle))
 		self.steering_ratio = 1
 		self.steering = self.steering_ratio * np.arctan2(2*L*sin(error),Lf)
 		# self.steering = error
 
 
 	def _PIDControl(self,target, current):
+		print "\n"
+		print "_PIDControl: ", target-current
+		print "\n"
 		a = self.kp * (target - current)
 		return a
 
@@ -191,7 +206,8 @@ def publish_(self):
 		ai = self.throttle
 		di = self.steering 
 
-		print "throttle and steering angle :: ",ai,di
+		# print "throttle and steering angle :: ",di
+		print "speed and steering angle :: ",ai,di
 		print "\n\n"
 
 		ackermann_cmd_msg = AckermannDrive()

main_carla.py

@@ -645,7 +645,7 @@ def main_loop(args):
 		print(snode,dnode)
 
 		node = globalPathServer(world.world,'carla',snode,dnode)
-		node.plot()
+		# node.plot()
 		r = rospy.Rate(10)
 		# while not rospy.is_shutdown():
 		while  True:

navigate.py

@@ -71,7 +71,7 @@ def PlotData(self, x, y):
 		self.xdata.append(x)
 		self.ydata.append(y)
 		self.x = x
-		self.y = y
+		self.y = -y
 		self.lines.set_data(self.xdata,self.ydata)
 		self.ax.relim()
 		self.ax.autoscale_view()