Merge pull request #119 from bark-simulator/scenario_generation_unifo…

…rm_ego_route Scenario generation uniform ego route
bark-simulator · Sep 19, 2019 · 5668170 · 5668170
2 parents 74cd252 + af968a7
commit 5668170
Show file tree

Hide file tree

Showing 8 changed files with 105 additions and 29 deletions.
diff --git a/examples/params/uct_planner.json b/examples/params/uct_planner.json
@@ -53,41 +53,41 @@
         "DumpTree": false,
         "MotionPrimitiveInputs": [
             [
-                1.0,
+                4.0,
                 0.0
             ],
             [
-                -1.0,
+                -4.0,
                 0.0
             ],
             [
                 0.0,
-                0.27
+                -0.13
             ],
             [
                 0.0,
-                -0.27
+                0.13
             ],
             [
-                0.0,
+                -1.0,
                 -0.13
             ],
             [
-                0.0,
+                -1.0,
                 0.13
             ],
             [
-                0.0,
-                -0.03
+                1.0,
+                -0.13
             ],
             [
-                0.0,
-                0.03
+                1.0,
+                0.13
             ]
         ],
         "ReturnLowerBound": -1000.0,
         "MaxSearchTime": 20000,
-        "MaxSearchTimeRandomHeuristic": 10.0,
+        "MaxSearchTimeRandomHeuristic": 100.0,
         "ReturnUpperBound": 100.0,
         "MaxNumIterationsRandomHeuristic": 1000.0,
         "MaxNumIterations": 50000,
