Controller working... check driving mode

Author: vvrs

GlobalPathCarla2ROS.py

@@ -20,6 +20,8 @@
 
 from get_topology import *
 
+plt.ion()
+plt.show()
 
 class globalPathServer(object):
 	"""Global is published everytime there is a request for global path over /get_global_path topic"""
@@ -102,7 +104,8 @@ def plot(self):
 		plt.plot(dest[0],dest[1],'ro--', linewidth=2, markersize=12)
 
 		plt.plot(self.p[:,0],self.p[:,1])
-		plt.show()
+		plt.draw()
+		plt.pause(0.001)
 
 	@staticmethod
 	def  directionFromTwoPointsQuaternion(p1,p2):

controller.py

@@ -80,20 +80,24 @@ def __init__(self,points):
 
 	def callback_odom(self,data):
 
-
+		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.linear.x
+		self.current_speed = data.twist.twist.linear.x
 
-		quaternion = data.pose.orientation
-		euler = tf.transformations.euler_from_quaternion(quaternion)
+		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]
 
 
 	def _controller(self):
 
-		############################### FIX #########################################
+		############################## FIX #########################################
 		data = rospy.wait_for_message("/carla/ego_vehicle/odometry", Odometry)
 
 		# print data
@@ -110,7 +114,7 @@ def _controller(self):
 		euler = euler_from_quaternion(quaternion)
 		# euler = tf.transformations.euler_from_quaternion(quaternion)
 		self.car_current_heading = euler[2]
-		##############################################################################
+		#############################################################################
 
 		path = closestPoint(self.points)
 		ind = path.closest_node((self.car_current_x,self.car_current_y))

get_topology.py

@@ -59,13 +59,18 @@ def get_topology(map):
 			x = j.transform.location.x
 			y = j.transform.location.y
 			z = j.transform.location.z
+
+			#logging
+			print x,y,z
 			xs.append(x)
 			ys.append(y)
 			zs.append(z)
 	xs = np.array(xs).reshape((len(xs),1))
 	ys = np.array(ys).reshape((len(ys),1))
 	zs = np.array(zs).reshape((len(zs),1))
 
+	# logging
+	# print ys,zs
 
 	waypoints = np.hstack((xs,ys,zs))
 

main_carla.py

@@ -40,7 +40,7 @@
 
 from __future__ import print_function
 
-# import rospy
+import rospy
 
 import carla
 from carla import ColorConverter as cc
@@ -641,9 +641,9 @@ def main_loop(args):
 
 		print(snode,dnode)
 
-		# node = globalPathServer(world.world,'carla',snode,dnode)
-		# node.plot()
-		# r = rospy.Rate(10)
+		node = globalPathServer(world.world,'carla',snode,dnode)
+		node.plot()
+		r = rospy.Rate(10)
 		# while not rospy.is_shutdown():
 		while  True:
 			clock = pygame.time.Clock()
@@ -654,7 +654,7 @@ def main_loop(args):
 			world.tick(clock)
 			world.render(display)
 			pygame.display.flip()
-			# r.sleep()
+			r.sleep()
 
 	finally:
 

navigate.py

@@ -5,11 +5,14 @@
 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 nav_msgs.msg import Path,Odometry
 
 import numpy as np
 
 import controller as cn 
+import matplotlib.pyplot as plt 
+plt.ion()
+plt.show()
 
 class Navigation(object):
 	"""docstring for Navigation"""
@@ -41,17 +44,60 @@ def get_points(self):
 		self.global_path_pub.publish("abc")
 		return np.array(self.points)
 
