Skip to content

Commit 0bb20da

Browse files
vvrsvvrs
committed
Added live plotting
1 parent eebe8ee commit 0bb20da

File tree

9 files changed

+249
-9
lines changed

9 files changed

+249
-9
lines changed

‎get_topology.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ def get_topology(map):
6161
z = j.transform.location.z
6262

6363
#logging
64-
print x,y,z
64+
# print x,y,z
6565
xs.append(x)
6666
ys.append(y)
6767
zs.append(z)

‎main_carla.py

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -159,7 +159,10 @@ def restart(self):
159159
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
160160
while self.player is None:
161161
spawn_points = self.map.get_spawn_points()
162-
spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
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+
163166
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
164167
# Set up the sensors.
165168
self.collision_sensor = CollisionSensor(self.player, self.hud)
@@ -630,7 +633,7 @@ def main_loop(args):
630633

631634

632635
topology,waypoints = get_topology(world.map)
633-
636+
print(type(topology))
634637
graph,id_map = build_graph(topology)
635638
points = np.array(id_map.keys())
636639

@@ -704,8 +707,8 @@ def main():
704707
argparser.add_argument(
705708
'--filter',
706709
metavar='PATTERN',
707-
default='vehicle.*',
708-
help='actor filter (default: "vehicle.lincoln*")')
710+
default='vehicle.lin*',
711+
help='actor filter (default: "vehicle.lin*")')
709712
args = argparser.parse_args()
710713

711714
args.width, args.height = [int(x) for x in args.res.split('x')]

‎main_topology.py

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,8 @@ def PlotData(self, x, y):
7777
self.ax.autoscale_view()
7878
#We need to draw *and* flush
7979
self.fig.canvas.draw()
80-
self.fig.canvas.flush_events()
80+
self.fig.canvas.flush_events()
81+
8182
def main(argv):
8283

8384
# namespace - 'carla'

‎plot2d.py

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -62,16 +62,17 @@ def PlotData(self, x, y):
6262

6363

6464
rospy.init_node("plot",anonymous=True)
65-
position_vector = rospy.wait_for_message("/vehicle/perfect_gps/utm",Vector3Stamped)
66-
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
6768
init_x = position_vector.vector.x
6869
init_y = position_vector.vector.y
6970
points = points + np.array([init_x,init_y])
7071
plot = DynamicUpdate(points)
7172

7273

7374
while not rospy.is_shutdown():
74-
pos=rospy.wait_for_message("/vehicle/perfect_gps/utm",Vector3Stamped)
75+
pos=rospy.wait_for_message("/vehicle/perfect_gps/utm",Odometry)
7576

7677
x=pos.vector.x
7778
y=pos.vector.y

‎routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz

1.8 KB
Binary file not shown.