@@ -100,9 +100,18 @@
         "Generation": {
             "UniformVehicleDistribution": {
                 "MapFilename": "modules/runtime/tests/data/city_highway_straight.xodr",
+                "EgoGoalStart": [
+                    5111.626,
+                    5006.8305
+                ],
                 "EgoGoalEnd": [
-                    5128,
-                    5200
+                    5111.626,
+                    5193.1725
+                ],
+                "EgoGoalStateLimits": [
+                    0.5,
+                    0,
+                    0
                 ],
                 "EgoRoute": [
                     [
@@ -116,7 +125,7 @@
                 ],
                 "OthersSource": [
                     [
-                        5000.626,
+                        5111.626,
                         5006.8305
                     ]
                 ],
@@ -127,12 +136,16 @@
                     ]
                 ],
                 "VehicleDistanceRange": [
-                    2,
+                    10,
                     20
                 ],
-                "VehicleVelocityRange": [
+                "OtherVehicleVelocityRange": [
                     2,
-                    15
+                    8
+                ],
+                "EgoVehicleVelocityRange": [
+                    10,
+                    12
                 ],
                 "VehicleModel": {
                     "behavior_model": "BehaviorIDMClassic",

diff --git a/examples/planner_uct.py b/examples/planner_uct.py
@@ -23,7 +23,7 @@
 scenario_generation = UniformVehicleDistribution(num_scenarios=1, random_seed=0, params=param_server)
 
 
-viewer = MPViewer(params=param_server, x_range=[-20,20], y_range=[-10,30], follow_agent_id=True)
+viewer = MPViewer(params=param_server, x_range=[5060, 5160], y_range=[5070,5150])
 sim_step_time = param_server["simulation"]["step_time",
                                         "Step-time used in simulation",
                                         0.2]
@@ -37,7 +37,7 @@
 
 # world_state.agents[scenario._eval_agent_ids[0]].behavior_model
 
-video_renderer = VideoRenderer(renderer=viewer, world_step_time=sim_step_time, render_intermediate_steps=10)
+video_renderer = VideoRenderer(renderer=viewer, world_step_time=sim_step_time)
 for _ in range(0, 40): # run scenario for 100 steps
     world_state.do_planning(sim_step_time)
     video_renderer.drawWorld(world_state, scenario._eval_agent_ids)

diff --git a/modules/models/behavior/longitudinal_acceleration/longitudinal_acceleration.cpp b/modules/models/behavior/longitudinal_acceleration/longitudinal_acceleration.cpp
@@ -52,8 +52,9 @@ dynamic::Trajectory behavior::BehaviorLongitudinalAcceleration::Plan(
       traj(i, StateDefinition::X_POSITION) = boost::geometry::get<0>(traj_point); // checked
       traj(i, StateDefinition::Y_POSITION) = boost::geometry::get<1>(traj_point); // checked
       traj(i, StateDefinition::THETA_POSITION) = traj_angle; // checked
-      traj(i, StateDefinition::VEL_POSITION) = std::max(std::min(current_vel, max_velocity), min_velocity); // checked
-      current_vel = current_vel + acceleration * sample_time;
+      traj(i, StateDefinition::VEL_POSITION) = current_vel; // checked
+      const float temp_velocity = current_vel + acceleration * sample_time;
+      current_vel = std::max(std::min(temp_velocity, max_velocity), min_velocity);
       run_time += sample_time;
     }
 

diff --git a/modules/runtime/scenario/scenario_generation/uniform_vehicle_distribution.py b/modules/runtime/scenario/scenario_generation/uniform_vehicle_distribution.py
@@ -68,7 +68,11 @@ def initialize_params(self, params):
       "Distance range between vehicles in meter given as tuple from which" + \
       "distances are sampled uniformly",
       (10, 20)]
-    self._velocity_range = params_temp["VehicleVelocityRange",
+    self._other_velocity_range = params_temp["OtherVehicleVelocityRange",
+      "Lower and upper bound of velocity in km/h given as tuple from which" + \
+      " velocities are sampled uniformly",
+      (20,30)]
+    self._ego_velocity_range = params_temp["EgoVehicleVelocityRange",
       "Lower and upper bound of velocity in km/h given as tuple from which" + \
       " velocities are sampled uniformly",
       (20,30)]
@@ -137,7 +141,7 @@ def create_single_scenario(self):
         sego = self.sample_srange_uniform([s_start, s_end])
         xy_point =  get_point_at_s(connecting_center_line, sego)
         angle = get_tangent_angle_at_s(connecting_center_line, sego)
-        velocity = self.sample_velocity_uniform(self._velocity_range)
+        velocity = self.sample_velocity_uniform(self._ego_velocity_range)
         agent_state = np.array([0, xy_point.x(), xy_point.y(), angle, velocity ])
 
         agent_params = self._agent_params.copy()
@@ -172,6 +176,7 @@ def create_single_scenario(self):
         goal_polygon = goal_polygon.translate(Point2d(self._ego_goal_end[0],
                                                     self._ego_goal_end[1]))
         ego_agent.goal_definition = GoalDefinitionPolygon(goal_polygon)
+        ego_agent.generate_local_map()
     else:
         connecting_center_line, s_start, s_end, _, lane_id_end = \
         self.center_line_between_source_and_sink(world.map,
@@ -213,7 +218,7 @@ def place_agents_along_linestring(self,
       xy_point =  get_point_at_s(linestring, s)
       angle = get_tangent_angle_at_s(linestring, s)
 
-      velocity = self.sample_velocity_uniform(self._velocity_range)
+      velocity = self.sample_velocity_uniform(self._other_velocity_range)
       agent_state = np.array([0, xy_point.x(), xy_point.y(), angle, velocity ])
 
       agent_params = self._agent_params.copy()

diff --git a/modules/runtime/tests/data/city_highway_curved.xodr b/modules/runtime/tests/data/city_highway_curved.xodr
@@ -0,0 +1,56 @@
+<?xml version="1.0" standalone="yes"?>
+<OpenDRIVE>
+    <header revMajor="1" revMinor="4" name="" version="1.00" date="Thu Jun  6 10:01:32 2019" north="0.0000000000000000e+00" south="0.0000000000000000e+00" east="0.0000000000000000e+00" west="0.0000000000000000e+00">
+    </header>
+    <road name="" length="2.5000000000000000e+02" id="16" junction="-1">
+        <link>
+        </link>
+        <type s="0.0000000000000000e+00" type="town"/>
+        <planView>
+            <geometry s="0.0000000000000000e+00" x="5.1126832348793750e+03" y="5.0549844643744773e+03" hdg="1.5707963267948966e+00" length="2.5000000000000000e+04">
+               <spiral curvStart="-0.0000000000000000e+00" curvEnd="-1.2698412698412698"/>
+            </geometry>
+        </planView>
+        <elevationProfile>
+            <elevation s="0.0000000000000000e+00" a="0.0000000000000000e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
+        </elevationProfile>
+        <lateralProfile>
+        </lateralProfile>
+        <lanes>
+            <laneSection s="0.0000000000000000e+00">
+                <left>
+                    <lane id="1" type="driving" level= "0">
+                        <link>
+                        </link>
+                        <width sOffset="0.0000000000000000e+00" a="3.6000000000000001e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="1.4999999999999999e-01" laneChange="none" height="1.9999999552965164e-02">
+                        </roadMark>
+                    </lane>
+                </left>
+                <center>
+                    <lane id="0" type="driving" level= "0">
+                        <link>
+                        <roadMark sOffset="0.0000000000000000e+00" type="broken" weight="standard" color="standard" width="1.4999999999999999e-01" laneChange="none" height="1.9999999552965164e-02">
+                        </roadMark>
+                        </link>
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level= "0">
+                        <link>
+                        </link>
+                        <width sOffset="0.0000000000000000e+00" a="3.6000000000000001e+00" b="0.0000000000000000e+00" c="0.0000000000000000e+00" d="0.0000000000000000e+00"/>
+                        <roadMark sOffset="0.0000000000000000e+00" type="solid" weight="standard" color="standard" width="1.4999999999999999e-01" laneChange="none" height="1.9999999552965164e-02">
+                        </roadMark>
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+        <objects>
+        </objects>
+        <signals>
+        </signals>
+        <surface>
+        </surface>
+    </road>
+</OpenDRIVE>
diff --git a/modules/world/objects/agent.cpp b/modules/world/objects/agent.cpp
@@ -47,8 +47,9 @@ goal_definition_(goal_definition) {
   pair.second = modules::models::behavior::Action(
           modules::models::behavior::DiscreteAction(0)); // Initially select a DiscreteAction  of zero
   history_.push_back(pair);
+
   if(map_interface != nullptr) {
-    set_goal_definition(goal_definition);
+    GenerateLocalMap();
   }
 }
 
@@ -116,13 +117,16 @@ bool Agent::AtGoal() const {
 }
 
 void Agent::GenerateLocalMap() {
+  local_map_->set_goal_definition(goal_definition_);
   State agent_state = get_current_state();
   Point2d agent_xy(agent_state(StateDefinition::X_POSITION),
                    agent_state(StateDefinition::Y_POSITION));
   if (!local_map_->Generate(agent_xy)) {
     LOG(ERROR) << "LocalMap generation for agent "
               << get_agent_id() << " failed." << std::endl;
   }
+  // TODO(@hart): parameter
+  UpdateDrivingCorridor(20.0);
 }
 
 void Agent::UpdateDrivingCorridor(double horizon = 20.0) {

diff --git a/modules/world/objects/agent.hpp b/modules/world/objects/agent.hpp
@@ -89,10 +89,6 @@ class Agent : public Object {
 
   void set_goal_definition(const GoalDefinitionPtr &goal_definition) {
     goal_definition_ = goal_definition;
-    local_map_->set_goal_definition(goal_definition);
-    GenerateLocalMap();
-    // TODO(@hart): parameter
-    UpdateDrivingCorridor(20.0);
   }
 
   void set_local_map(const modules::world::map::LocalMapPtr& local_map) {local_map_ = local_map; }

diff --git a/python/world/agent.cpp b/python/world/agent.cpp
@@ -65,6 +65,7 @@ void python_agent(py::module m)
       .def_property_readonly("state", &Agent::get_current_state)
       .def_property("goal_definition", &Agent::get_goal_definition, &Agent::set_goal_definition)
       .def("set_agent_id", &Object::set_agent_id)
+      .def("generate_local_map", &Agent::GenerateLocalMap)
       .def(py::pickle(
         [](const Agent& a) -> py::tuple { // __getstate__
             /* Return a tuple that fully encodes the state of the object */