Make p3iv metpackage

.gitignore

@@ -2,4 +2,4 @@
 setup.py
 *.pyc
 CMakeLists.txt.user
-/outputs/**
+

LICENSE

@@ -0,0 +1,11 @@
+Copyright 2020 FZI Forschungszentrum Informatik
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

p3iv/.gitignore

@@ -0,0 +1,5 @@
+*~
+setup.py
+*.pyc
+CMakeLists.txt.user
+/outputs/**

p3iv/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(p3iv)
+find_package(catkin REQUIRED)
+catkin_metapackage()

clean.sh → p3iv/clean.sh

@@ -1,4 +1,4 @@
 #!/usr/bin/env bash
 
 echo Cleaning...
-rm -rf ./../../outputs/*
+rm -rf ./outputs/*

p3iv/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>p3iv</name>
+  <version>0.0.0</version>
+  <description>Meta pkg for Probabilistic Prediction and Planning for Intelligent Vehicles Simulator</description>
+
+  <license>BSD</license>
+  <maintainer email="[email protected]">Sahin Tas</maintainer>
+  <author email="[email protected]">Sahin Tas</author>
+  <url type="repository">https://gitlab.mrt.uni-karlsruhe.de/planning-simulation/p3iv</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>p3iv_core</exec_depend>
+  <exec_depend>p3iv_types</exec_depend>
+  <exec_depend>p3iv_modules</exec_depend>
+  <exec_depend>p3iv_utils</exec_depend>
+  <exec_depend>p3iv_visualization</exec_depend>
+
+  <export>
+    <metapackage />
+  </export>
+</package>

scripts/main.py → p3iv/scripts/main.py

@@ -15,10 +15,9 @@
 from p3iv_utils.consoleprint import Print2Console
 from p3iv_utils.ofstream import create_output_dir, create_output_path, save_settings
 from p3iv_utils.lanelet_map_reader import lanelet_map_reader
-from p3iv.types.vehicle import Vehicle
-from p3iv.execute import drive, predict
-from p3iv.configurations.test_cases import test_cases
-from p3iv.configurations.utils import load_configurations
+from p3iv_types.vehicle import Vehicle
+from p3iv_modules.execute import drive, predict
+from p3iv_core.configurations.utils import load_configurations
 
 
 def run(configurations, f_execute=drive):
@@ -38,9 +37,9 @@ def run(configurations, f_execute=drive):
 
     # Get ground-truth object data
     if configurations["source"] == "interaction_sim":
-        from p3iv.bindings import InteractionDatasetBindings
+        from p3iv_core.bindings import InteractionDatasetBindings
 
-        bindings = InteractionDatasetBindings(configurations["map"], configurations["temporal"]["dt"])
+        bindings = InteractionDatasetBindings(configurations["map"], configurations["interaction_dataset_dir"], configurations["temporal"]["dt"])
         environment_model = bindings.get_environment_model(configurations["timestamp_begin"])
         ground_truth = bindings.create_ground_truth(environment_model.objects(), laneletmap, configurations)
         assert configurations["vehicle_of_interest"] in ground_truth.keys()

p3iv_core/.gitlab-ci.yml

@@ -0,0 +1,5 @@
+#version=1.6
+include:
+   - project: 'pub/mrt_build_config'
+     ref: master
+     file: '/ci_templates/default_catkin_project.yml'

CMakeLists.txt → p3iv_core/CMakeLists.txt

@@ -1,7 +1,7 @@
 set(MRT_PKG_VERSION 4.0.0)
 # Modify only if you know what you are doing!
 cmake_minimum_required(VERSION 3.5.1)
-project(p3iv)
+project(p3iv_core)
 
 ###################
 ## Find packages ##

p3iv_core/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>p3iv_core</name>
+  <version>0.0.0</version>
+  <description>Core functions of p3iv</description>
+
+  <license>MRT</license>
+  <maintainer email="[email protected]">Sahin Tas</maintainer>
+  <author email="[email protected]">Sahin Tas</author>
+  <url type="repository"></url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>mrt_cmake_modules</build_depend>
+  <test_depend>gtest</test_depend>
+
+
+  <depend>p3iv_utils</depend>
+  <depend>p3iv_types</depend>
+
+
+</package>

p3iv_core/setup.py

@@ -0,0 +1,13 @@
+## THIS FILE HAS BEEN AUTOGENERATED FOR USE WITH CATKIN AND IS NOT SUPPOSED TO BE TRACKED BY GIT!
+## DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(package_xml_path='/home/tas/00_workspaces/catkin_ws/motion_planning_sim/src/p3iv/p3iv_core',
+    packages=['p3iv_core'],
+    package_data={'': ['']},
+    package_dir={'': 'src'})
+
+setup(**setup_args)

src/p3iv/bindings/interaction_dataset/data_converter.py → p3iv_core/src/p3iv_core/bindings/interaction_dataset/data_converter.py

@@ -3,8 +3,7 @@
 import warnings
 from matplotlib import colors as mcolors
 from external.dataset_types import Track
-from p3iv.types.state import VehicleState
-from util_motion.motion import Motion
+from p3iv_types.state import VehicleState
 
 
 def get_color(index):

src/p3iv/bindings/interaction_dataset/main.py → p3iv_core/src/p3iv_core/bindings/interaction_dataset/main.py

@@ -1,16 +1,16 @@
 import warnings
-from p3iv.types.vehicle import Vehicle
-from p3iv.types.ground_truth import GroundTruth
-from p3iv.types.environment_model import EnvironmentModel
-from p3iv.modules import VehicleModules
+from p3iv_types.vehicle import Vehicle
+from p3iv_types.ground_truth import GroundTruth
+from p3iv_types.environment_model import EnvironmentModel
+from p3iv_modules.modules import VehicleModules
 from p3iv_utils.consoleprint import Print2Console
 from track_reader import track_reader
 from data_converter import DataConverter
 
 
 class InteractionDatasetBindings(object):
-    def __init__(self, track_name, dt):
-        track_dictionary = track_reader(track_name)
+    def __init__(self, track_name, interaction_dataset_dir, dt):
+        track_dictionary = track_reader(track_name, interaction_dataset_dir)
         self._dataset_handler = DataConverter(int(dt), track_dictionary)
 
     def get_environment_model(self, timestamp):

src/p3iv/bindings/interaction_dataset/track_reader.py → p3iv_core/src/p3iv_core/bindings/interaction_dataset/track_reader.py

@@ -7,13 +7,9 @@
 from external import dict_utils
 
 
-def track_reader(
-    scenario_name,
-    track_file_number=0,
-    catkin_ws_rel_dir="../../../../../INTERACTION-Dataset-DR-v1_0/recorded_trackfiles",
-):
+def track_reader(scenario_name, interaction_dataset_dir, track_file_number=0):
 
-    tracks_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), catkin_ws_rel_dir)
+    tracks_dir = os.path.join(interaction_dataset_dir, "recorded_trackfiles")
     error_string = ""
 
     scenario_dir = tracks_dir + "/" + scenario_name

src/p3iv/configurations/settings.py → p3iv_core/src/p3iv_core/configurations/settings.py

@@ -1,6 +1,6 @@
 import os
 
-ws_path = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../../.."))
+ws_path = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../../../.."))
 ws_path = os.path.realpath(ws_path)
 
 settings = {
@@ -24,7 +24,7 @@
         "process_noise": 0.5,
     },
     "prediction": {
-        "set_ground_truh_values": True,
+        "set_ground_truh_values": False,
         "multi_modal": False,  # True for ITSC'18 settings, False for IV'18 settings
         "politeness_factor": 0.5,
         "deceleration_comfortable": -5.0,

src/p3iv/configurations/utils.py → p3iv_core/src/p3iv_core/configurations/utils.py

@@ -4,7 +4,7 @@
 
 def get_settings():
 
-    from p3iv.configurations.settings import settings
+    from p3iv_core.configurations.settings import settings
 
     settings["start_time"] = datetime.now().strftime("%m/%d/%Y, %H:%M:%S")
 
@@ -17,7 +17,7 @@ def get_settings():
 
 def load_configurations(test_case_id):
 
-    from p3iv.configurations.test_cases import test_cases
+    from p3iv_core.configurations.test_cases import test_cases
 
     try:
         configurations = test_cases[test_case_id]

p3iv_core/src/p3iv_core/fooo.py

@@ -0,0 +1,121 @@
+# -*- coding: utf-8 -*-
+from __future__ import division
+import os
+import pickle
+import time
+from pyfiglet import Figlet
+from datetime import datetime
+from pprint import pprint
+import sys
+import logging
+import traceback
+from termcolor import colored
+import itertools
+import shutil
+from p3iv_utils.consoleprint import Print2Console
+from p3iv_utils.ofstream import create_output_dir, create_output_path, save_settings
+from p3iv_utils.lanelet_map_reader import lanelet_map_reader
+from p3iv.types.vehicle import Vehicle
+from p3iv.execute import drive, predict
+from p3iv.configurations.test_cases import test_cases
+from p3iv.configurations.utils import load_configurations
+
+
+def run(configurations, f_execute=drive):
+
+    # Print system time
+    Print2Console.p("ss", ["Analysis start time:", time.ctime()], style="bright")
+
+    # Print settings
+    Print2Console.p("s", ["=" * 72], style="magenta", bold=True)
+    Print2Console.p("s", ["Simulation configurations:"], style="magenta", bold=True)
+    Print2Console.p("s", ["=" * 72], style="magenta", bold=True)
+    pprint(configurations)
+
+    # Load lanelet2 map
+    maps_dir = os.path.join(configurations["interaction_dataset_dir"], "maps")
+    laneletmap = lanelet_map_reader(configurations["map"], maps_dir=maps_dir)
+
+    # Get ground-truth object data
+    if configurations["source"] == "interaction_sim":
+        from p3iv.bindings import InteractionDatasetBindings
+
+        bindings = InteractionDatasetBindings(configurations["map"], configurations["temporal"]["dt"])
+        environment_model = bindings.get_environment_model(configurations["timestamp_begin"])
+        ground_truth = bindings.create_ground_truth(environment_model.objects(), laneletmap, configurations)
+        assert configurations["vehicle_of_interest"] in ground_truth.keys()
+    else:
+        raise Exception("Specify ground truth object data!")
+
+    # Extract timestamps to be computed
+    timestamps = range(
+        configurations["timestamp_begin"], configurations["timestamp_end"] + 1, configurations["temporal"]["dt"]
+    )
+
+    # Perform computation
+    for i, ts_now in enumerate(timestamps):
+        # Print information
+        Print2Console.p("s", ["=" * 72], style="magenta", bold=True)
+        Print2Console.p("sf", ["Computing timestamp:", ts_now], first_col_w=38, style="magenta", bold=True)
+        Print2Console.p("s", ["=" * 72], style="magenta", bold=True)
+
+        # update planned motion from previous solution or from dataset
+        if configurations["simulation_type"] == "open-loop" or i == 0:
+            # update ground truth objects
+            bindings.update_open_loop_simulation(ground_truth, ts_now, laneletmap, configurations)
+
+        elif configurations["simulation_type"] == "semi-open-loop":
+            # update ground truth objects
+            bindings.update_open_loop_simulation(ground_truth, ts_now, laneletmap, configurations)
+
+            o = ground_truth[configurations["vehicle_of_interest"]]
+            past_motion = o.timestamps.previous().motion[1:]
+            driven = o.timestamps.previous().plan_optimal.motion[1]
+            o.timestamps.latest().motion = past_motion
+            o.timestamps.latest().motion.append(driven)
+
+        elif configurations["simulation_type"] == "closed-loop":
+            # closed-loop simulation
+            # (ground truth object list remains the same; no new entries)
+            for v in ground_truth.values():
+                past_motion = v.timestamps.latest().motion[1:]
+                # planned trajectory includes past three points and the current;
+                # Those extra three points are trimmed away in Plan().
+                # Therefore, take the first element in the motion array.
+                driven = v.timestamps.latest().plan_optimal.motion[1]
+                v.timestamps.create_and_add(ts_now)
+                v.timestamps.latest().motion = past_motion
+                v.timestamps.latest().motion.append(driven)
+        else:
+            msg = (
+                "'simulation_type' in configurations is wrong."
+                + "Choose between 'open-loop' / 'closed-loop' / 'semi-open-loop'"
+            )
+            raise Exception(msg)
+
+        # Compute the trajectory of vehicles who have a 'toLanelet' in their **objective**!
+        for vehicle in [_v for _v in ground_truth.vehicles() if _v.objective.toLanelet]:
+            try:
+                f_execute(vehicle, ground_truth)
+
+                # plot results
+                curr_save_dir = os.path.join(configurations["save_dir"], str(ts_now), str(vehicle.v_id))
+                os.makedirs(curr_save_dir)
+
+                # Update vehicle data
+                ground_truth.update(vehicle)
+            except:
+                traceback.print_exc()
+                msg = "Simulation terminated before timestamp " + str(configurations["timestamp_end"])
+                msg += "\nThere may be a problem in calculations. "
+                msg += "\nMaybe the vehicle has reached its destination?"
+                print colored(msg, "red")
+                break
+        else:
+            continue
+        break
+
+    Print2Console.p("s", ["=" * 72], style="magenta", bold=True)
+    Print2Console.p("s", ["Simulation completed!"], style="magenta", bold=True)
+    Print2Console.p("s", ["=" * 72], style="magenta", bold=True)
+    return ground_truth

p3iv_modules/.gitignore

@@ -0,0 +1,4 @@
+*~
+setup.py
+*.pyc
+CMakeLists.txt.user

p3iv_modules/.gitlab-ci.yml

@@ -0,0 +1,5 @@
+#version=1.6
+include:
+   - project: 'pub/mrt_build_config'
+     ref: master
+     file: '/ci_templates/default_catkin_project.yml'