‎routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/doc.kml

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<kml xmlns="http://www.opengis.net/kml/2.2">
3+
<Document>
4+
<name>Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA</name>
5+
<Style id="icon-ci-1-nodesc-normal">
6+
<IconStyle>
7+
<scale>0.7</scale>
8+
<Icon>
9+
<href>images/icon-1.png</href>
10+
</Icon>
11+
</IconStyle>
12+
<LabelStyle>
13+
<scale>0</scale>
14+
</LabelStyle>
15+
<BalloonStyle>
16+
<text><![CDATA[<h3>$[name]</h3>]]></text>
17+
</BalloonStyle>
18+
</Style>
19+
<Style id="icon-ci-1-nodesc-highlight">
20+
<IconStyle>
21+
<scale>0.7</scale>
22+
<Icon>
23+
<href>images/icon-1.png</href>
24+
</Icon>
25+
</IconStyle>
26+
<LabelStyle>
27+
<scale>0.7</scale>
28+
</LabelStyle>
29+
<BalloonStyle>
30+
<text><![CDATA[<h3>$[name]</h3>]]></text>
31+
</BalloonStyle>
32+
</Style>
33+
<StyleMap id="icon-ci-1-nodesc">
34+
<Pair>
35+
<key>normal</key>
36+
<styleUrl>#icon-ci-1-nodesc-normal</styleUrl>
37+
</Pair>
38+
<Pair>
39+
<key>highlight</key>
40+
<styleUrl>#icon-ci-1-nodesc-highlight</styleUrl>
41+
</Pair>
42+
</StyleMap>
43+
<Style id="icon-ci-2-nodesc-normal">
44+
<IconStyle>
45+
<scale>0.7</scale>
46+
<Icon>
47+
<href>images/icon-2.png</href>
48+
</Icon>
49+
</IconStyle>
50+
<LabelStyle>
51+
<scale>0</scale>
52+
</LabelStyle>
53+
<BalloonStyle>
54+
<text><![CDATA[<h3>$[name]</h3>]]></text>
55+
</BalloonStyle>
56+
</Style>
57+
<Style id="icon-ci-2-nodesc-highlight">
58+
<IconStyle>
59+
<scale>0.7</scale>
60+
<Icon>
61+
<href>images/icon-2.png</href>
62+
</Icon>
63+
</IconStyle>
64+
<LabelStyle>
65+
<scale>0.7</scale>
66+
</LabelStyle>
67+
<BalloonStyle>
68+
<text><![CDATA[<h3>$[name]</h3>]]></text>
69+
</BalloonStyle>
70+
</Style>
71+
<StyleMap id="icon-ci-2-nodesc">
72+
<Pair>
73+
<key>normal</key>
74+
<styleUrl>#icon-ci-2-nodesc-normal</styleUrl>
75+
</Pair>
76+
<Pair>
77+
<key>highlight</key>
78+
<styleUrl>#icon-ci-2-nodesc-highlight</styleUrl>
79+
</Pair>
80+
</StyleMap>
81+
<Style id="line-1267FF-5000-nodesc-normal">
82+
<LineStyle>
83+
<color>ffff6712</color>
84+
<width>5</width>
85+
</LineStyle>
86+
<BalloonStyle>
87+
<text><![CDATA[<h3>$[name]</h3>]]></text>
88+
</BalloonStyle>
89+
</Style>
90+
<Style id="line-1267FF-5000-nodesc-highlight">
91+
<LineStyle>
92+
<color>ffff6712</color>
93+
<width>7.5</width>
94+
</LineStyle>
95+
<BalloonStyle>
96+
<text><![CDATA[<h3>$[name]</h3>]]></text>
97+
</BalloonStyle>
98+
</Style>
99+
<StyleMap id="line-1267FF-5000-nodesc">
100+
<Pair>
101+
<key>normal</key>
102+
<styleUrl>#line-1267FF-5000-nodesc-normal</styleUrl>
103+
</Pair>
104+
<Pair>
105+
<key>highlight</key>
106+
<styleUrl>#line-1267FF-5000-nodesc-highlight</styleUrl>
107+
</Pair>
108+
</StyleMap>
109+
<Placemark>
110+
<name>Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA</name>
111+
<styleUrl>#line-1267FF-5000-nodesc</styleUrl>
112+
<LineString>
113+
<tessellate>1</tessellate>
114+
<coordinates>
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+
</coordinates>
122+
</LineString>
123+
</Placemark>
124+
<Placemark>
125+
<name>Atwater Kent Laboratories, Worcester, MA 01609, USA</name>
126+
<styleUrl>#icon-ci-1-nodesc</styleUrl>
127+
<Point>
128+
<coordinates>
129+
-71.8076058,42.275317,0
130+
</coordinates>
131+
</Point>
132+
</Placemark>
133+
<Placemark>
134+
<name>151 Salisbury St, Worcester, MA 01609, USA</name>
135+
<styleUrl>#icon-ci-2-nodesc</styleUrl>
136+
<Point>
137+
<coordinates>
138+
-71.8073916,42.2760079,0
139+
</coordinates>
140+
</Point>
141+
</Placemark>
142+
</Document>
143+
</kml>

‎routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/images/icon-1.png

387 Bytes
Loading

‎routes/Directions from Atwater Kent Laboratories, Worcester, MA 01609, USA to 151 Salisbury St, Worcester, MA 01609, USA.kmz_FILES/images/icon-2.png

379 Bytes
Loading

0 commit comments

Comments
 (0)