Skip to content

Commit

Permalink
Merge pull request #11 from StanfordMSL/planner_bugfix
Browse files Browse the repository at this point in the history
Closes #7 #6 #4
  • Loading branch information
exoticDFT authored Nov 18, 2020
2 parents 317e9df + 6514008 commit 1a107d5
Show file tree
Hide file tree
Showing 9 changed files with 465 additions and 246 deletions.
38 changes: 15 additions & 23 deletions launch/car_simulation.launch
Original file line number Diff line number Diff line change
Expand Up @@ -2,45 +2,37 @@

<launch>

<arg name="rolename" default="stanford_ego" />
<arg name="steps" default="20" />
<arg name="rolename" default="hero" />
<arg name="track_steps" default="300" />
<arg name="distance" default="4.0" />
<arg name="color" default="255, 200, 200" />
<arg name="spawn_point" default="-6.0, -40.0, 2.0, 0.0, 0.0, 270.0" />
<arg name="max_speed" default="5.0" />
<arg name="max_speed" default="2.0" />
<arg name="opp_speed" default="5.0" />
<arg name="plan_horizon" default="5.0" />
<arg name="plan_horizon" default="3.0" />
<arg name="plan_steps" default="10" />
<arg name="time_step" value="$(eval arg('plan_horizon')/arg('plan_steps'))" />

<group ns="$(arg rolename)">

<!-- start service call for game node -->
<node name="restart_call" pkg="carla_circle" type="game_start_client.py" output="screen" />

<!-- "Perception" node - currently filters out vehicles outside ROI-->
<node name="game_ado_car_filter" pkg="carla_circle" type="game_ado_vehicle_filter.py" output="screen">
<param name="rolename" value="$(arg rolename)" />
</node>

<!-- Planner node - works for game planner to publish current track
<!-- Mapper node - publish local track
center and track width for two cars -->
<node name="map_updater" pkg="carla_circle" type="map_updater.py" output="screen">
<param name="steps" value="$(arg steps)" />
<param name="distance" value="$(arg distance)" />
<param name="rolename" value="$(arg rolename)" />
<param name="steps" value="$(arg track_steps)" />
<param name="update_frequency" value="10.0" />
<param name="exit_time" value="600" />
<param name="max_speed" value="$(arg max_speed)" />
<param name="opp_speed" value="$(arg opp_speed)" />
<param name="plan_horizon" value="$(arg plan_horizon)" />
</node>

<!-- path server updates path according to carla world map -->
<node name="path_server" pkg="carla_circle" type="path_server.py" output="screen">
<param name="steps" value="$(arg steps)" />
<param name="distance" value="$(arg distance)" />
<!-- Planner node -->
<node name="traj_planner" pkg="carla_circle" type="trivial_planner.py" output="screen" >
<param name="rolename" value="$(arg rolename)" />
<param name="time_step" value="$(arg time_step)" />
<param name="horizon" value="$(arg plan_horizon)" />
<param name="max_speed" value="$(arg max_speed)" />
</node>


<!-- Controller node -->
<node name="controller" pkg="carla_circle" type="controller.py" output="screen">
<param name="rolename" value="$(arg rolename)" />
Expand Down Expand Up @@ -71,4 +63,4 @@
<arg name="role_name" value="$(arg rolename)" />
</include>

</launch>
</launch>
14 changes: 1 addition & 13 deletions launch/mcity_event1.launch
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,6 @@
<arg name="fixed_delta_seconds" value="false" />
</include>

<!-- shared by all the cars -->
<node name="car_filter" pkg="carla_circle" type="car_filter.py" output="screen" />

<include file="$(find carla_circle)/launch/car_simulation.launch">
<arg name="rolename" value="$(arg rolename)" />
<arg name="color" value="255, 0, 0" />
Expand All @@ -31,15 +28,6 @@
</include>

<!-- visualization -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find carla_ros_bridge)/config/carla_default_rviz.cfg.rviz" />

<!-- game planner -->
<include file="$(find uavgame)/launch/main_mpc_gurobi.launch">
<arg name="role_name" value="$(arg rolename)" />
<arg name="high_vel" value="$(arg max_speed)" />
<arg name="low_vel" value="$(arg opp_speed)" />
<arg name="plan_horizon" value="$(arg plan_horizon)" />
<arg name="plan_steps" value="$(arg plan_steps)" />
</include>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find carla_circle)/resource/carla_default_rviz.cfg.rviz" />

</launch>
260 changes: 260 additions & 0 deletions resource/carla_default_rviz.cfg.rviz
Original file line number Diff line number Diff line change
@@ -0,0 +1,260 @@
Panels:
- Class: rviz/Displays
Help Height: 75
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Ego trajectory1
Splitter Ratio: 0.6000000238418579
Tree Height: 366
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
- /ThirdPersonFollower1
- /ThirdPersonFollower2
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
- Class: rviz/Displays
Help Height: 75
Name: Displays
Property Tree Widget:
Expanded:
- /EgoOdometry1/Shape1
- /Origin1
Splitter Ratio: 0.44411763548851013
Tree Height: 481
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: EgoOdometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 1
Head Radius: 1
Shaft Length: 3
Shaft Radius: 1
Value: Arrow
Topic: /carla/hero/odometry
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /hero/carla/viz/des_waypoints
Name: Ego trajectory
Namespaces:
"": true
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /hero/carla/viz/ego_track
Name: Ego Track
Namespaces:
"": true
Queue Size: 100
Value: true
- Alpha: 0.5
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Global Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /hero/global_plan
Unreliable: false
Value: true
- Class: rviz/Axes
Enabled: true
Length: 10
Name: Origin
Radius: 0.5
Reference Frame: map
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /hero/carla/viz/road_topo
Name: Road Topology
Namespaces:
"": true
Queue Size: 100
Value: true
- Alpha: 0.5
Cell Size: 10
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: map
Value: true
Enabled: true
Global Options:
Background Color: 80; 80; 80
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/ThirdPersonFollower
Distance: 68.55451965332031
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.6602022647857666
Target Frame: hero
Value: ThirdPersonFollower (rviz)
Yaw: 4.739986896514893
Saved:
- Class: rviz/ThirdPersonFollower
Distance: 200
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: ThirdPersonFollower
Near Clip Distance: 0.009999999776482582
Pitch: 0.20000000298023224
Target Frame: hero
Value: ThirdPersonFollower (rviz)
Yaw: 3.140000104904175
- Class: rviz/ThirdPersonFollower
Distance: 50
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: ThirdPersonFollower
Near Clip Distance: 0.009999999776482582
Pitch: 0.5
Target Frame: hero
Value: ThirdPersonFollower (rviz)
Yaw: 3.140000104904175
Window Geometry:
Displays:
collapsed: false
Height: 808
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001ab00000259fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000000d7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003d000000e80000000000000000fb0000000a0049006d006100670065010000003d000002590000000000000000fb0000000a0049006d00610067006501000001680000012e0000000000000000fb0000000a0049006d00610067006501000002b800000131000000000000000000000001000001fb00000286fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d00000286000000e60100001cfa000000000100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb0000000a00560069006500770073010000063a0000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054b00000042fc0100000002fb0000000800540069006d006501000000000000054b000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000034a0000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1355
X: 204
Y: 27
Loading

0 comments on commit 1a107d5

Please sign in to comment.