-		
+class DynamicUpdate():
+	def __init__(self,points):
+		global i
+		self.xdata=[]
+		self.ydata=[]
+		self.x = None
+		self.y = None
+		self.points = points
+
+		# print self.points
+		self.fig, self.ax = plt.subplots(1, 1)
+		self.ax.axis('equal')
+		self.ax.plot(self.points[:,0],self.points[:,1])
+		if(bool(len(self.xdata))):
+			self.ax.plot(self.xdata[-1],self.ydata[-1],'ro')
+		self.lines, = self.ax.plot(self.xdata,self.ydata,color='g', linewidth=2.0)
+		self.ax.set_autoscaley_on(True)
+		self.ax.set_xlabel('X (m)')
+		self.ax.set_ylabel('Y (m)')
+		# self.ax.set_ylim([-40.0, 40.0])
+		# self.ax.set_xlim([-40.0, 40.0])
+		self.ax.grid()
+
+	def PlotData(self, x, y):
+		self.xdata.append(x)
+		self.ydata.append(y)
+		self.x = x
+		self.y = y
+		self.lines.set_data(self.xdata,self.ydata)
+		self.ax.relim()
+		self.ax.autoscale_view()
+		#We need to draw *and* flush
+		self.fig.canvas.draw()
+		self.fig.canvas.flush_events()		
 def main(argv):
 
 	# namespace - 'carla'
 	node = Navigation('carla')
 	points = node.get_points()
+	plt.plot(points[:,0],points[:,1])
+
+	# plt.draw()
+	# plt.pause(0.001)
 	# print points
 	controller_object = cn.purePursuit(points)
+	plot = DynamicUpdate(points)
 	r = rospy.Rate(50)
 
 	while not rospy.is_shutdown():
+		pos=rospy.wait_for_message("/carla/ego_vehicle/odometry",Odometry)
+
+		x=pos.pose.pose.position.x
+		y=pos.pose.pose.position.y
+		plot.PlotData(x,y)
 		controller_object.publish_()
 		r.sleep()
 

plot2d.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d.axes3d as p3
+import rospy
+from nav_msgs.msg import Odometry
+#from nav_msgs.msg import Odometry
+import numpy as np
+import rospy
+from std_msgs.msg import String, Float64
+from geometry_msgs.msg import Twist, Vector3Stamped
+
+points = []
+
+x = 0
+while x<=2*np.pi:
+    # points.append((40*((1-np.sin(x))*np.cos(x)),40*(1-np.sin(x))))
+    points.append((20*np.sin(x),20*np.cos(x)))
+    x += 0.05
+
+
+i=0
+points = np.array(points)
+plt.ion()
+class DynamicUpdate():
+    def __init__(self,points):
+        global i
+        self.xdata=[]
+        self.ydata=[]
+        self.x = None
+        self.y = None
+        self.points = points
+
+        # print self.points
+        self.fig, self.ax = plt.subplots(1, 1)
+        self.ax.axis('equal')
+        self.ax.plot(self.points[:,0],self.points[:,1])
+        if(bool(len(self.xdata))):
+        	self.ax.plot(self.xdata[-1],self.ydata[-1],'ro')
+        self.lines, = self.ax.plot(self.xdata,self.ydata,color='g', linewidth=2.0)
+        self.ax.set_autoscaley_on(True)
+        self.ax.set_xlabel('X (m)')
+        self.ax.set_ylabel('Y (m)')
+        # self.ax.set_ylim([-40.0, 40.0])
+        # self.ax.set_xlim([-40.0, 40.0])
+        self.ax.grid()
+
+    def PlotData(self, x, y):
+        global i
+        i+=1
+        self.xdata.append(x)
+        self.ydata.append(y)
+        self.x = x
+        self.y = y
+        self.lines.set_data(self.xdata,self.ydata)
+        self.ax.relim()
+        self.ax.autoscale_view()
+        #We need to draw *and* flush
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+
+
+
+rospy.init_node("plot",anonymous=True)
+position_vector = rospy.wait_for_message("/vehicle/perfect_gps/utm",Vector3Stamped)
+
+init_x = position_vector.vector.x
+init_y = position_vector.vector.y
+points = points + np.array([init_x,init_y])
+plot = DynamicUpdate(points)
+
+
+while not rospy.is_shutdown():
+    pos=rospy.wait_for_message("/vehicle/perfect_gps/utm",Vector3Stamped)
+
+    x=pos.vector.x
+    y=pos.vector.y
+    plot.PlotData(x,y)