├── .gitignore ├── Dockerfile.first ├── Dockerfile.second ├── README.md ├── catkin_ws ├── launch │ ├── feature_model.launch │ ├── laneletsmap.launch │ ├── scenario_extraction.launch │ └── validation_dataset.launch └── src │ ├── dataset_tools │ ├── custom_point_types │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── custom_point_types │ │ │ │ ├── point_xyzir.h │ │ │ │ ├── point_xyzirc.h │ │ │ │ ├── point_xyzirl.h │ │ │ │ ├── point_xyzirrgb.h │ │ │ │ ├── point_xyziruv.h │ │ │ │ └── point_xyziruvcov.h │ │ └── package.xml │ ├── dataset_msgs │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── dataset_msgs │ │ │ │ └── blank.hpp │ │ ├── msg │ │ │ ├── DatasetEvent.msg │ │ │ ├── Innovation.msg │ │ │ ├── LocaliserStats.msg │ │ │ └── Observation.msg │ │ └── package.xml │ ├── dataset_playback │ │ ├── CMakeLists.txt │ │ ├── CMakeLists.txt.user │ │ ├── cmake │ │ │ └── Findgraphviz.cmake │ │ ├── destination_files │ │ │ ├── crashlab.yaml │ │ │ └── main_quad.yaml │ │ ├── include │ │ │ └── dataset_playback │ │ │ │ ├── dataset_panel.h │ │ │ │ ├── treeitem.h │ │ │ │ └── treemodel.h │ │ ├── launch │ │ │ └── run.launch │ │ ├── package.xml │ │ └── src │ │ │ ├── dataset_panel.cpp │ │ │ ├── main.cpp │ │ │ ├── treeitem.cpp │ │ │ └── treemodel.cpp │ ├── dataset_tools │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── include │ │ │ └── dataset_tools │ │ │ │ ├── bag_input.hpp │ │ │ │ ├── bag_output.hpp │ │ │ │ ├── behavior_tree_pipeline.hpp │ │ │ │ ├── icp_matcher_pipeline.hpp │ │ │ │ ├── pipeline_components.hpp │ │ │ │ ├── pipeline_images.hpp │ │ │ │ ├── point_cloud_features_pipeline.hpp │ │ │ │ └── run_pipeline.hpp │ │ ├── package.xml │ │ └── src │ │ │ ├── bag_input.cpp │ │ │ ├── bag_output.cpp │ │ │ ├── behavior_tree_pipeline.cpp │ │ │ ├── icp_matcher_pipeline.cpp │ │ │ ├── point_cloud_features_pipeline.cpp │ │ │ └── run_pipeline.cpp │ └── h264_bag_playback │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── config │ │ └── time_offsets.yaml │ │ ├── include │ │ └── h264_bag_playback │ │ │ ├── bag_container.hpp │ │ │ ├── bag_static_transform_publisher.hpp │ │ │ ├── corrected_imu_playback.hpp │ │ │ ├── h264_bag_playback.hpp │ │ │ ├── helper_functions.hpp │ │ │ ├── replace_msg.h │ │ │ └── video.hpp │ │ ├── launch │ │ ├── h264_playback.launch │ │ ├── nodelet_test.launch │ │ └── replaceMsg_writeToBag.launch │ │ ├── nodelet_definition.xml │ │ ├── package.xml │ │ ├── scripts │ │ ├── convert_folder.sh │ │ └── convert_multiple_folders.sh │ │ ├── setup.bash │ │ ├── src │ │ ├── bag_container.cpp │ │ ├── h264_bag_playback.cpp │ │ ├── helper_functions.cpp │ │ ├── replace_msg.cpp │ │ └── video.cpp │ │ └── test │ │ ├── bag_data.h.in │ │ ├── pipeline.bag │ │ └── test_playback.cpp │ ├── extraction │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ ├── extraction.cpp │ │ ├── helper_functions.hpp │ │ └── json.hpp │ ├── feature_model │ ├── CMakeLists.txt │ ├── package.xml │ └── script │ │ ├── feature_model.py │ │ └── rss.py │ ├── gmsl_frame_msg │ ├── CMakeLists.txt │ ├── README.md │ ├── msg │ │ └── FrameInfo.msg │ └── package.xml │ ├── ibeo_object_msg │ ├── CMakeLists.txt │ ├── msg │ │ └── IbeoObject.msg │ └── package.xml │ ├── lane_points_msg │ ├── CMakeLists.txt │ ├── msg │ │ └── LanePoints.msg │ └── package.xml │ └── laneletsmap_generator │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ └── laneletsmap_generator │ │ ├── Conversions.h │ │ ├── draw_shapes.hpp │ │ └── laneletsmap_generator.hpp │ ├── launch │ ├── project_lidar.launch │ ├── semantic.launch │ └── test_parameters.launch │ ├── package.xml │ ├── scripts │ └── test_yaw_correction.sh │ └── src │ ├── helper_functions.hpp │ ├── json.hpp │ └── laneletsmap_generator.cpp ├── generated_openx_files ├── gate_south-end_north-end_20210528_064001.0.xodr ├── gate_south-end_north-end_20210528_064001.0_cutin_1.xosc ├── gate_south-end_north-end_20210528_064001.0_cutin_2.xosc ├── gate_south-end_north-end_20210528_064001.0_cutin_3.xosc ├── gate_south-end_north-end_20210608_054523.0.xodr ├── gate_south-end_north-end_20210608_054523.0_cutin_1.xosc ├── gate_south-end_north-end_20210615_043348.0.xodr ├── gate_south-end_north-end_20210615_043348.0_cutin_1.xosc ├── gate_south-end_north-end_20210615_043348.0_cutin_2.xosc ├── gate_south-end_north-end_20210615_043348.0_cutin_3.xosc ├── gate_south-end_north-end_20210616_044127.0.xodr ├── gate_south-end_north-end_20210616_044127.0_cutin_1.xosc └── gate_south-end_north-end_20210616_044127.0_cutin_2.xosc ├── helper.bash ├── lz4_streamdecode_error_fix.bash ├── parameters ├── esmini-bin_ubuntu │ ├── EnvironmentSimulator │ │ ├── Applications │ │ │ └── odrplot │ │ │ │ └── xodr.py │ │ └── Libraries │ │ │ ├── esminiLib │ │ │ ├── ESMiniWrapper.cs │ │ │ └── esminiLib.hpp │ │ │ └── esminiRMLib │ │ │ ├── esminiRMLib.hpp │ │ │ ├── esminiRMUnityUtil.cs │ │ │ └── esminiRMWrapper.cs │ ├── bin │ │ ├── dat2csv │ │ ├── esmini │ │ ├── esmini-dyn │ │ ├── odrplot │ │ ├── odrviewer │ │ ├── osireceiver │ │ └── replayer │ ├── log.txt │ └── version.txt ├── esmini_data.py ├── generate_openx.py ├── plot.py ├── plots │ ├── comparison.psd │ ├── gate_south-end_north-end_20210608_054523.0 │ │ ├── test_ego_full.png │ │ ├── test_ego_half.png │ │ ├── test_full.png │ │ ├── test_half.png │ │ ├── test_s_t_full.png │ │ └── test_s_t_half.png │ ├── gate_south-end_north-end_20210615_043348.0 │ │ ├── test_2.png │ │ ├── test_all.png │ │ ├── test_ego_2.png │ │ ├── test_ego_all.png │ │ ├── test_ego_half.png │ │ ├── test_half.png │ │ ├── test_s_t_2.png │ │ ├── test_s_t_all.png │ │ └── test_s_t_half.png │ ├── rss_comparison.png │ ├── speed_comaprison.png │ ├── test.png │ ├── test_ego.png │ ├── test_other_milli.png │ ├── test_rss.png │ ├── test_s_t.png │ ├── trajectory_2_sec.png │ ├── trajectory_5_sec.png │ └── trajectory_all_sec.png ├── rss.py ├── scenario_data │ ├── gate_south-end_north-end_20210615_043348.0.cutin │ ├── gate_south-end_north-end_20210615_043348.0.cutout │ └── gate_south-end_north-end_20210615_043348.0.laneletToOD └── scenario_files │ ├── gate_south-end_north-end_20210608_054523.0.xodr │ ├── gate_south-end_north-end_20210608_054523.0_cutin_1.xosc │ ├── gate_south-end_north-end_20210608_054523.0_cutin_2.xosc │ ├── gate_south-end_north-end_20210608_054523.0_cutin_3.xosc │ ├── gate_south-end_north-end_20210615_043348.0.xodr │ ├── gate_south-end_north-end_20210615_043348.0_cutin_1.xosc │ ├── gate_south-end_north-end_20210615_043348.0_cutin_2.xosc │ └── gate_south-end_north-end_20210615_043348.0_cutin_3.xosc ├── plot2.py ├── process.py └── process_bags.bash /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | pip-wheel-metadata/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | target/ 78 | 79 | # Jupyter Notebook 80 | .ipynb_checkpoints 81 | 82 | # IPython 83 | profile_default/ 84 | ipython_config.py 85 | 86 | # pyenv 87 | .python-version 88 | 89 | # pipenv 90 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 91 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 92 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 93 | # install all needed dependencies. 94 | #Pipfile.lock 95 | 96 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 97 | __pypackages__/ 98 | 99 | # Celery stuff 100 | celerybeat-schedule 101 | celerybeat.pid 102 | 103 | # SageMath parsed files 104 | *.sage.py 105 | 106 | # Environments 107 | .env 108 | .venv 109 | env/ 110 | venv/ 111 | ENV/ 112 | env.bak/ 113 | venv.bak/ 114 | 115 | # Spyder project settings 116 | .spyderproject 117 | .spyproject 118 | 119 | # Rope project settings 120 | .ropeproject 121 | 122 | # mkdocs documentation 123 | /site 124 | 125 | # mypy 126 | .mypy_cache/ 127 | .dmypy.json 128 | dmypy.json 129 | 130 | # Pyre type checker 131 | .pyre/ 132 | 133 | catkin_ws/.catkin_tools/ 134 | catkin_ws/devel/ 135 | catkin_ws/logs/ 136 | -------------------------------------------------------------------------------- /Dockerfile.first: -------------------------------------------------------------------------------- 1 | FROM ubuntu:18.04 2 | 3 | # Cloning git repo 4 | RUN apt-get update 5 | 6 | # Setup git 7 | RUN apt-get install -y git 8 | RUN git config --global user.name "Dhanoop Karunakaran" 9 | RUN git config --global user.email "dkar9051@uni.sydney.edu.au" 10 | 11 | # IMPORTANT TO CHANGE DEPENDS ON THE CONFIG YOU HAVE: ADD /root/.ssh/id_rsa 12 | #ADD .ssh /root/.ssh 13 | #RUN touch /root/.ssh/known_hosts 14 | 15 | #ADD .vimrc /root/.vimrc 16 | 17 | # IMPORTANT TO CHANGE DEPENDS ON THE GIT REPO YOU HAVE: RUN ssh-keyscan >> /root/.ssh/known_hosts 18 | #RUN ssh-keyscan gitlab.acfr.usyd.edu.au >> /root/.ssh/known_hosts 19 | RUN apt-get update 20 | 21 | # Installing ROS-melodic 22 | RUN apt-get install -y gnupg2 23 | RUN apt-get install -y curl 24 | RUN apt-get install -y lsb-core 25 | ARG DEBIAN_FRONTEND=noninteractive 26 | 27 | # INSTALL OTHER NECESSARY PACKAGES 28 | RUN apt-get install -y vim 29 | RUN apt-get install -y wget 30 | RUN apt-get update 31 | RUN apt-get install -y python-pip 32 | RUN apt-get install -y libpng16-16 33 | RUN apt-get install -y libjpeg-turbo8 34 | RUN apt-get install -y libtiff5 35 | RUN apt-get update 36 | #RUN pip install tensorflow==2.0.0b0 37 | RUN pip install python-pcl 38 | RUN pip install pyproj 39 | RUN pip install scipy 40 | RUN pip install sklearn 41 | 42 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 43 | RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 44 | RUN curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | apt-key add - 45 | RUN apt update 46 | RUN apt install -y ros-melodic-desktop 47 | RUN apt-get install -y python-rosdep 48 | RUN rosdep init 49 | RUN rosdep update 50 | RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc 51 | #RUN source ~/.bashrc 52 | RUN apt install -y python-rosinstall python-rosinstall-generator python-wstool build-essential 53 | 54 | # Intalling pyton-catkin 55 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list' 56 | RUN wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 57 | RUN apt-get update 58 | RUN apt-get install -y python-catkin-tools 59 | RUN apt-get install -y software-properties-common 60 | 61 | # Other important packages 62 | RUN apt-get install -y ros-melodic-tf2-sensor-msgs 63 | RUN apt-get install -y ros-melodic-tf2-geometry-msgs 64 | RUN apt-get install -y ros-melodic-lanelet2 65 | RUN pip install networkx 66 | 67 | RUN apt-get update 68 | RUN apt-get install -y ros-melodic-jsk-recognition-msgs 69 | RUN apt-get install -y ros-melodic-jsk-rviz-plugins 70 | RUN apt-get install -y ros-melodic-grid-map 71 | RUN apt-get install -y ros-melodic-audio-common 72 | RUN apt-get install -y libglew-dev 73 | RUN apt-get install -y ros-melodic-geodesy 74 | RUN apt-get install -y freeglut3-dev 75 | 76 | RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bash_profile 77 | 78 | CMD ["tail", "-f", "/dev/null"] 79 | -------------------------------------------------------------------------------- /Dockerfile.second: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | 3 | # Cloning git repo 4 | RUN apt-get update 5 | 6 | # Setup git 7 | RUN apt-get install -y git 8 | RUN git config --global user.name "Dhanoop Karunakaran" 9 | RUN git config --global user.email "dkar9051@uni.sydney.edu.au" 10 | 11 | RUN apt-get update 12 | 13 | # Installing ROS-melodic 14 | RUN apt-get install -y gnupg2 15 | RUN apt-get install -y curl 16 | #RUN apt-get install -y lsb-core 17 | ARG DEBIAN_FRONTEND=noninteractive 18 | 19 | RUN apt-get update 20 | RUN apt-get install -y python3-pip 21 | RUN pip install scenariogeneration 22 | 23 | 24 | # INSTALL OTHER NECESSARY PACKAGES 25 | RUN apt-get install -y vim 26 | RUN apt-get install -y wget 27 | RUN apt-get update 28 | RUN apt-get install -y libpng16-16 29 | #RUN apt-get install -y libjpeg-turbo8 30 | RUN apt-get install -y libtiff5 31 | RUN apt-get update 32 | RUN pip install pyproj 33 | RUN pip install scipy 34 | RUN pip install sklearn 35 | 36 | 37 | RUN pip install networkx 38 | #RUN ssh-keyscan github.com >> /root/.ssh/known_hosts 39 | RUN apt-get update 40 | RUN apt-get install -y libglew-dev 41 | RUN apt-get install -y freeglut3-dev 42 | RUN apt-get install -y libxrandr2 43 | RUN apt-get install -y libxinerama1 44 | 45 | RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bash_profile 46 | 47 | CMD ["tail", "-f", "/dev/null"] 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Scenario Extraction Framework 2 | This is a code base for the scenario extraction framework paper. There are two stages in code base: scenario extraction from real-world data and OpenX files generation. We have two docker containers for first and second stage. In the first docker container, ROS Melodic and all other dependencies to run the first stage is added. ROS melodic runs on python2, but second stage require python3. Once we have the scenario extraction result finished in the first stage, we use the second docker container for generating the OpenX files. 3 | 4 | There are 3 ros nodes in the first stage: lanepoint filetring & lane construction, scenario detection, and scenario parameter extraction. Lanepoint filetring & lane construction node is responsible for filtering the lane points, then build lanes and convert them to lanelet representation. It is called laneletsmap_generator. Scenario detection node identifies the scenarios such as cut-in and cut-out, then mark it as scenarios. Also it stores all the position data of ego vehicle and other vehicles in frenet frame. The name of the node is extraction. The last node, feature_model is reposible for extracting the required parameters from the marked scenarios. 5 | 6 | ## Steps to setup the first docker container for extracting the scenario from the rosbag file 7 | * Build the first docker image 8 | ``` 9 | bash helper.bash build_first_image 10 | ``` 11 | Example: 12 | ``` 13 | bash helper.bash build_first_image /home/something/Documents/scenario_extraction_framework 14 | ``` 15 | 16 | * Create the first container 17 | ``` 18 | bash helper.bash create_first_container 19 | ``` 20 | 21 | * Run the container 22 | ``` 23 | bash helper.bash start_first_container 24 | ``` 25 | 26 | * Once you are in container, naviagate to /validation/ and run the following code 27 | ``` 28 | bash lz4_streamdecode_error_fix.bash 29 | ``` 30 | 31 | * Then change dir to catkin_ws, and run following code to build the ROS packages 32 | ``` 33 | catkin build 34 | ``` 35 | If we have all 12 packages got installed and then good to go for next step to extract the scenarios. 36 | 37 | 38 | ## Run roslaunch file to extract scenarios 39 | * Navigate to /validation/catkin_ws 40 | ``` 41 | cd /validation/catkin_ws 42 | ``` 43 | 44 | * Run roslaunch to extract scenario paramters from rosbag file 45 | 46 | ``` 47 | roslaunch launch/validation_dataset.launch bag_file:= output_image:=/validation/top_view_image_of_lanes.png laneletsmap_file:=/validation/map.osm 48 | ``` 49 | 50 | * If everythin goes smooth, files with extension 'cutin' and 'cutout' have been created in the /validation/parameters/scenario_data/ 51 | 52 | 53 | ## Steps to setup the second docker container for generating OpenX files 54 | * Build the first docker image 55 | ``` 56 | bash helper.bash build_second_image 57 | ``` 58 | Example: 59 | ``` 60 | bash helper.bash build_second_image /home/something/Documents/scenario_extraction_framework 61 | ``` 62 | 63 | * Create the first container 64 | ``` 65 | bash helper.bash create_second_container 66 | ``` 67 | 68 | * Run the container 69 | ``` 70 | bash helper.bash start_second_container 71 | ``` 72 | 73 | 74 | 75 | ## Generate OpenX files 76 | Once we are in the container, follow below steps 77 | 78 | * Navigate the the parameters folder 79 | ``` 80 | cd /validation/parameters/ 81 | ``` 82 | 83 | * Generate OpenX files such as OpenSCENARIO and OpendDRIVE files 84 | 85 | ``` 86 | python3 generate_openx.py 87 | ``` 88 | 89 | * This step will execute OpenX files in Esmini OpenSCENARIO player and generate date for plotting. At the moment we are running esmini headless as we haven't added GUI to docker. 90 | 91 | ``` 92 | python3 esmini_data.py 93 | ``` 94 | 95 | * Create some plots for visulaisation and you can find them in /validation/parameters/plots folder 96 | 97 | ``` 98 | python3 plot.py 99 | ``` 100 | 101 | ## Sytem requirement 102 | 103 | Tested it on Ubuntu 18.04 and 20.04 as the host pc. 104 | 105 | ## Sample data 106 | Sample data in rosbag format can be found here: https://drive.google.com/drive/folders/1gsqGG5PrW-1_k2a9Ulu-Tquj_oUIe5wQ?usp=sharing 107 | 108 | 109 | -------------------------------------------------------------------------------- /catkin_ws/launch/feature_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/launch/laneletsmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | ["ibeo/lidar/dynamic,point,150,3", 6 | "ibeo/lidar/static,point,150,3", 7 | "/velodyne/front/pole_extractor/observations,1,10", 8 | "/velodyne/front/points,circle,10,6", 9 | "/velodyne/front/points,point,255,1", 10 | "/velodyne/front/pole_extractor/points/all,circle,200,16", 11 | "/velodyne/front/corner_extractor/points/all,circle,200,16", 12 | "/pointcloud,point,255,1"] 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | [90, 91, 92, 93, 94] 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /catkin_ws/launch/scenario_extraction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /catkin_ws/launch/validation_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | ["ibeo/lidar/dynamic,point,150,3", 6 | "ibeo/lidar/static,point,150,3", 7 | "/velodyne/front/pole_extractor/observations,1,10", 8 | "/velodyne/front/points,circle,10,6", 9 | "/velodyne/front/points,point,255,1", 10 | "/velodyne/front/pole_extractor/points/all,circle,200,16", 11 | "/velodyne/front/corner_extractor/points/all,circle,200,16", 12 | "/pointcloud,point,255,1"] 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | [90, 91, 92, 93, 94] 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(custom_point_types) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 6 | 7 | 8 | #find catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | pcl_ros 13 | ) 14 | 15 | link_directories(${PCL_LIBRARY_DIRS}) 16 | add_definitions(${PCL_DEFINITIONS}) 17 | 18 | catkin_package( 19 | CATKIN_DEPENDS pcl_ros 20 | INCLUDE_DIRS include 21 | ) 22 | 23 | include_directories( 24 | include/${PROJECT_NAME} 25 | ) 26 | 27 | 28 | install(DIRECTORY include/${PROJECT_NAME}/ 29 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 30 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/include/custom_point_types/point_xyzir.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTXYZIR_CLASS 2 | #define POINTXYZIR_CLASS 3 | 4 | #define PCL_NO_PRECOMPILE 5 | 6 | #include 7 | 8 | // Algorithms we want this type to work with 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | namespace pcl { 15 | 16 | struct PointXYZIR { 17 | PCL_ADD_POINT4D // Macro quad-word XYZ 18 | float intensity; // Laser intensity 19 | uint16_t ring; // Laser ring number 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Ensure proper alignment 21 | } EIGEN_ALIGN16; 22 | 23 | } // end namespace pcl 24 | 25 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR, 26 | (float, x, x) 27 | (float, y, y) 28 | (float, z, z) 29 | (float, intensity, intensity) 30 | (uint16_t, ring, ring)) 31 | 32 | typedef pcl::PointCloud PointCloudXYZIR; 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/include/custom_point_types/point_xyzirc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define PCL_NO_PRECOMPILE 4 | 5 | #include 6 | 7 | // Algorithms we want this type to work with 8 | #include 9 | #include 10 | #include 11 | 12 | namespace pcl 13 | { 14 | struct PointXYZIRC 15 | { 16 | PCL_ADD_POINT4D // Macro quad-word XYZ 17 | float intensity; // Laser intensity 18 | uint16_t ring; // Laser ring number 19 | uint16_t cluster; // Cluster number 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Ensure proper alignment 21 | } EIGEN_ALIGN16; 22 | 23 | } // end namespace pcl 24 | 25 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRC, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( 26 | uint16_t, ring, ring)(uint16_t, cluster, cluster)) 27 | 28 | typedef pcl::PointCloud PointCloudXYZIRC; 29 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/include/custom_point_types/point_xyzirl.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTXYZIRL_CLASS 2 | #define POINTXYZIRL_CLASS 3 | 4 | #define PCL_NO_PRECOMPILE 5 | 6 | #include 7 | 8 | // Algorithms we want this type to work with 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | namespace pcl { 15 | 16 | struct PointXYZIRL { 17 | PCL_ADD_POINT4D // Macro quad-word XYZ 18 | float intensity; // Laser intensity 19 | uint16_t ring; // Laser ring number 20 | uint16_t label; // Label number 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Ensure proper alignment 22 | } EIGEN_ALIGN16; 23 | 24 | } // end namespace pcl 25 | 26 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRL, 27 | (float, x, x) 28 | (float, y, y) 29 | (float, z, z) 30 | (float, intensity, intensity) 31 | (uint16_t, ring, ring) 32 | (uint16_t, label, label)) 33 | 34 | typedef pcl::PointCloud PointCloudXYZIRL; 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/include/custom_point_types/point_xyzirrgb.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTXYZIRRGB_CLASS 2 | #define POINTXYZIRRGB_CLASS 3 | 4 | #define PCL_NO_PRECOMPILE 5 | 6 | #include 7 | 8 | // Algorithms we want this type to work with 9 | #include 10 | 11 | namespace pcl { 12 | 13 | struct PointXYZIRRGB { 14 | PCL_ADD_POINT4D; 15 | float intensity; 16 | std::uint16_t ring; 17 | float rgb; 18 | 19 | 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | } EIGEN_ALIGN16; 22 | 23 | } // end namespace pcl 24 | 25 | 26 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRRGB, 27 | (float, x, x) 28 | (float, y, y) 29 | (float, z, z) 30 | (float, intensity, intensity) 31 | (std::uint16_t, ring, ring) 32 | (float, rgb, rgb)) 33 | 34 | typedef pcl::PointCloud PointCloudXYZIRRGB; 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/include/custom_point_types/point_xyziruv.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTXYZIRUV_CLASS 2 | #define POINTXYZIRUV_CLASS 3 | 4 | #define PCL_NO_PRECOMPILE 5 | 6 | #include 7 | 8 | // Algorithms we want this type to work with 9 | #include 10 | 11 | namespace pcl 12 | { 13 | 14 | struct PointXYZIRUV { 15 | PCL_ADD_POINT4D; 16 | float intensity; 17 | std::uint16_t ring; 18 | float U; 19 | float V; 20 | 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | } EIGEN_ALIGN16; 23 | 24 | } // end namespace pcl 25 | 26 | 27 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRUV, 28 | (float, x, x) 29 | (float, y, y) 30 | (float, z, z) 31 | (float, intensity, intensity) 32 | (std::uint16_t, ring, ring) 33 | (float, U, U) 34 | (float, V, V)) 35 | 36 | 37 | typedef pcl::PointCloud PointCloudXYZIRUV; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/include/custom_point_types/point_xyziruvcov.h: -------------------------------------------------------------------------------- 1 | #ifndef POINTXYZIRUVCOV_CLASS 2 | #define POINTXYZIRUVCOV_CLASS 3 | 4 | #define PCL_NO_PRECOMPILE 5 | 6 | #include 7 | 8 | namespace pcl 9 | { 10 | 11 | struct PointXYZIRUVCOV { 12 | PCL_ADD_POINT4D; 13 | float intensity; 14 | std::uint16_t ring; 15 | float U; 16 | float V; 17 | float cov_xx; 18 | float cov_yy; 19 | float cov_zz; 20 | float cov_xy; 21 | float cov_xz; 22 | float cov_yz; 23 | float cov_UU; 24 | float cov_VV; 25 | float cov_UV; 26 | 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | } EIGEN_ALIGN16; 29 | 30 | } // end namespace pcl 31 | 32 | 33 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRUVCOV, 34 | (float, x, x) 35 | (float, y, y) 36 | (float, z, z) 37 | (float, intensity, intensity) 38 | (std::uint16_t, ring, ring) 39 | (float, U, U) 40 | (float, V, V) 41 | (float, cov_xx, cov_xx) 42 | (float, cov_yy, cov_yy) 43 | (float, cov_zz, cov_zz) 44 | (float, cov_xy, cov_xy) 45 | (float, cov_xz, cov_xz) 46 | (float, cov_yz, cov_yz) 47 | (float, cov_UU, cov_UU) 48 | (float, cov_VV, cov_VV) 49 | (float, cov_UV, cov_UV)) 50 | 51 | 52 | typedef pcl::PointCloud PointCloudXYZIRUVCOV; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/custom_point_types/package.xml: -------------------------------------------------------------------------------- 1 | 2 | custom_point_types 3 | 0.0.1 4 | Some custom point types used throughout the ACFR datasets 5 | Stewart Worrall 6 | 7 | BSD 8 | 9 | http://sydney.edu.au/acfr/its 10 | 11 | Stewart Worrall 12 | 13 | catkin 14 | 15 | roscpp 16 | pcl_ros 17 | 18 | 19 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dataset_msgs) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 6 | 7 | 8 | #find catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | std_msgs 11 | nav_msgs 12 | sensor_msgs 13 | tf2_sensor_msgs 14 | tf2_geometry_msgs 15 | message_generation 16 | ) 17 | 18 | 19 | ## Generate messages in the 'msg' folder 20 | add_message_files( 21 | FILES 22 | DatasetEvent.msg 23 | Observation.msg 24 | Innovation.msg 25 | LocaliserStats.msg 26 | ) 27 | 28 | ## Generate added messages and services with any dependencies listed here 29 | generate_messages( 30 | DEPENDENCIES 31 | std_msgs # Or other packages containing msgs 32 | ) 33 | 34 | 35 | catkin_package( 36 | INCLUDE_DIRS include 37 | CATKIN_DEPENDS nav_msgs sensor_msgs std_msgs sensor_msgs message_runtime 38 | ) 39 | 40 | 41 | include_directories( 42 | include/${PROJECT_NAME} 43 | ${catkin_INCLUDE_DIRS} 44 | ) 45 | 46 | 47 | # Mark other files for installation (e.g. launch and bag files, etc.) 48 | install(DIRECTORY include/${PROJECT_NAME} 49 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 50 | ) 51 | 52 | 53 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/include/dataset_msgs/blank.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DATASET_MSG_BLANK_HPP 2 | #ifndef DATASET_MSG_BLANK_HPP 3 | 4 | 5 | #ifndef DATASET_MSG_BLANK_HPP 6 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/msg/DatasetEvent.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | string event_description 4 | 5 | time start_stamp 6 | time end_stamp 7 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/msg/Innovation.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 yaw -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/msg/LocaliserStats.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | string source 4 | 5 | dataset_msgs/Observation observation 6 | dataset_msgs/Innovation innovation 7 | float32 confidence 8 | 9 | # Row-major representation of the 3x3 covariance matrix 10 | # The orientation parameters use a fixed-axis representation. 11 | # In order, the parameters are: 12 | # (x, y, yaw) 13 | float64[9] covariance 14 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/msg/Observation.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float32 yaw 4 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dataset_msgs 3 | 0.0.1 4 | Messages used by the dataset tools ecosystem 5 | Stewart Worrall 6 | 7 | BSD 8 | 9 | http://sydney.edu.au/acfr/its 10 | 11 | Stewart Worrall 12 | 13 | catkin 14 | message_generation 15 | 16 | message_runtime 17 | 18 | std_msgs 19 | nav_msgs 20 | sensor_msgs 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dataset_playback) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 5 | 6 | set(DISTRO $ENV{ROS_DISTRO}) 7 | message(STATUS "Distribution: " ${DISTRO}) 8 | 9 | #set(DEFAULT_BUILD_QT5 OFF) 10 | #if(${DISTRO} STREQUAL "kinetic") 11 | # set(DEFAULT_BUILD_QT5 ON) 12 | #endif() 13 | set(DEFAULT_BUILD_QT5 ON) 14 | 15 | #option(UseQt5 "Build w/ QT version 5" ${DEFAULT_BUILD_QT5}) 16 | set(UseQt5 True) 17 | 18 | if (UseQt5) 19 | #find_package(Qt5 COMPONENTS Core Gui Network Concurrent REQUIRED) 20 | find_package(Qt5Widgets) 21 | find_package(Qt5Core) 22 | find_package(Qt5Gui) 23 | else() 24 | find_package(Qt4 COMPONENTS QtCore QtGui QtNetwork REQUIRED) 25 | include(${QT_USE_FILE}) 26 | endif() 27 | 28 | # package currently needs opencv 29 | find_package(OpenCV 3 REQUIRED) 30 | 31 | find_package(catkin REQUIRED COMPONENTS 32 | roscpp 33 | h264_bag_playback 34 | dataset_tools 35 | dataset_msgs 36 | ) 37 | 38 | catkin_package(CATKIN_DEPENDS dataset_tools) 39 | 40 | 41 | set(${PROJECT_NAME}_SOURCES 42 | src/dataset_panel.cpp 43 | src/main.cpp 44 | src/treeitem.cpp 45 | src/treemodel.cpp 46 | ) 47 | 48 | set(${PROJECT_NAME}_HEADERS 49 | include/${PROJECT_NAME}/dataset_panel.h 50 | include/${PROJECT_NAME}/treeitem.h 51 | include/${PROJECT_NAME}/treemodel.h 52 | ) 53 | 54 | # invoke MOC and UI/ include Qt headers/ link Qt libraries 55 | if (UseQt5) 56 | qt5_wrap_cpp(${PROJECT_NAME}_MOCSrcs ${${PROJECT_NAME}_HEADERS}) 57 | include_directories( 58 | ${Qt5Widgets_INCLUDE_DIRS} 59 | ${Qt5Core_INCLUDE_DIRS} 60 | ${Qt5Gui_INCLUDE_DIRS} 61 | ${Qt5Xml_INCLUDE_DIRS} 62 | ${Qt5Network_INCLUDE_DIRS} 63 | ${Qt5Concurrent_INCLUDE_DIRS} 64 | ) 65 | link_libraries( 66 | ${Qt5Core_LIBRARIES} 67 | ${Qt5Widgets_LIBRARIES} 68 | ${Qt5Gui_LIBRARIES} 69 | ${Qt5Xml_LIBRARIES} 70 | ${Qt5Svg_LIBRARIES} 71 | ${Qt5Network_LIBRARIES} 72 | ${Qt5Concurrent_LIBRARIES} 73 | ) 74 | else() 75 | qt4_wrap_cpp(${PROJECT_NAME}_MOCSrcs ${${PROJECT_NAME}_HEADERS}) 76 | include_directories(${Qt4_INCLUDE_DIR}) 77 | link_libraries( 78 | ${QT_LIBRARIES} 79 | ) 80 | endif() 81 | 82 | # Other includes 83 | include_directories( 84 | ${CMAKE_CURRENT_BINARY_DIR} 85 | ${catkin_INCLUDE_DIRS} 86 | ${QT_LIBRARIES} 87 | include/${PROJECT_NAME} 88 | ) 89 | 90 | #LINK_DIRECTORIES( ${LINK_DIRECTORIES} ) 91 | 92 | # Other libraries 93 | link_libraries( 94 | ${QT_LIBRARIES} 95 | ${catkin_LIBRARIES} 96 | ${OpenCV_LIBRARIES} 97 | ) 98 | 99 | add_definitions("-Wall -Wunused -std=c++11") 100 | 101 | set(PROJECT_SOURCE_FILES 102 | ${${PROJECT_NAME}_SOURCES} 103 | ${${PROJECT_NAME}_MOCSrcs} 104 | ) 105 | 106 | add_executable(${PROJECT_NAME} 107 | ${PROJECT_SOURCE_FILES} 108 | ) 109 | 110 | install(TARGETS ${PROJECT_NAME} 111 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 112 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 113 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 114 | ) 115 | 116 | 117 | install(DIRECTORY launch 118 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 119 | ) 120 | 121 | install(DIRECTORY destination_files 122 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 123 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/cmake/Findgraphviz.cmake: -------------------------------------------------------------------------------- 1 | 2 | if ( NOT WIN32 ) 3 | 4 | find_package(PkgConfig) 5 | pkg_check_modules( graphviz ${REQUIRED} libgvc libagraph libcdt libgraph libpathplan ) 6 | if ( graphviz_FOUND ) 7 | set ( graphviz_INCLUDE_DIRECTORIES ${graphviz_INCLUDE_DIRS} ) 8 | endif ( graphviz_FOUND ) 9 | 10 | endif ( NOT WIN32 ) 11 | 12 | find_path( graphviz_INCLUDE_DIRECTORIES 13 | NAMES gvc.h 14 | PATHS 15 | ${graphviz_INCLUDE_DIRS} 16 | /usr/local/include 17 | /usr/include 18 | /usr/include/graphviz 19 | ) 20 | 21 | find_library( graphviz_AGRAPH_LIBRARY 22 | NAMES agraph 23 | PATHS 24 | ${graphviz_LIBRARY_DIRS} 25 | /usr/local/lib64 26 | /usr/lib64 27 | /usr/local/lib 28 | /usr/lib 29 | ) 30 | find_library( graphviz_GVC_LIBRARY 31 | NAMES gvc 32 | PATHS 33 | ${graphviz_LIBRARY_DIRS} 34 | /usr/local/lib64 35 | /usr/lib64 36 | /usr/local/lib 37 | /usr/lib 38 | ) 39 | find_library( graphviz_CDT_LIBRARY 40 | NAMES cdt 41 | PATHS 42 | ${graphviz_LIBRARY_DIRS} 43 | /usr/local/lib64 44 | /usr/lib64 45 | /usr/local/lib 46 | /usr/lib 47 | ) 48 | find_library( graphviz_GRAPH_LIBRARY 49 | NAMES graph 50 | PATHS 51 | ${graphviz_LIBRARY_DIRS} 52 | /usr/local/lib64 53 | /usr/lib64 54 | /usr/local/lib 55 | /usr/lib 56 | ) 57 | find_library( graphviz_PATHPLAN_LIBRARY 58 | NAMES pathplan 59 | PATHS 60 | ${graphviz_LIBRARY_DIRS} 61 | /usr/local/lib64 62 | /usr/lib64 63 | /usr/local/lib 64 | /usr/lib 65 | ) 66 | if ( graphviz_INCLUDE_DIRECTORIES AND graphviz_AGRAPH_LIBRARY AND 67 | graphviz_GVC_LIBRARY AND graphviz_CDT_LIBRARY AND 68 | graphviz_GRAPH_LIBRARY AND graphviz_PATHPLAN_LIBRARY ) 69 | set ( graphviz_FOUND 1 ) 70 | set ( graphviz_LIBRARIES 71 | "${graphviz_GVC_LIBRARY};${graphviz_AGRAPH_LIBRARY};${graphviz_GRAPH_LIBRARY};" 72 | "${graphviz_CDT_LIBRARY};${graphviz_PATHPLAN_LIBRARY}" 73 | CACHE FILEPATH "Libraries for graphviz" ) 74 | else ( graphviz_INCLUDE_DIRECTORIES AND graphviz_AGRAPH_LIBRARY AND 75 | graphviz_GVC_LIBRARY AND graphviz_CDT_LIBRARY AND 76 | graphviz_GRAPH_LIBRARY AND graphviz_PATHPLAN_LIBRARY ) 77 | set ( graphviz_FOUND 0 ) 78 | endif ( graphviz_INCLUDE_DIRECTORIES AND graphviz_AGRAPH_LIBRARY AND 79 | graphviz_GVC_LIBRARY AND graphviz_CDT_LIBRARY AND 80 | graphviz_GRAPH_LIBRARY AND graphviz_PATHPLAN_LIBRARY ) 81 | 82 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/destination_files/crashlab.yaml: -------------------------------------------------------------------------------- 1 | pose_set: 2 | marquee: 3 | position: 4 | x: -26577.5546875 5 | y: 7968.74853516 6 | z: 0.0 7 | orientation: 8 | x: 0.0 9 | y: 0.0 10 | z: 0.89061803758 11 | w: -0.454752142532 12 | far_side_straight: 13 | position: 14 | x: -26602.84375 15 | y: 7936.296875 16 | z: 0.0 17 | orientation: 18 | x: 0.0 19 | y: 0.0 20 | z: 0.890306876935 21 | w: -0.455361026969 22 | big_loop_start: 23 | position: 24 | x: -26616.3867188 25 | y: 7916.15087891 26 | z: 0.0 27 | orientation: 28 | x: 0.0 29 | y: 0.0 30 | z: 0.908528871457 31 | w: -0.417822079035 32 | big_loop_end: 33 | position: 34 | x: -26624.9648438 35 | y: 7938.70947266 36 | z: 0.0 37 | orientation: 38 | x: 0.0 39 | y: 0.0 40 | z: -0.32902042207 41 | w: 0.944322805962 42 | far_side_return_straight: 43 | position: 44 | x: -26607.2285156 45 | y: 7940.18310547 46 | z: 0.0 47 | orientation: 48 | x: 0.0 49 | y: 0.0 50 | z: 0.434960022109 51 | w: 0.900449764933 52 | opposite_marquee: 53 | position: 54 | x: -26584.2128906 55 | y: 7968.94482422 56 | z: 0.0 57 | orientation: 58 | x: 0.0 59 | y: 0.0 60 | z: 0.47299731392 61 | w: 0.881063868868 62 | little_loop: 63 | position: 64 | x: -26576.4765625 65 | y: 7988.52197266 66 | z: 0.0 67 | orientation: 68 | x: 0.0 69 | y: 0.0 70 | z: 0.438008259381 71 | w: 0.898970947647 72 | relationships: 73 | - from: marquee 74 | to: far_side_straight 75 | - from: far_side_straight 76 | to: big_loop_start 77 | - from: big_loop_start 78 | to: big_loop_end 79 | - from: big_loop_end 80 | to: far_side_return_straight 81 | - from: far_side_return_straight 82 | to: opposite_marquee 83 | - from: opposite_marquee 84 | to: little_loop 85 | - from: little_loop 86 | to: marquee 87 | - from: big_loop_end 88 | to: big_loop_start 89 | 90 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/destination_files/main_quad.yaml: -------------------------------------------------------------------------------- 1 | pose_set: 2 | grass_area_1: 3 | position: 4 | x: 1287.037 5 | y: -1114.272 6 | z: 0.0 7 | orientation: 8 | x: 0.0 9 | y: 0.0 10 | z: 0.753 11 | w: 0.658 12 | grass_area_2: 13 | position: 14 | x: 1283.105 15 | y: -1087.832 16 | z: 0.0 17 | orientation: 18 | x: 0.0 19 | y: 0.0 20 | z: 0.774 21 | w: 0.633 22 | grass_area_3: 23 | position: 24 | x: 1279.690 25 | y: -1063.749 26 | z: 0.0 27 | orientation: 28 | x: 0.0 29 | y: 0.0 30 | z: 0.765 31 | w: 0.644 32 | great_hall_steps: 33 | position: 34 | x: 1279.488 35 | y: -1022.286 36 | z: 0.0 37 | orientation: 38 | x: 0.0 39 | y: 0.0 40 | z: 0.680 41 | w: 0.733 42 | opp_grass_area_3: 43 | position: 44 | x: 1285.544 45 | y: -1057.088 46 | z: 0.0 47 | orientation: 48 | x: 0.0 49 | y: 0.0 50 | z: -0.648 51 | w: 0.761 52 | opp_grass_area_2: 53 | position: 54 | x: 1289.282 55 | y: -1080.578 56 | z: 0.0 57 | orientation: 58 | x: 0.0 59 | y: 0.0 60 | z: -0.651 61 | w: 0.759 62 | relationships: 63 | - from: grass_area_1 64 | to: grass_area_2 65 | - from: grass_area_2 66 | to: grass_area_3 67 | - from: grass_area_3 68 | to: great_hall_steps 69 | - from: great_hall_steps 70 | to: opp_grass_area_3 71 | - from: opp_grass_area_3 72 | to: opp_grass_area_2 73 | - from: opp_grass_area_2 74 | to: grass_area_1 75 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/include/dataset_playback/dataset_panel.h: -------------------------------------------------------------------------------- 1 | #ifndef BUS_STOP_PANEL_H 2 | #define BUS_STOP_PANEL_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include "treemodel.h" 25 | 26 | class DatasetThread : public QThread 27 | { 28 | 29 | 30 | Q_OBJECT 31 | 32 | void run() { 33 | QString result; 34 | /* expensive or blocking operation */ 35 | ros::NodeHandle("~").setParam("bag_file", file_name); 36 | 37 | bag_playback.init_playback(); 38 | bag_playback.limit_playback_speed = false; 39 | 40 | bag_playback.OpenBags(); 41 | 42 | current_state = PLAYBACK_PAUSE; 43 | 44 | do 45 | { 46 | while(current_state == PLAYBACK_PAUSE) { 47 | // Spin once so that any other ros controls/pub/sub can be actioned 48 | ros::spinOnce(); 49 | } 50 | 51 | if (!ros::ok() || current_state == PLAYBACK_STOP) { 52 | break; 53 | } 54 | 55 | if (current_state == PLAYBACK_SEEK) { 56 | /*if (seek_time < bag_playback.last_packet_time) { 57 | // need to reload the data as the iterator only goes one way 58 | bag_playback.init_playback(); 59 | 60 | for (auto video: bag_playback.videos) { 61 | video.second.frame_counter = 0; 62 | } 63 | 64 | bag_playback.OpenBags(); 65 | } 66 | */ 67 | bag_playback.SeekTime(seek_time); 68 | current_state = PLAYBACK_START; 69 | } 70 | 71 | if (!bag_playback.ReadNextPacket()) { 72 | current_state = PLAYBACK_PAUSE; 73 | } 74 | 75 | } while (true); 76 | 77 | bag_playback.CloseBags(); 78 | 79 | emit resultReady(result); 80 | } 81 | signals: 82 | void resultReady(const QString &s); 83 | 84 | public: 85 | std::string file_name; 86 | 87 | ros::Time seek_time; 88 | 89 | typedef enum PLAYBACK_CONTROL { 90 | PLAYBACK_LOADING, 91 | PLAYBACK_STOP, 92 | PLAYBACK_START, 93 | PLAYBACK_PAUSE, 94 | PLAYBACK_SEEK 95 | } PLAYBACK_CONTROL; 96 | 97 | PLAYBACK_CONTROL current_state; 98 | 99 | dataset_toolkit::h264_bag_playback bag_playback; 100 | 101 | DatasetThread() {current_state = PLAYBACK_LOADING;} 102 | 103 | }; 104 | 105 | 106 | class DatasetPanel: public QFrame 107 | { 108 | Q_OBJECT 109 | public: 110 | DatasetPanel( QWidget* parent = 0 ); 111 | 112 | DatasetThread *workerThread; 113 | 114 | protected Q_SLOTS: 115 | 116 | // slot for when the start button is pressed 117 | void startPressed(); 118 | void stopPressed(); 119 | void pausePressed(); 120 | void convertPressed(); 121 | 122 | void sliderPressed(); 123 | void sliderMoved(int new_value); 124 | void sliderReleased(); 125 | 126 | void changePlaybackRealtime(); 127 | 128 | void OnClickedTree(QModelIndex clicked_item); 129 | 130 | void recordEvent(); 131 | void saveEventsToFile(); 132 | 133 | void PollROS(); 134 | 135 | void selectBagFile(); 136 | 137 | 138 | protected: 139 | 140 | 141 | void addEventToCategory(std::string category, dataset_msgs::DatasetEvent::ConstPtr event); 142 | 143 | QPushButton* start_button_; 144 | QPushButton* pause_button_; 145 | QPushButton* stop_button_; 146 | QPushButton* convert_button_; 147 | QPushButton* file_select_button_; 148 | 149 | QCheckBox* playback_realtime_; 150 | 151 | QTreeView* tree_view_; 152 | TreeModel* tree_model; 153 | 154 | 155 | QLineEdit *eventText; 156 | QLineEdit *currentEventTimeText; 157 | QPushButton* recordEventButton; 158 | QPushButton* saveEventsButton; 159 | 160 | 161 | QTextEdit *statusText; 162 | QLineEdit *currentTimeText; 163 | 164 | QSlider *slider; 165 | 166 | // The ROS node handle. 167 | ros::NodeHandle nh_; 168 | 169 | }; 170 | 171 | 172 | #endif // BUS_STOP_PANEL_H 173 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/include/dataset_playback/treeitem.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** 3 | ** Copyright (C) 2016 The Qt Company Ltd. 4 | ** Contact: https://www.qt.io/licensing/ 5 | ** 6 | ** This file is part of the examples of the Qt Toolkit. 7 | ** 8 | ** $QT_BEGIN_LICENSE:BSD$ 9 | ** Commercial License Usage 10 | ** Licensees holding valid commercial Qt licenses may use this file in 11 | ** accordance with the commercial license agreement provided with the 12 | ** Software or, alternatively, in accordance with the terms contained in 13 | ** a written agreement between you and The Qt Company. For licensing terms 14 | ** and conditions see https://www.qt.io/terms-conditions. For further 15 | ** information use the contact form at https://www.qt.io/contact-us. 16 | ** 17 | ** BSD License Usage 18 | ** Alternatively, you may use this file under the terms of the BSD license 19 | ** as follows: 20 | ** 21 | ** "Redistribution and use in source and binary forms, with or without 22 | ** modification, are permitted provided that the following conditions are 23 | ** met: 24 | ** * Redistributions of source code must retain the above copyright 25 | ** notice, this list of conditions and the following disclaimer. 26 | ** * Redistributions in binary form must reproduce the above copyright 27 | ** notice, this list of conditions and the following disclaimer in 28 | ** the documentation and/or other materials provided with the 29 | ** distribution. 30 | ** * Neither the name of The Qt Company Ltd nor the names of its 31 | ** contributors may be used to endorse or promote products derived 32 | ** from this software without specific prior written permission. 33 | ** 34 | ** 35 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 36 | ** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 37 | ** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 38 | ** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 39 | ** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 40 | ** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 41 | ** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 42 | ** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 43 | ** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 44 | ** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 45 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE." 46 | ** 47 | ** $QT_END_LICENSE$ 48 | ** 49 | ****************************************************************************/ 50 | 51 | #ifndef TREEITEM_H 52 | #define TREEITEM_H 53 | 54 | #include 55 | #include 56 | #include 57 | 58 | //! [0] 59 | class TreeItem 60 | { 61 | public: 62 | explicit TreeItem(const QVector &data, TreeItem *parent = 0); 63 | ~TreeItem(); 64 | 65 | TreeItem *child(int number); 66 | int childCount() const; 67 | int columnCount() const; 68 | QVariant data(int column) const; 69 | bool insertChildren(int position, int count, int columns); 70 | bool insertColumns(int position, int columns); 71 | TreeItem *parent(); 72 | bool removeChildren(int position, int count); 73 | bool removeColumns(int position, int columns); 74 | int childNumber() const; 75 | bool setData(int column, const QVariant &value); 76 | 77 | private: 78 | QList childItems; 79 | QVector itemData; 80 | TreeItem *parentItem; 81 | }; 82 | //! [0] 83 | 84 | #endif // TREEITEM_H 85 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/include/dataset_playback/treemodel.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** 3 | ** Copyright (C) 2016 The Qt Company Ltd. 4 | ** Contact: https://www.qt.io/licensing/ 5 | ** 6 | ** This file is part of the examples of the Qt Toolkit. 7 | ** 8 | ** $QT_BEGIN_LICENSE:BSD$ 9 | ** Commercial License Usage 10 | ** Licensees holding valid commercial Qt licenses may use this file in 11 | ** accordance with the commercial license agreement provided with the 12 | ** Software or, alternatively, in accordance with the terms contained in 13 | ** a written agreement between you and The Qt Company. For licensing terms 14 | ** and conditions see https://www.qt.io/terms-conditions. For further 15 | ** information use the contact form at https://www.qt.io/contact-us. 16 | ** 17 | ** BSD License Usage 18 | ** Alternatively, you may use this file under the terms of the BSD license 19 | ** as follows: 20 | ** 21 | ** "Redistribution and use in source and binary forms, with or without 22 | ** modification, are permitted provided that the following conditions are 23 | ** met: 24 | ** * Redistributions of source code must retain the above copyright 25 | ** notice, this list of conditions and the following disclaimer. 26 | ** * Redistributions in binary form must reproduce the above copyright 27 | ** notice, this list of conditions and the following disclaimer in 28 | ** the documentation and/or other materials provided with the 29 | ** distribution. 30 | ** * Neither the name of The Qt Company Ltd nor the names of its 31 | ** contributors may be used to endorse or promote products derived 32 | ** from this software without specific prior written permission. 33 | ** 34 | ** 35 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 36 | ** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 37 | ** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 38 | ** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 39 | ** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 40 | ** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 41 | ** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 42 | ** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 43 | ** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 44 | ** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 45 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE." 46 | ** 47 | ** $QT_END_LICENSE$ 48 | ** 49 | ****************************************************************************/ 50 | 51 | #ifndef TREEMODEL_H 52 | #define TREEMODEL_H 53 | 54 | #include 55 | #include 56 | #include 57 | 58 | class TreeItem; 59 | 60 | //! [0] 61 | class TreeModel : public QAbstractItemModel 62 | { 63 | Q_OBJECT 64 | 65 | public: 66 | TreeModel(const QStringList &headers, const QString &data, 67 | QObject *parent = 0); 68 | ~TreeModel(); 69 | //! [0] //! [1] 70 | 71 | QVariant data(const QModelIndex &index, int role) const override; 72 | QVariant headerData(int section, Qt::Orientation orientation, 73 | int role = Qt::DisplayRole) const override; 74 | 75 | QModelIndex index(int row, int column, 76 | const QModelIndex &parent = QModelIndex()) const override; 77 | QModelIndex parent(const QModelIndex &index) const override; 78 | 79 | int rowCount(const QModelIndex &parent = QModelIndex()) const override; 80 | int columnCount(const QModelIndex &parent = QModelIndex()) const override; 81 | //! [1] 82 | 83 | //! [2] 84 | Qt::ItemFlags flags(const QModelIndex &index) const override; 85 | bool setData(const QModelIndex &index, const QVariant &value, 86 | int role = Qt::EditRole) override; 87 | bool setHeaderData(int section, Qt::Orientation orientation, 88 | const QVariant &value, int role = Qt::EditRole) override; 89 | 90 | bool insertColumns(int position, int columns, 91 | const QModelIndex &parent = QModelIndex()) override; 92 | bool removeColumns(int position, int columns, 93 | const QModelIndex &parent = QModelIndex()) override; 94 | bool insertRows(int position, int rows, 95 | const QModelIndex &parent = QModelIndex()) override; 96 | bool removeRows(int position, int rows, 97 | const QModelIndex &parent = QModelIndex()) override; 98 | 99 | private: 100 | void setupModelData(const QStringList &lines, TreeItem *parent); 101 | TreeItem *getItem(const QModelIndex &index) const; 102 | 103 | TreeItem *rootItem; 104 | }; 105 | //! [2] 106 | 107 | #endif // TREEMODEL_H 108 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dataset_playback 3 | 0.0.1 4 | 5 | Qt application for selecting a vehicle control trajectory 6 | 7 | Stewart Worrall 8 | BSD 9 | 10 | Stewart Worrall 11 | 12 | catkin 13 | 14 | qtbase5-dev 15 | h264_bag_playback 16 | dataset_tools 17 | dataset_msgs 18 | libqt5-core 19 | libqt5-gui 20 | libqt5-widgets 21 | roscpp 22 | 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "dataset_panel.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | 11 | int main(int argv, char **args) 12 | { 13 | ros::init(argv, args, "DatasetPlayback"); 14 | 15 | QApplication app(argv, args); 16 | 17 | DatasetPanel *textEdit = new DatasetPanel; 18 | 19 | QVBoxLayout *layout = new QVBoxLayout; 20 | layout->addWidget(textEdit); 21 | 22 | QWidget window; 23 | window.setLayout(layout); 24 | 25 | window.show(); 26 | 27 | return app.exec(); 28 | } 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_playback/src/treeitem.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** 3 | ** Copyright (C) 2016 The Qt Company Ltd. 4 | ** Contact: https://www.qt.io/licensing/ 5 | ** 6 | ** This file is part of the examples of the Qt Toolkit. 7 | ** 8 | ** $QT_BEGIN_LICENSE:BSD$ 9 | ** Commercial License Usage 10 | ** Licensees holding valid commercial Qt licenses may use this file in 11 | ** accordance with the commercial license agreement provided with the 12 | ** Software or, alternatively, in accordance with the terms contained in 13 | ** a written agreement between you and The Qt Company. For licensing terms 14 | ** and conditions see https://www.qt.io/terms-conditions. For further 15 | ** information use the contact form at https://www.qt.io/contact-us. 16 | ** 17 | ** BSD License Usage 18 | ** Alternatively, you may use this file under the terms of the BSD license 19 | ** as follows: 20 | ** 21 | ** "Redistribution and use in source and binary forms, with or without 22 | ** modification, are permitted provided that the following conditions are 23 | ** met: 24 | ** * Redistributions of source code must retain the above copyright 25 | ** notice, this list of conditions and the following disclaimer. 26 | ** * Redistributions in binary form must reproduce the above copyright 27 | ** notice, this list of conditions and the following disclaimer in 28 | ** the documentation and/or other materials provided with the 29 | ** distribution. 30 | ** * Neither the name of The Qt Company Ltd nor the names of its 31 | ** contributors may be used to endorse or promote products derived 32 | ** from this software without specific prior written permission. 33 | ** 34 | ** 35 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 36 | ** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 37 | ** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 38 | ** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 39 | ** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 40 | ** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 41 | ** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 42 | ** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 43 | ** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 44 | ** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 45 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE." 46 | ** 47 | ** $QT_END_LICENSE$ 48 | ** 49 | ****************************************************************************/ 50 | 51 | /* 52 | treeitem.cpp 53 | 54 | A container for items of data supplied by the simple tree model. 55 | */ 56 | 57 | #include "treeitem.h" 58 | 59 | #include 60 | 61 | //! [0] 62 | TreeItem::TreeItem(const QVector &data, TreeItem *parent) 63 | { 64 | parentItem = parent; 65 | itemData = data; 66 | } 67 | //! [0] 68 | 69 | //! [1] 70 | TreeItem::~TreeItem() 71 | { 72 | qDeleteAll(childItems); 73 | } 74 | //! [1] 75 | 76 | //! [2] 77 | TreeItem *TreeItem::child(int number) 78 | { 79 | return childItems.value(number); 80 | } 81 | //! [2] 82 | 83 | //! [3] 84 | int TreeItem::childCount() const 85 | { 86 | return childItems.count(); 87 | } 88 | //! [3] 89 | 90 | //! [4] 91 | int TreeItem::childNumber() const 92 | { 93 | if (parentItem) 94 | return parentItem->childItems.indexOf(const_cast(this)); 95 | 96 | return 0; 97 | } 98 | //! [4] 99 | 100 | //! [5] 101 | int TreeItem::columnCount() const 102 | { 103 | return itemData.count(); 104 | } 105 | //! [5] 106 | 107 | //! [6] 108 | QVariant TreeItem::data(int column) const 109 | { 110 | return itemData.value(column); 111 | } 112 | //! [6] 113 | 114 | //! [7] 115 | bool TreeItem::insertChildren(int position, int count, int columns) 116 | { 117 | if (position < 0 || position > childItems.size()) 118 | return false; 119 | 120 | for (int row = 0; row < count; ++row) { 121 | QVector data(columns); 122 | TreeItem *item = new TreeItem(data, this); 123 | childItems.insert(position, item); 124 | } 125 | 126 | return true; 127 | } 128 | //! [7] 129 | 130 | //! [8] 131 | bool TreeItem::insertColumns(int position, int columns) 132 | { 133 | if (position < 0 || position > itemData.size()) 134 | return false; 135 | 136 | for (int column = 0; column < columns; ++column) 137 | itemData.insert(position, QVariant()); 138 | 139 | foreach (TreeItem *child, childItems) 140 | child->insertColumns(position, columns); 141 | 142 | return true; 143 | } 144 | //! [8] 145 | 146 | //! [9] 147 | TreeItem *TreeItem::parent() 148 | { 149 | return parentItem; 150 | } 151 | //! [9] 152 | 153 | //! [10] 154 | bool TreeItem::removeChildren(int position, int count) 155 | { 156 | if (position < 0 || position + count > childItems.size()) 157 | return false; 158 | 159 | for (int row = 0; row < count; ++row) 160 | delete childItems.takeAt(position); 161 | 162 | return true; 163 | } 164 | //! [10] 165 | 166 | bool TreeItem::removeColumns(int position, int columns) 167 | { 168 | if (position < 0 || position + columns > itemData.size()) 169 | return false; 170 | 171 | for (int column = 0; column < columns; ++column) 172 | itemData.remove(position); 173 | 174 | foreach (TreeItem *child, childItems) 175 | child->removeColumns(position, columns); 176 | 177 | return true; 178 | } 179 | 180 | //! [11] 181 | bool TreeItem::setData(int column, const QVariant &value) 182 | { 183 | if (column < 0 || column >= itemData.size()) 184 | return false; 185 | 186 | itemData[column] = value; 187 | return true; 188 | } 189 | //! [11] 190 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dataset_tools) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 6 | 7 | 8 | #find catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | tf 12 | dataset_msgs 13 | std_msgs 14 | nav_msgs 15 | sensor_msgs 16 | rosbag 17 | image_transport 18 | tf2 19 | tf2_sensor_msgs 20 | tf2_geometry_msgs 21 | gmsl_frame_msg 22 | custom_point_types 23 | ) 24 | 25 | 26 | 27 | 28 | catkin_package( 29 | LIBRARIES ${PROJECT_NAME} 30 | CATKIN_DEPENDS nav_msgs sensor_msgs std_msgs sensor_msgs gmsl_frame_msg rosbag dataset_msgs 31 | INCLUDE_DIRS include 32 | ) 33 | 34 | 35 | include_directories( 36 | ${catkin_INCLUDE_DIRS} 37 | include/${PROJECT_NAME} 38 | ${PCL_INCLUDE_DIRS} 39 | ) 40 | 41 | 42 | add_library(${PROJECT_NAME} 43 | src/bag_output.cpp 44 | src/bag_input.cpp 45 | src/run_pipeline.cpp 46 | src/point_cloud_features_pipeline.cpp 47 | src/icp_matcher_pipeline.cpp 48 | src/behavior_tree_pipeline.cpp 49 | ) 50 | 51 | 52 | target_link_libraries(${PROJECT_NAME} 53 | ${catkin_LIBRARIES} 54 | ) 55 | 56 | 57 | # Mark other files for installation (e.g. launch and bag files, etc.) 58 | install(DIRECTORY 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) 60 | 61 | install(DIRECTORY include/${PROJECT_NAME}/ 62 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 63 | 64 | install(TARGETS ${PROJECT_NAME} 65 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 68 | 69 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/README.md: -------------------------------------------------------------------------------- 1 | The dataset tools contains a number of classes that are commonly used throughout 2 | nodes that work with the ACFR dataset. 3 | 4 | ## run_pipeline 5 | This code provides a function to publish a message, then block until receiving 6 | a specific response. The code allows waiting for multiple messages of different 7 | types. An example of an implementation using this base class is 8 | shown below: 9 | 10 | ``` 11 |
12 | #include "run_pipeline.hpp" 13 | 14 | #include "point_xyzir.h" 15 | #include "point_xyzirc.h" 16 | 17 | class ICPMatcherPipeline : public RunPipeline { 18 | 19 | public: 20 | ICPMatcherPipeline(); 21 | ~ICPMatcherPipeline() {} 22 | 23 | void receive_message(const pcl::PointCloud::Ptr& poles_pointcloud, 24 | const pcl::PointCloud::Ptr& corners_pointcloud); 25 | private: 26 | PipelineInput> input_poles, input_corners; 27 | PipelineOutput output_pose; 28 | 29 | }; 30 | 31 | 32 | #include "icp_matcher_pipeline.hpp" 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | 39 | ICPMatcherPipeline::ICPMatcherPipeline() { 40 | 41 | input_poles.Initialise("/velodyne/front/pole_stacker/average"); 42 | input_corners.Initialise("/velodyne/front/corner_stacker/average"); 43 | 44 | output_pose.Initialise("/icp/icp_matcher/odom_corrected", pipes_out); 45 | } 46 | 47 | 48 | void 49 | ICPMatcherPipeline::receive_message(const pcl::PointCloud::Ptr& poles_pointcloud, 50 | const pcl::PointCloud::Ptr& corners_pointcloud) { 51 | 52 | ResetMessageFlags(); 53 | 54 | input_poles.PublishMessage(poles_pointcloud); 55 | input_corners.PublishMessage(corners_pointcloud); 56 | 57 | ROS_INFO_STREAM("wait for ICP matcher"); 58 | 59 | if (WaitForMessages()) { 60 | ROS_INFO_STREAM("ICP Matcher received response " << output_pose.last_message->pose.pose.position.x << ", " 61 | << output_pose.last_message->pose.pose.position.y); 62 | } 63 | else { 64 | ROS_INFO_STREAM("ICP Matcher No messages received"); 65 | } 66 | } 67 | 68 | ``` -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/bag_input.hpp: -------------------------------------------------------------------------------- 1 | #ifndef bag_input_h 2 | #define bag_input_h 3 | 4 | #include 5 | 6 | // include messages to write to bag file 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | 15 | namespace rosbag { 16 | class Bag; 17 | } 18 | 19 | /*! 20 | * \brief Class to publish the ros messages 21 | * 22 | */ 23 | 24 | class BagInput { 25 | public: 26 | BagInput(); 27 | ~BagInput(); 28 | 29 | void ReadBag(std::string bag_file); 30 | 31 | std::function publish_odom_update; 32 | std::function publish_fix_update; 33 | std::function publish_speed_update; 34 | std::function publish_imu_update; 35 | std::function publish_pointcloud_update; 36 | 37 | std::set odom_update_topics; 38 | std::set fix_update_topics; 39 | std::set odom_speed_topics; 40 | std::set pointcloud_topics; 41 | std::set imu_topics; 42 | 43 | std::shared_ptr bag; 44 | 45 | 46 | }; 47 | 48 | 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/bag_output.hpp: -------------------------------------------------------------------------------- 1 | #ifndef bag_output_h 2 | #define bag_output_h 3 | 4 | // include messages to write to bag file 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | 13 | namespace rosbag { 14 | class Bag; 15 | } 16 | 17 | /*! 18 | * \brief Class to publish the ros messages 19 | * 20 | */ 21 | 22 | class BagOutput { 23 | public: 24 | BagOutput(); 25 | ~BagOutput(); 26 | 27 | void Initialise(std::string bag_file); 28 | 29 | void publish_odom(nav_msgs::Odometry &msg, std::string topic_name); 30 | void publish_fix(sensor_msgs::NavSatFix &msg, std::string topic_name); 31 | void publish_tf(tf::StampedTransform &msg, std::string topic_name); 32 | void publish_stats(dataset_msgs::LocaliserStats &msg, std::string topic_name); 33 | 34 | std::shared_ptr bag; 35 | 36 | }; 37 | 38 | 39 | 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/behavior_tree_pipeline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef BEHAVIOR_TREE_PIPELINE_H 2 | #define BEHAVIOR_TREE_PIPELINE_H 3 | #include "run_pipeline.hpp" 4 | #include 5 | 6 | 7 | class BehaviorTreePipeline : public RunPipeline { 8 | 9 | public: 10 | BehaviorTreePipeline(); 11 | ~BehaviorTreePipeline() {} 12 | 13 | void receive_message(const nav_msgs::Odometry::ConstPtr& input_odom); 14 | 15 | //no output function needed for this pipeline 16 | // std::function::Ptr&, const pcl::PointCloud::Ptr&)> publish_poles_corners; 17 | 18 | PipelineOutput output_prompt; 19 | 20 | private: 21 | PipelineInput input_prompt; 22 | 23 | 24 | }; 25 | 26 | #endif // BEHAVIOR_TREE_PIPELINE_H 27 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/icp_matcher_pipeline.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 27/08/19. 3 | // 4 | 5 | #ifndef LOCALISER_ICP_MATCHER_PIPELINE_HPP 6 | #define LOCALISER_ICP_MATCHER_PIPELINE_HPP 7 | 8 | #include "run_pipeline.hpp" 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class ICPMatcherPipeline : public RunPipeline { 16 | 17 | public: 18 | ICPMatcherPipeline(); 19 | ~ICPMatcherPipeline() {} 20 | 21 | void receive_message(const pcl::PointCloud::Ptr& poles_pointcloud, 22 | const pcl::PointCloud::Ptr& corners_pointcloud); 23 | 24 | std::function publish_pose; 25 | 26 | PipelineOutput output_pose; 27 | 28 | private: 29 | PipelineInput> input_poles, input_corners; 30 | 31 | double datum_x_; 32 | double datum_y_; 33 | 34 | tf::TransformListener transform_listener; 35 | }; 36 | 37 | #endif //LOCALISER_POINT_CLOUD_FEATURES_PIPELINE_HPP 38 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/pipeline_components.hpp: -------------------------------------------------------------------------------- 1 | #ifndef pipeline_components_h 2 | #define pipeline_components_h 3 | 4 | #include 5 | 6 | 7 | template 8 | class PipelineInput { 9 | public: 10 | 11 | PipelineInput() {} 12 | ~PipelineInput() {} 13 | 14 | typedef boost::shared_ptr MessageTypePtr; 15 | 16 | 17 | void Initialise(std::string topic) { 18 | ros::NodeHandle nh; 19 | publisher_ = nh.advertise(topic, 1); 20 | } 21 | 22 | void PublishMessage(MessageTypePtr message) { 23 | publisher_.publish(message); 24 | } 25 | 26 | private: 27 | 28 | std::string topic_; 29 | ros::Publisher publisher_; 30 | 31 | }; 32 | 33 | class PipelineConnectorOutput { 34 | public: 35 | 36 | PipelineConnectorOutput(): message_received_(false) { 37 | 38 | } 39 | 40 | bool message_received_; 41 | 42 | }; 43 | 44 | 45 | template 46 | class PipelineOutput : public PipelineConnectorOutput { 47 | public: 48 | 49 | typedef boost::shared_ptr MessageTypePtr; 50 | 51 | PipelineOutput() { 52 | MessageTypePtr last_message_(new MessageType); 53 | } 54 | ~PipelineOutput() {} 55 | 56 | void Initialise(std::string topic, std::vector &pipe_container) { 57 | ros::NodeHandle nh; 58 | subscriber_ = nh.subscribe(topic, 1, &PipelineOutput::SubscriberCallback, this); 59 | pipe_container.push_back(this); 60 | } 61 | 62 | MessageTypePtr last_message; 63 | 64 | private: 65 | 66 | void SubscriberCallback(const MessageTypePtr message) { 67 | last_message = message; 68 | message_received_ = true; 69 | } 70 | 71 | 72 | std::string topic_; 73 | ros::Subscriber subscriber_; 74 | }; 75 | 76 | 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/pipeline_images.hpp: -------------------------------------------------------------------------------- 1 | #ifndef pipeline_images_h 2 | #define pipeline_images_h 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | template 11 | class PipelineOutputImage : public PipelineConnectorOutput { 12 | public: 13 | 14 | typedef boost::shared_ptr MessageTypePtr; 15 | 16 | PipelineOutputImage() { 17 | MessageTypePtr last_message_(new MessageType); 18 | } 19 | ~PipelineOutputImage() {} 20 | 21 | void Initialise(std::string topic, std::vector &pipe_container) { 22 | ros::NodeHandle nh; 23 | image_transport::ImageTransport it(nh); 24 | subscriber_ = it.subscribe(topic, 1, &PipelineOutputImage::SubscriberCallback, this); 25 | pipe_container.push_back(this); 26 | } 27 | 28 | MessageTypePtr last_message; 29 | 30 | private: 31 | 32 | void SubscriberCallback(const MessageTypePtr &message) { 33 | last_message = message; 34 | message_received_ = true; 35 | } 36 | 37 | 38 | std::string topic_; 39 | image_transport::Subscriber subscriber_; 40 | }; 41 | 42 | 43 | template 44 | class PipelineInputImage { 45 | public: 46 | 47 | PipelineInputImage() {} 48 | ~PipelineInputImage() {} 49 | 50 | typedef boost::shared_ptr MessageTypePtr; 51 | 52 | 53 | void Initialise(std::string topic) { 54 | ros::NodeHandle nh; 55 | image_transport::ImageTransport it(nh); 56 | publisher_ = it.advertise(topic, 1); 57 | } 58 | 59 | void PublishMessage(MessageTypePtr message) { 60 | publisher_.publish(message); 61 | } 62 | 63 | private: 64 | 65 | std::string topic_; 66 | image_transport::Publisher publisher_; 67 | 68 | }; 69 | 70 | 71 | 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/point_cloud_features_pipeline.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 27/08/19. 3 | // 4 | 5 | #ifndef LOCALISER_POINT_CLOUD_FEATURES_PIPELINE_HPP 6 | #define LOCALISER_POINT_CLOUD_FEATURES_PIPELINE_HPP 7 | 8 | #include "run_pipeline.hpp" 9 | 10 | #include "custom_point_types/point_xyzir.h" 11 | #include "custom_point_types/point_xyzirc.h" 12 | 13 | class PointCloudFeaturesPipeline : public RunPipeline { 14 | 15 | public: 16 | PointCloudFeaturesPipeline(); 17 | ~PointCloudFeaturesPipeline() {} 18 | 19 | void receive_message(const sensor_msgs::PointCloud2::ConstPtr& input_pointcloud); 20 | 21 | std::function::Ptr&, const pcl::PointCloud::Ptr&)> publish_poles_corners; 22 | 23 | 24 | PipelineOutput> output_poles, output_corners; 25 | 26 | private: 27 | PipelineInput> input_pointcloud; 28 | 29 | }; 30 | 31 | #endif //LOCALISER_POINT_CLOUD_FEATURES_PIPELINE_HPP 32 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/include/dataset_tools/run_pipeline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef run_pipeline_h 2 | #define run_pipeline_h 3 | 4 | #include 5 | 6 | // include messages to write to bag file 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "pipeline_components.hpp" 14 | #include "pipeline_images.hpp" 15 | 16 | 17 | /*! 18 | * \brief Call an external pipeline (ros node) and wait for the responses 19 | * 20 | */ 21 | 22 | class RunPipeline { 23 | public: 24 | RunPipeline(); 25 | ~RunPipeline(); 26 | 27 | bool WaitForMessages(float time_out = 1.); 28 | void ResetMessageFlags(); 29 | 30 | protected: 31 | 32 | std::vector pipes_out; 33 | }; 34 | 35 | 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dataset_tools 3 | 0.0.1 4 | Some common tools that are used throughout the USYD dataset tools 5 | Stewart Worrall 6 | 7 | BSD 8 | 9 | http://sydney.edu.au/acfr/its 10 | 11 | Stewart Worrall 12 | 13 | catkin 14 | 15 | dataset_msgs 16 | roscpp 17 | rosbag 18 | gmsl_frame_msg 19 | std_msgs 20 | nav_msgs 21 | tf2_sensor_msgs 22 | image_transport 23 | sensor_msgs 24 | custom_point_types 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/src/bag_input.cpp: -------------------------------------------------------------------------------- 1 | #include "bag_input.hpp" 2 | 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | 18 | BagInput::BagInput() { 19 | 20 | bag = std::make_shared(); 21 | 22 | } 23 | 24 | BagInput::~BagInput() { 25 | bag->close(); 26 | } 27 | 28 | /* 29 | void BagOutput::publish_odom(nav_msgs::Odometry &msg, std::string topic_name) { 30 | if (bag.isOpen()) 31 | bag.write(topic_name, msg.header.stamp, msg); 32 | } 33 | 34 | 35 | void BagOutput::publish_fix(sensor_msgs::NavSatFix &msg, std::string topic_name) { 36 | if (bag.isOpen()) 37 | bag.write(topic_name, msg.header.stamp, msg); 38 | } 39 | */ 40 | 41 | void BagInput::ReadBag(std::string bag_file) { 42 | 43 | //PointCloudFeaturesPipeline run_pipeline(); 44 | 45 | bag->open(bag_file); 46 | for(rosbag::MessageInstance const m: rosbag::View(*bag)) 47 | { 48 | if (publish_imu_update) { 49 | auto msg = m.instantiate(); 50 | if (msg != NULL && imu_topics.count(m.getTopic()) != 0) 51 | publish_imu_update(msg); 52 | } 53 | 54 | if (publish_fix_update) { 55 | auto msg = m.instantiate(); 56 | if (msg && fix_update_topics.count(m.getTopic()) != 0) { 57 | publish_fix_update(msg); 58 | } 59 | } 60 | 61 | if (publish_pointcloud_update) { 62 | auto msg = m.instantiate(); 63 | if (msg && pointcloud_topics.count(m.getTopic()) != 0) 64 | publish_pointcloud_update(msg); 65 | } 66 | 67 | if (publish_speed_update) { 68 | auto msg = m.instantiate(); 69 | if (msg && odom_speed_topics.count(m.getTopic()) != 0) 70 | publish_speed_update(msg); 71 | } 72 | 73 | if (publish_odom_update) { 74 | auto msg = m.instantiate(); 75 | if (msg && odom_update_topics.count(m.getTopic()) != 0) 76 | publish_odom_update(msg); 77 | } 78 | 79 | if (!ros::ok()) 80 | break; 81 | 82 | ros::spinOnce(); 83 | } 84 | 85 | bag->close(); 86 | 87 | } 88 | 89 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/src/bag_output.cpp: -------------------------------------------------------------------------------- 1 | #include "bag_output.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | 17 | 18 | 19 | BagOutput::BagOutput() { 20 | 21 | bag = std::make_shared(); 22 | 23 | } 24 | 25 | BagOutput::~BagOutput() { 26 | bag->close(); 27 | } 28 | 29 | 30 | void BagOutput::publish_stats(dataset_msgs::LocaliserStats &msg, std::string topic_name){ 31 | if (bag->isOpen()) 32 | bag->write(topic_name, msg.header.stamp, msg); 33 | } 34 | 35 | 36 | void BagOutput::publish_odom(nav_msgs::Odometry &msg, std::string topic_name) { 37 | if (bag->isOpen()) 38 | bag->write(topic_name, msg.header.stamp, msg); 39 | } 40 | 41 | 42 | void BagOutput::publish_fix(sensor_msgs::NavSatFix &msg, std::string topic_name) { 43 | if (bag->isOpen()) 44 | bag->write(topic_name, msg.header.stamp, msg); 45 | } 46 | 47 | 48 | void BagOutput::publish_tf(tf::StampedTransform &msg, std::string topic_name) { 49 | if (bag->isOpen()) { 50 | 51 | tf2_msgs::TFMessage tf_pub_message; 52 | geometry_msgs::TransformStamped geom_tf; 53 | geom_tf.transform.rotation.x = msg.getRotation()[0]; 54 | geom_tf.transform.rotation.y = msg.getRotation()[1]; 55 | geom_tf.transform.rotation.z = msg.getRotation()[2]; 56 | geom_tf.transform.rotation.w = msg.getRotation()[3]; 57 | geom_tf.transform.translation.x = msg.getOrigin()[0]; 58 | geom_tf.transform.translation.y = msg.getOrigin()[1]; 59 | geom_tf.transform.translation.z = msg.getOrigin()[2]; 60 | geom_tf.header.stamp = msg.stamp_; 61 | geom_tf.header.frame_id = msg.frame_id_; 62 | geom_tf.child_frame_id = msg.child_frame_id_; 63 | tf_pub_message.transforms.push_back(geom_tf); 64 | 65 | bag->write(topic_name, msg.stamp_, tf_pub_message); 66 | } 67 | } 68 | 69 | 70 | 71 | void BagOutput::Initialise(std::string bag_file) { 72 | bag->open(bag_file, rosbag::bagmode::Write); 73 | } 74 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/src/behavior_tree_pipeline.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_tree_pipeline.hpp" 2 | 3 | BehaviorTreePipeline::BehaviorTreePipeline() 4 | { 5 | input_prompt.Initialise("/zio/odometry/rear"); 6 | 7 | output_prompt.Initialise("/tock", pipes_out); 8 | } 9 | 10 | 11 | 12 | void 13 | BehaviorTreePipeline::receive_message(const nav_msgs::Odometry::ConstPtr& input_odom) { 14 | 15 | // ROS_INFO_STREAM("sending tick to behavior tree" ); 16 | 17 | 18 | ResetMessageFlags(); 19 | 20 | input_prompt.PublishMessage(input_odom); 21 | 22 | if (WaitForMessages()) { 23 | // ROS_INFO_STREAM("received tock and return to read bag msgs"); 24 | 25 | // if (publish_poles_corners) { 26 | // publish_poles_corners(output_poles.last_message, output_corners.last_message); 27 | // } 28 | }else { 29 | ROS_INFO_STREAM("No messages received and return to read bag msgs"); 30 | output_prompt.last_message = NULL; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/src/icp_matcher_pipeline.cpp: -------------------------------------------------------------------------------- 1 | #include "icp_matcher_pipeline.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | ICPMatcherPipeline::ICPMatcherPipeline(): datum_x_(0.), datum_y_(0.) { 8 | 9 | input_poles.Initialise("/velodyne/front/pole_stacker/average"); 10 | input_corners.Initialise("/velodyne/front/corner_stacker/average"); 11 | 12 | output_pose.Initialise("/localiser/icp_matcher/odom_corrected", pipes_out); 13 | } 14 | 15 | 16 | void 17 | ICPMatcherPipeline::receive_message(const pcl::PointCloud::Ptr& poles_pointcloud, 18 | const pcl::PointCloud::Ptr& corners_pointcloud) { 19 | 20 | ResetMessageFlags(); 21 | 22 | input_poles.PublishMessage(poles_pointcloud); 23 | input_corners.PublishMessage(corners_pointcloud); 24 | 25 | 26 | // if (datum_x_ == 0. || datum_y_ == 0.) { 27 | 28 | // tf::StampedTransform transform; 29 | // try { 30 | // transform_listener.lookupTransform("utm", "map", ros::Time(0), transform); 31 | // datum_x_ = transform.getOrigin().x(); 32 | // datum_y_ = transform.getOrigin().y(); 33 | // ROS_WARN_STREAM("icp_matcher_pipeline initialised datum: " << datum_x_ << ", " << datum_y_ ); 34 | // } 35 | // catch (tf::TransformException &ex) { 36 | // ROS_WARN_STREAM_THROTTLE(1, "icp_matcher_pipeline is looking for datum: " << ex.what()); 37 | // } 38 | // } 39 | 40 | 41 | if (WaitForMessages()) { 42 | // ROS_INFO_STREAM("ICP Matcher received response " << output_pose.last_message->pose.pose.position.x << ", " 43 | // << output_pose.last_message->pose.pose.position.y); 44 | 45 | // output_pose.last_message->pose.pose.position.x += datum_x_; 46 | // output_pose.last_message->pose.pose.position.y += datum_y_; 47 | 48 | // if(std::isnan(output_pose.last_message->pose.pose.position.x) ){ 49 | // ROS_INFO_STREAM_THROTTLE(1, "ICP Matcher received response " << std::fixed << output_pose.last_message->pose.pose.position.x << ", " 50 | // << output_pose.last_message->pose.pose.position.y); 51 | // }else{ 52 | // ROS_INFO_STREAM_THROTTLE(1, "ICP Matcher received response " << std::fixed << output_pose.last_message->pose.pose.position.x << ", " 53 | // << output_pose.last_message->pose.pose.position.y ); 54 | // } 55 | 56 | if (publish_pose) { 57 | publish_pose(output_pose.last_message); 58 | } 59 | } 60 | else { 61 | ROS_INFO_STREAM("ICP Matcher No messages received"); 62 | output_pose.last_message = NULL; 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/src/point_cloud_features_pipeline.cpp: -------------------------------------------------------------------------------- 1 | #include "point_cloud_features_pipeline.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | PointCloudFeaturesPipeline::PointCloudFeaturesPipeline() { 9 | 10 | input_pointcloud.Initialise("/velodyne/front/points"); 11 | 12 | output_poles.Initialise("/velodyne/front/pole_stacker/average", pipes_out); 13 | output_corners.Initialise("/velodyne/front/corner_stacker/average", pipes_out); 14 | } 15 | 16 | 17 | void 18 | PointCloudFeaturesPipeline::receive_message(const sensor_msgs::PointCloud2::ConstPtr& input_pc_sensor_msg) { 19 | 20 | // ROS_INFO_STREAM("received pointcloud in pipeline " << input_pc_sensor_msg.fields.size()); 21 | 22 | // convert sensor_msg PointCloud2 into pcl message 23 | pcl::PointCloud::Ptr input_pointcloud_pcl(new pcl::PointCloud); 24 | pcl::fromROSMsg(*input_pc_sensor_msg, *input_pointcloud_pcl); 25 | 26 | ResetMessageFlags(); 27 | 28 | input_pointcloud.PublishMessage(input_pointcloud_pcl); 29 | 30 | if (WaitForMessages()) { 31 | //ROS_INFO_STREAM("received response " << output_poles.last_message->points.size() << ", " 32 | // << output_corners.last_message->points.size()); 33 | 34 | if (publish_poles_corners) { 35 | publish_poles_corners(output_poles.last_message, output_corners.last_message); 36 | } 37 | } 38 | else { 39 | ROS_INFO_STREAM("No messages received"); 40 | output_poles.last_message = NULL; 41 | output_corners.last_message = NULL; 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/dataset_tools/src/run_pipeline.cpp: -------------------------------------------------------------------------------- 1 | #include "run_pipeline.hpp" 2 | 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | 19 | RunPipeline::RunPipeline() { 20 | } 21 | 22 | 23 | RunPipeline::~RunPipeline() { 24 | } 25 | 26 | 27 | bool 28 | RunPipeline::WaitForMessages(float time_out) { 29 | 30 | ros::Time start_time = ros::Time::now(); 31 | bool messages_received = false; 32 | do { 33 | 34 | ros::spinOnce(); 35 | 36 | messages_received = true; 37 | for (auto pipe_out: pipes_out) { 38 | if (!pipe_out->message_received_) { 39 | messages_received = false; 40 | } 41 | } 42 | 43 | if ((ros::Time::now() - start_time).toSec() > time_out) { 44 | ROS_ERROR("wait for pipeline messages timed out"); 45 | break; 46 | } 47 | 48 | } while (!messages_received && ros::ok()); 49 | 50 | return messages_received; 51 | } 52 | 53 | 54 | void 55 | RunPipeline::ResetMessageFlags() { 56 | 57 | for (auto pipe_out: pipes_out) { 58 | pipe_out->message_received_ = false; 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-debug 2 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(h264_bag_playback) 3 | 4 | find_package(catkin REQUIRED COMPONENTS nodelet 5 | rosbag 6 | #rosbag_storage 7 | gmsl_frame_msg 8 | roscpp 9 | tf2_ros 10 | tf2_sensor_msgs 11 | image_transport 12 | dataset_msgs 13 | std_msgs 14 | cv_bridge 15 | nav_msgs 16 | sensor_msgs) 17 | 18 | find_package(OpenCV REQUIRED) 19 | 20 | ## Setup include directories 21 | include_directories(${catkin_INCLUDE_DIRS}, 22 | include/${PROJECT_NAME} 23 | include) 24 | 25 | catkin_package( 26 | LIBRARIES ${PROJECT_NAME} 27 | INCLUDE_DIRS include 28 | CATKIN_DEPENDS nodelet 29 | roscpp 30 | rosbag 31 | rosbag_storage 32 | gmsl_frame_msg 33 | image_transport 34 | dataset_msgs 35 | std_msgs 36 | tf2_ros 37 | tf2_sensor_msgs 38 | nav_msgs 39 | sensor_msgs 40 | cv_bridge 41 | ) 42 | 43 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 44 | 45 | 46 | # Create the nodelet tutorial library 47 | add_executable(${PROJECT_NAME}_node 48 | src/h264_bag_playback.cpp 49 | src/video.cpp 50 | src/helper_functions.cpp) 51 | 52 | target_link_libraries(${PROJECT_NAME}_node 53 | ${catkin_LIBRARIES} 54 | ${OpenCV_LIBS}) 55 | 56 | add_library(${PROJECT_NAME} 57 | src/h264_bag_playback.cpp 58 | src/video.cpp 59 | src/helper_functions.cpp 60 | src/bag_container.cpp) 61 | 62 | target_link_libraries(${PROJECT_NAME} 63 | ${catkin_LIBRARIES} 64 | ${OpenCV_LIBS}) 65 | 66 | 67 | if(catkin_EXPORTED_LIBRARIES) 68 | add_dependencies(${PROJECT_NAME} 69 | ${catkin_EXPORTED_LIBRARIES} 70 | ${catkin_LIBRARIES} 71 | ${OpenCV_LIBS}) 72 | endif() 73 | 74 | 75 | ## Mark other files for installation (e.g. launch and bag files, etc.) 76 | install(FILES nodelet_definition.xml 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 78 | 79 | ## Mark executables and/or libraries for installation 80 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 81 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 84 | ) 85 | 86 | install(DIRECTORY launch 87 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 88 | 89 | install(DIRECTORY config 90 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 91 | 92 | install(DIRECTORY include/${PROJECT_NAME}/ 93 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 94 | 95 | 96 | 97 | ## Add gtest based cpp test target and link libraries 98 | get_filename_component(BAG_TEST_FOLDER "test/" ABSOLUTE) 99 | configure_file(test/bag_data.h.in test/bag_data.h) 100 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/test) 101 | 102 | catkin_add_gtest(${PROJECT_NAME}-test 103 | test/test_playback.cpp 104 | ) 105 | 106 | if(TARGET ${PROJECT_NAME}-test) 107 | target_link_libraries(${PROJECT_NAME}-test h264_bag_playback) 108 | endif() 109 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/README.md: -------------------------------------------------------------------------------- 1 | **NOTE: this branch reads mp4 files, and allows the playback tool to seek immediately to the correct location in the file** 2 | 3 | Usage: First, you need to convert the existing .h264 files into .mp4 files. There is a script dataset_tools/h264_bag_playback/scripts/convert_videos.sh 4 | 5 | Run this script with ./convert_videos /root_folder_for_datasets 6 | This script currently expects a folder containing one or more dataset folders (i.e. /data/Week1, /data/Week2, etc). The script is designed to work off the /data folder 7 | 8 | Once the files are converted, run the playback tool the same way as before. The only difference you will notice is that the video seeks straight away, there is no "discarding frames" messages anymore. 9 | 10 | Previous README: 11 | h264_bag_tools is a c++ program that takes a bag file + h.264 files and replays them as if on a live system. The bag 12 | file must contain frame_info_msg messages that link each frame of the h.264 video files to the bag. The playback 13 | tool publishes topics for the uncorrected images /gmsl/\/image_color and the corrected images (given the 14 | camera_info_msgs) on /gmsl/\/image_rect_color. 15 | 16 | NOTE: The images are only extracted from the h.264 file if someone has subscribed to the topic. Because the h.264 17 | encoding requires a sequence of frames, all of the images to the current time must be read in sequence. This means that 18 | if you subscribe to an image topic after playback has commenced, the playback will pause while the program reads all of 19 | the images up to the current time. This can be a bit slow if you subscribe to the image topic a long time after playback 20 | starts. 21 | 22 | parameters: 23 | 24 | name="output_width" value in pixels 25 | set the width of the images 26 | NOTE: if either of the width/height parameters are not defined (or set to 0) the 27 | original image size is used 28 | 29 | name="output_height" value in pixels 30 | set the height of the images 31 | NOTE: if either of the width/height parameters are not defined (or set to 0) the 32 | original image size is used 33 | 34 | 35 | name="time_start" value in iso format (i.e. "2018-07-05T05:39:22.701861") 36 | If both of time_start and percentage_start is missing, it will play from the beginning 37 | On startup, the ROS INFO message will print the start and end times for the bag 38 | 39 | 40 | name="time_end" value in iso format (i.e. "2018-07-05T05:40:22.701861") 41 | If both of time_end and playback_end is missing, it will play through to the end 42 | On startup, the ROS INFO message will print the start and end times for the bag 43 | 44 | 45 | name="percentage_start" value in floating-point percentage (i.e. 23.4 will play from 23.4% of bag) 46 | If both of time_start and percentage_start is missing, it will play from the beginning 47 | If at least one of time_start and time_end parameter pair is specified, percentage_start and percentage_end won't be processed 48 | 49 | 50 | name="percentage_end" value in floating-point percentage (i.e. 99.99 will play until 99.99% of bag) 51 | If the end is missing, it will play through to the end 52 | If at least one of time_start and time_end parameter pair is specified, percentage_start and percentage_end won't be processed 53 | 54 | 55 | name="horizon_in_buffer" value in boolean, default to false 56 | if horizon_in_buffer set to true, will read in /vn100/imu messages and compute base_link->base_link_horizon and 57 | base_footprint->base_footprint_horizon. Buffer up to 2 seconds of horizon tfs in the future 58 | 59 | name="limit_playback_speed" value is boolean 60 | Either playback the bag + images as fast as possible, or restrict to (close to) 61 | realtime playback 62 | 63 | name="bag_file" value is a string 64 | The name of the bag file to read. The h.264 files and camera names are 65 | automatically extracted from the bag file name using the first part of the bag 66 | file name (without the extension) and the camera name separated using an '_'. 67 | Note that this is the standard format for the files used in the ACFR campus dataset 68 | 69 | additional requirements: 70 | * The gmsl_frame_msg package is required, and is available at https://gitlab.acfr.usyd.edu.au/nvidia/gmsl_frame_msg 71 | * ros-kinetic-tf2-sensor-msgs or ros-melodic-tf2-sensor-msgs is required, this package is not usually installed by default in ROS and is available through apt 72 | 73 | example usage: 74 | 75 | roslaunch h264_bag_playback h264_playback.launch bag_file_name:="/home/stew/data/callan-park/2019-04-15-14-37-06_callan_park_loop.bag" 76 | 77 | NOTE: change the output image size in h264_playback.launch 78 | 79 | 80 | 81 | tests: 82 | 83 | a new roscore has to be spinned up for all tests to pass. 84 | 85 | catkin run_tests --no-deps --this --verbose 86 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/config/time_offsets.yaml: -------------------------------------------------------------------------------- 1 | dataset_time_correction: 2 | 2018-08-29-17-03-45_Dataset_year: 0.015 3 | 2018-09-04-17-36-44_Dataset_year: 0.0225 4 | 2019-02-11-15-42-12_Dataset_year: 0.000001 5 | 2018-10-18-09-19-33_Dataset_year: -84606220.67 6 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/include/h264_bag_playback/bag_container.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 22/05/20. 3 | // 4 | 5 | #ifndef DATASET_TOOLS_BAG_CONTAINER_HPP 6 | #define DATASET_TOOLS_BAG_CONTAINER_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace dataset_toolkit { 15 | 16 | 17 | class BagContainer { 18 | public: 19 | 20 | ~BagContainer() { 21 | bag.close(); 22 | } 23 | 24 | bool Open(std::string file_name) { 25 | 26 | bag_file_name = file_name; 27 | 28 | bag.open(file_name); 29 | 30 | if (!bag.isOpen()) { 31 | ROS_INFO_STREAM("Could not OPEN bagfile " << file_name); 32 | return false; 33 | } 34 | 35 | rosbag::View connections_view(bag); 36 | for (const rosbag::ConnectionInfo *info: connections_view.getConnections()) { 37 | topics.insert(info->topic); 38 | } 39 | return true; 40 | } 41 | 42 | rosbag::Bag bag; 43 | std::shared_ptr view; 44 | rosbag::View::iterator iter; 45 | std::string bag_file_name; 46 | std::set topics; 47 | }; 48 | 49 | 50 | } 51 | 52 | #endif //DATASET_TOOLS_BAG_CONTAINER_HPP 53 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/include/h264_bag_playback/bag_static_transform_publisher.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 16/05/21. 3 | // 4 | 5 | #ifndef H264_BAG_PLAYBACK_BAGSTATICTRANSFORMPUBLISHER_HPP 6 | #define H264_BAG_PLAYBACK_BAGSTATICTRANSFORMPUBLISHER_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | class BagStaticTransformBroadcaster : public tf2_ros::StaticTransformBroadcaster { 14 | 15 | 16 | public: 17 | 18 | /** 19 | * @brief h264_bag_playback::StaticTfPublisher extract and publish /tf_static and store in member tf buffer transformer_ 20 | * only reads in the first 10 /tf_static messages and discard all msgs after 10, to prevent a node spaming /tf_static msgs. 21 | * @param bag bag that contains static tf 22 | * @param do_publish if want to publish /tf_static defaut to true. 23 | */ 24 | void StaticTfPublisher(rosbag::Bag &bag, bool do_publish, std::shared_ptr &transformer_) { 25 | 26 | //static tf2_ros::StaticTransformBroadcaster static_broadcaster; 27 | 28 | ros::NodeHandle private_nh("~"); 29 | std::vector topics; 30 | 31 | topics.push_back(std::string("tf_static")); 32 | topics.push_back(std::string("/tf_static")); 33 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 34 | int n_static_tf = 0; 35 | for (rosbag::MessageInstance const &m: view) { 36 | const auto tf = m.instantiate(); 37 | static_transforms.push_back(tf); 38 | 39 | for (const auto &transform: tf->transforms) { 40 | 41 | if (transform.child_frame_id == "velodyne_front_link") { 42 | // replace base->velodyne for testing 43 | 44 | tf2::Transform odom_tf; 45 | tf2::fromMsg(transform.transform, odom_tf); 46 | double r, p, yaw; 47 | odom_tf.getBasis().getRPY(r, p, yaw); 48 | ROS_ERROR_STREAM("original velodyne tf yaw " << yaw << " pitch " << p << " roll " << r); 49 | 50 | geometry_msgs::TransformStamped transformStamped; 51 | transformStamped.header.stamp = ros::Time::now(); 52 | transformStamped.header.frame_id = "base_link"; 53 | transformStamped.child_frame_id = "velodyne_front_link"; 54 | 55 | transformStamped.transform.translation.x = transform.transform.translation.x; 56 | transformStamped.transform.translation.y = transform.transform.translation.y; 57 | transformStamped.transform.translation.z = transform.transform.translation.z; 58 | 59 | float new_roll, new_pitch, new_yaw; 60 | private_nh.param("new_roll", new_roll, 0.); 61 | private_nh.param("new_pitch", new_pitch, 0.); 62 | private_nh.param("new_yaw", new_yaw, 0.); 63 | ROS_ERROR_STREAM("offseting velodyne tf yaw " << new_yaw << " pitch " << new_pitch << " roll " << new_roll); 64 | 65 | new_yaw += yaw; 66 | new_pitch += p; 67 | new_roll += r; 68 | 69 | tf2::Quaternion quat; 70 | quat.setRPY(new_roll, new_pitch, new_yaw); 71 | 72 | transformStamped.transform.rotation.x = quat.x(); 73 | transformStamped.transform.rotation.y = quat.y(); 74 | transformStamped.transform.rotation.z = quat.z(); 75 | transformStamped.transform.rotation.w = quat.w(); 76 | 77 | this->sendTransform(transformStamped); 78 | transformer_->setTransform(transformStamped, "zio", true); 79 | 80 | tf2::Transform odom_tf_2; 81 | tf2::fromMsg(transformStamped.transform, odom_tf_2); 82 | odom_tf_2.getBasis().getRPY(r, p, yaw); 83 | ROS_ERROR_STREAM("velodyne tf now reads yaw " << yaw << " pitch " << p << " roll " << r); 84 | 85 | } else if (transform.child_frame_id == "utm") { 86 | continue; 87 | } else { 88 | transformer_->setTransform(transform, "zio", true); 89 | } 90 | 91 | if (do_publish) { 92 | //this->sendTransform(tf->transforms); 93 | this->sendTransform(transform); 94 | } 95 | } 96 | n_static_tf++; 97 | if (n_static_tf > 10) 98 | break; 99 | 100 | } 101 | ROS_INFO_STREAM("Loaded static TF tree data"); 102 | } 103 | 104 | 105 | std::list static_transforms; 106 | 107 | }; 108 | 109 | #endif //H264_BAG_PLAYBACK_BAGSTATICTRANSFORMPUBLISHER_HPP 110 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/include/h264_bag_playback/corrected_imu_playback.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 16/05/21. 3 | // 4 | 5 | #ifndef H264_BAG_PLAYBACK_CORRECTED_IMU_PLAYBACK_HPP 6 | #define H264_BAG_PLAYBACK_CORRECTED_IMU_PLAYBACK_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | 14 | class CorrectedImuPlayback : public rosbag::View { 15 | 16 | public: 17 | 18 | CorrectedImuPlayback(rosbag::Bag const &bag, 19 | boost::function< bool(rosbag::ConnectionInfo const *)> query, 20 | ros::Time const & start_time = ros::TIME_MIN, 21 | ros::Time const & end_time = ros::TIME_MAX) : 22 | 23 | rosbag::View(bag, query, start_time, end_time), 24 | last_imu_time(ros::Time(0.01)){ 25 | 26 | } 27 | 28 | void ResetPlayback() { 29 | imu_iter = this->begin(); 30 | last_imu_time = ros::Time(0.01); 31 | } 32 | 33 | void CalculateHorizon(std::shared_ptr &transformer_, 34 | ros::Time ¤t_time/*, 35 | geometry_msgs::TransformStamped &baselink, 36 | geometry_msgs::TransformStamped &footprint*/) { 37 | 38 | // if horizonInBuffer param is set, calculate base_link to base_link_horizon tf 39 | // query the bag imu msgs so that transformer buffers horizon tf up to 2s in the future 40 | while (imu_iter != this->end() 41 | && last_imu_time < current_time + ros::Duration(2)) { 42 | 43 | auto imu_msg = imu_iter->instantiate(); 44 | 45 | if (imu_msg) { 46 | 47 | geometry_msgs::TransformStamped baselink, footprint; 48 | CorrectedImuPlayback::imu2horizontf(imu_msg, baselink, footprint); 49 | 50 | transformer_->setTransform(baselink, "zio", false); 51 | transformer_->setTransform(footprint, "zio", false); 52 | 53 | last_imu_time = imu_msg->header.stamp; 54 | imu_iter++; 55 | } 56 | } 57 | } 58 | 59 | static void 60 | imu2horizontf(sensor_msgs::Imu::Ptr &imu_msg, geometry_msgs::TransformStamped &baselink, 61 | geometry_msgs::TransformStamped &footprint) { 62 | if (imu_msg) { 63 | // calculate horizon to base tf, and push to tf buffer 64 | tf2::Transform imu_tf; 65 | tf2::Quaternion q1(imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z, imu_msg->orientation.w); 66 | imu_tf.setRotation(q1); 67 | double roll, pitch, yaw; 68 | imu_tf.getBasis().getRPY(roll, pitch, yaw); 69 | 70 | tf2::Quaternion q; 71 | q.setRPY(-roll, -pitch, 0.); 72 | 73 | baselink.transform.rotation.x = footprint.transform.rotation.x = q.x(); 74 | baselink.transform.rotation.y = footprint.transform.rotation.y = q.y(); 75 | baselink.transform.rotation.z = footprint.transform.rotation.z = q.z(); 76 | baselink.transform.rotation.w = footprint.transform.rotation.w = q.w(); 77 | baselink.header.stamp = footprint.header.stamp = imu_msg->header.stamp; 78 | baselink.header.frame_id = "base_link"; 79 | footprint.header.frame_id = "base_footprint"; 80 | baselink.child_frame_id = "base_link_horizon"; 81 | footprint.child_frame_id = "base_footprint_horizon"; 82 | } 83 | } 84 | 85 | ros::Time last_imu_time; 86 | 87 | rosbag::View::iterator imu_iter; 88 | }; 89 | 90 | 91 | #endif //H264_BAG_PLAYBACK_CORRECTED_IMU_PLAYBACK_HPP 92 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/include/h264_bag_playback/helper_functions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef H264_BAG_TOOLS_HELPER_FUNCTIONS_HPP 2 | #define H264_BAG_TOOLS_HELPER_FUNCTIONS_HPP 3 | 4 | #include 5 | #include 6 | 7 | std::string remove_last_of_string(const std::string& filename, const std::string divider_string); 8 | 9 | std::string keep_last_of_string(const std::string& filename, const std::string divider_string); 10 | 11 | void get_files_pattern(const std::string &pattern, std::vector &fileList); 12 | 13 | 14 | #endif //H264_BAG_TOOLS_HELPER_FUNCTIONS_HPP 15 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/include/h264_bag_playback/replace_msg.h: -------------------------------------------------------------------------------- 1 | #ifndef REPLACE_MSG_H 2 | #define REPLACE_MSG_H 3 | 4 | //#include 5 | 6 | //#include 7 | //#include 8 | 9 | #include 10 | 11 | //#include 12 | //#include 13 | 14 | //#include 15 | //#include //fabs 16 | 17 | #include 18 | 19 | //#include 20 | //#include 21 | //#include 22 | //#include 23 | //#include 24 | //#include 25 | //#include 26 | 27 | #include "h264_bag_playback.hpp" 28 | //#include "bag_container.hpp" 29 | //#include "video.hpp" 30 | 31 | 32 | namespace dataset_toolkit 33 | { 34 | 35 | 36 | class replace_msg : public h264_bag_playback { 37 | 38 | public: 39 | replace_msg(); 40 | 41 | 42 | protected: 43 | void MessagePublisher(ros::Publisher &publisher, const rosbag::MessageInstance &message); 44 | void CameraInfoPublisher(ros::Publisher &publisher, const rosbag::MessageInstance &message, 45 | const sensor_msgs::CameraInfoConstPtr &scaled_info_msg); 46 | void StaticTfPublisher(rosbag::Bag &bag, bool do_publish=true); 47 | 48 | 49 | rosbag::Bag out_bag; 50 | 51 | }; 52 | } 53 | 54 | #endif // REPLACE_MSG_H 55 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/include/h264_bag_playback/video.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VIDEO_HEADER 2 | #define VIDEO_HEADER 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | 14 | class Video { 15 | public: 16 | 17 | Video() {} 18 | 19 | bool InitialiseVideo(std::string camera_name, std::string video_filename); 20 | void InitialiseCameraInfo(sensor_msgs::CameraInfo &camera_info); 21 | 22 | bool valid_camera_info; 23 | 24 | // scale the camera info message for a different output size 25 | static void ScaleCameraInfoMsg(int original_width, 26 | int scaled_width, 27 | int original_height, 28 | int scaled_height, 29 | sensor_msgs::CameraInfo::Ptr &scaled_info_msg); 30 | 31 | 32 | bool SeekFrame(uint32_t requested_frame); 33 | 34 | cv::Mat camera_matrix, distance_coeffs, map1, map2; 35 | 36 | std::string file_name; 37 | 38 | sensor_msgs::CameraInfo camera_info_msg, corrected_camera_info_msg; 39 | 40 | cv::VideoCapture video_device; 41 | uint32_t frame_counter; 42 | 43 | 44 | ros::Publisher corrected_info_publisher; 45 | ros::Publisher uncorrected_info_publisher; 46 | 47 | image_transport::Publisher corrected_publisher; 48 | image_transport::Publisher uncorrected_publisher; 49 | }; 50 | 51 | 52 | 53 | 54 | 55 | #endif -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/launch/h264_playback.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 21 | 25 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/launch/nodelet_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/launch/replaceMsg_writeToBag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 19 | 23 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/nodelet_definition.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | description of the nodelet goes here 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/package.xml: -------------------------------------------------------------------------------- 1 | 2 | h264_bag_playback 3 | 0.0.1 4 | Project to synchronise the playback of h264 files and ros bags with frame_info messages included. 5 | Stewart Worrall 6 | Stewart Worrall 7 | BSD 8 | 9 | catkin 10 | 11 | nodelet 12 | roscpp 13 | std_msgs 14 | nav_msgs 15 | sensor_msgs 16 | dataset_msgs 17 | rosbag 18 | rosbag_storage 19 | cv_bridge 20 | gmsl_frame_msg 21 | image_transport 22 | tf2_ros 23 | tf2_sensor_msgs 24 | 25 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/scripts/convert_folder.sh: -------------------------------------------------------------------------------- 1 | 2 | GREEN='\033[0;32m' 3 | YELLOW='\033[1;33m' 4 | NC='\033[0m' 5 | 6 | input_folder=$1 7 | 8 | #find $input_folder/* -prune -type d | while IFS= read -r d; do 9 | d=${input_folder} 10 | for input_file in $d/*[0-9].h264; do 11 | if [ ! -f "$input_file" ]; then 12 | echo "Could not find video files in $d" 13 | else 14 | output_file=$(echo $input_file | sed "s/.h264/.mp4/g") 15 | temp_file=$(echo $input_file | sed "s/.h264/.tmp.mp4/g") 16 | echo "Found h264 file $input_file" 17 | if [ ! -f "$output_file" ]; then 18 | echo -e "${GREEN}Writing to $output_file${NC}" 19 | ffmpeg -nostats -hide_banner -loglevel error -y -i $input_file -map 0:v -vcodec copy -bsf:v h264_mp4toannexb $temp_file 20 | ffmpeg -nostats -hide_banner -loglevel error -y -fflags +genpts -r 30 -i $temp_file -vcodec copy $output_file 21 | rm $temp_file 22 | else 23 | echo -e "${YELLOW}$output_file already exists${NC}" 24 | fi 25 | fi 26 | done 27 | #done 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/scripts/convert_multiple_folders.sh: -------------------------------------------------------------------------------- 1 | 2 | GREEN='\033[0;32m' 3 | YELLOW='\033[1;33m' 4 | NC='\033[0m' 5 | 6 | input_folder=$1 7 | 8 | find $input_folder/* -prune -type d | while IFS= read -r d; do 9 | for input_file in $d/*[0-9].h264; do 10 | if [ ! -f "$input_file" ]; then 11 | echo "Could not find video files in $d" 12 | else 13 | output_file=$(echo $input_file | sed "s/.h264/.mp4/g") 14 | temp_file=$(echo $input_file | sed "s/.h264/.tmp.mp4/g") 15 | echo "Found h264 file $input_file" 16 | if [ ! -f "$output_file" ]; then 17 | echo -e "${GREEN}Writing to $output_file${NC}" 18 | ffmpeg -nostats -hide_banner -loglevel error -y -i $input_file -map 0:v -vcodec copy -bsf:v h264_mp4toannexb $temp_file 19 | ffmpeg -nostats -hide_banner -loglevel error -y -fflags +genpts -r 30 -i $temp_file -vcodec copy $output_file 20 | rm $temp_file 21 | else 22 | echo -e "${YELLOW}$output_file already exists${NC}" 23 | fi 24 | fi 25 | done 26 | done 27 | 28 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/setup.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #https://github.com/ethz-asl/lidar_align/issues/16 4 | 5 | sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak 6 | sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak 7 | 8 | sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h 9 | sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h 10 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/src/bag_container.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 22/05/20. 3 | // 4 | 5 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/src/helper_functions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | std::string remove_last_of_string(const std::string& filename, const std::string divider_string) { 6 | size_t lastdot = filename.find_last_of(divider_string); 7 | if (lastdot == std::string::npos) return filename; 8 | return filename.substr(0, lastdot); 9 | } 10 | 11 | 12 | std::string keep_last_of_string(const std::string& filename, const std::string divider_string) { 13 | size_t lastdot = filename.find_last_of(divider_string); 14 | if (lastdot == std::string::npos) return filename; 15 | return filename.substr(lastdot + 1); 16 | } 17 | 18 | void get_files_pattern(const std::string &pattern, std::vector &fileList) 19 | { 20 | //Declare glob_t for storing the results of globbing 21 | glob_t globbuf; 22 | 23 | //Glob.. GLOB_TILDE tells the globber to expand "~" in the pattern to the home directory 24 | glob( pattern.c_str(), GLOB_TILDE, NULL, &globbuf); 25 | 26 | for( int i = 0; i < globbuf.gl_pathc; ++i ) 27 | fileList.push_back( globbuf.gl_pathv[i] ); 28 | 29 | //Free the globbuf structure 30 | if( globbuf.gl_pathc > 0 ) 31 | globfree( &globbuf ); 32 | } 33 | 34 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/src/replace_msg.cpp: -------------------------------------------------------------------------------- 1 | #include "replace_msg.h" 2 | #include 3 | //#include 4 | //#include 5 | #include 6 | #include 7 | 8 | namespace dataset_toolkit 9 | { 10 | replace_msg::replace_msg() 11 | { 12 | 13 | out_bag.open("replace_msg.bag", rosbag::bagmode::Write); 14 | 15 | 16 | // bag.close(); 17 | } 18 | 19 | /** 20 | * @brief h264_bag_playback::StaticTfPublisher extract and publish /tf_static and store in member tf buffer transformer_ 21 | * only reads in the first 10 /tf_static messages and discard all msgs after 10, to prevent a node spaming /tf_static msgs. 22 | * @param bag bag that contains static tf 23 | * @param do_publish if want to publish /tf_static defaut to true. 24 | */ 25 | void replace_msg::StaticTfPublisher(rosbag::Bag &bag, bool do_publish) { 26 | 27 | static tf2_ros::StaticTransformBroadcaster static_broadcaster; 28 | 29 | std::vector topics; 30 | 31 | topics.push_back(std::string("tf_static")); 32 | topics.push_back(std::string("/tf_static")); 33 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 34 | int n_static_tf = 0; 35 | for(rosbag::MessageInstance const &m: view) { 36 | const auto tf = m.instantiate(); 37 | tf2_msgs::TFMessage::Ptr tf_corrected = boost::make_shared(); 38 | *tf_corrected = *tf; 39 | ROS_ERROR_STREAM("tf_corrected->transforms.size() "<transforms.size()); 40 | tf_corrected->transforms.clear(); 41 | 42 | if (do_publish) { 43 | static_broadcaster.sendTransform(tf->transforms); 44 | } 45 | 46 | for (auto &transform: tf->transforms) { 47 | 48 | if(transform.child_frame_id == "velodyne_front_link"){ 49 | // replace base->velodyne for testing 50 | 51 | tf2::Transform odom_tf; 52 | tf2::fromMsg(transform.transform, odom_tf); 53 | double r,p,yaw; 54 | odom_tf.getBasis().getRPY(r,p,yaw); 55 | ROS_ERROR_STREAM("original velodyne tf yaw "<("new_roll", new_roll, 0.); 68 | private_nh.param("new_pitch", new_pitch, 0.); 69 | private_nh.param("new_yaw", new_yaw, 0.); 70 | ROS_ERROR_STREAM("offseting velodyne tf yaw "<setTransform(transformStamped, "zio", true); 86 | tf_corrected->transforms.push_back(transformStamped); 87 | 88 | tf2::Transform odom_tf_2; 89 | tf2::fromMsg(transformStamped.transform, odom_tf_2); 90 | odom_tf_2.getBasis().getRPY(r,p,yaw); 91 | ROS_ERROR_STREAM("velodyne tf now reads yaw "<setTransform(transform, "zio", true); 97 | tf_corrected->transforms.push_back(transform); 98 | } 99 | 100 | } 101 | 102 | std::string const& topic = m.getTopic(); 103 | ros::Time const& header_time = m.getTime(); 104 | out_bag.write(topic, header_time, tf_corrected); 105 | 106 | n_static_tf++; 107 | if(n_static_tf>10) 108 | break; 109 | 110 | } 111 | ROS_INFO_STREAM("Loaded static TF tree data"); 112 | } 113 | 114 | 115 | void 116 | replace_msg::CameraInfoPublisher(ros::Publisher &publisher, const rosbag::MessageInstance &message, 117 | const sensor_msgs::CameraInfoConstPtr &scaled_info_msg) { 118 | std::string const& topic = message.getTopic(); 119 | ros::Time const& header_time = message.getTime(); 120 | out_bag.write(topic, header_time, scaled_info_msg); 121 | } 122 | 123 | 124 | void 125 | replace_msg::MessagePublisher(ros::Publisher &publisher, const rosbag::MessageInstance &message) { 126 | std::string const& topic = message.getTopic(); 127 | ros::Time const& header_time = message.getTime(); 128 | 129 | if (topic == "vn100/imu" || topic == "/vn100/imu") { 130 | 131 | auto msg = message.instantiate(); 132 | if (msg) { 133 | // compute horizon transforms from imu msg and publish them 134 | geometry_msgs::TransformStamped baselink, footprint; 135 | imu2horizontf(msg, baselink, footprint); 136 | // tf_broadcaster.sendTransform(baselink); 137 | // tf_broadcaster.sendTransform(footprint); 138 | 139 | tf2_msgs::TFMessage tf_horizon; 140 | tf_horizon.transforms.push_back(baselink); 141 | tf_horizon.transforms.push_back(footprint); 142 | 143 | out_bag.write("/tf", header_time, tf_horizon); 144 | } 145 | } 146 | 147 | out_bag.write(topic, header_time, message); 148 | } 149 | } 150 | 151 | int main(int argc, char **argv) { 152 | 153 | //Initialize Node and handles 154 | ros::init(argc, argv, "replaceMsg_writeToBag"); 155 | ros::NodeHandle n; 156 | 157 | dataset_toolkit::replace_msg bag_tools; 158 | //bag_tools.bypass_init(); 159 | bag_tools.init_playback(); 160 | bag_tools.ReadFromBag(); 161 | 162 | return 0; 163 | } 164 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/src/video.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | void 5 | Video::ScaleCameraInfoMsg(int original_width, 6 | int scaled_width, 7 | int original_height, 8 | int scaled_height, 9 | sensor_msgs::CameraInfo::Ptr &scaled_info_msg) { 10 | 11 | double scale_y = static_cast(scaled_height) / original_height; 12 | double scale_x = static_cast(scaled_width) / original_width; 13 | 14 | scaled_info_msg->height = scaled_height; 15 | scaled_info_msg->width = scaled_width; 16 | 17 | scaled_info_msg->K[0] = scaled_info_msg->K[0] * scale_x; // fx 18 | scaled_info_msg->K[2] = scaled_info_msg->K[2] * scale_x; // cx 19 | scaled_info_msg->K[4] = scaled_info_msg->K[4] * scale_y; // fy 20 | scaled_info_msg->K[5] = scaled_info_msg->K[5] * scale_y; // cy 21 | 22 | scaled_info_msg->P[0] = scaled_info_msg->P[0] * scale_x; // fx 23 | scaled_info_msg->P[2] = scaled_info_msg->P[2] * scale_x; // cx 24 | scaled_info_msg->P[3] = scaled_info_msg->P[3] * scale_x; // T 25 | scaled_info_msg->P[5] = scaled_info_msg->P[5] * scale_y; // fy 26 | scaled_info_msg->P[6] = scaled_info_msg->P[6] * scale_y; // cy 27 | } 28 | 29 | 30 | bool Video::SeekFrame(uint32_t requested_frame) { 31 | if (frame_counter != requested_frame) { 32 | 33 | // try to jump forward through the video 34 | if (video_device.set(CV_CAP_PROP_POS_FRAMES, requested_frame)) { 35 | ROS_INFO_STREAM("tracking video " << file_name << " from frame " << frame_counter << " to frame " << requested_frame); 36 | } 37 | else { 38 | ROS_ERROR_STREAM("could not move video " << file_name << " to frame " << requested_frame); 39 | } 40 | 41 | frame_counter = video_device.get(CV_CAP_PROP_POS_FRAMES); 42 | } 43 | 44 | // see if the frame was successfully found 45 | if (frame_counter == requested_frame) 46 | return true; 47 | 48 | return false; 49 | } 50 | 51 | 52 | void Video::InitialiseCameraInfo(sensor_msgs::CameraInfo &camera_info) { 53 | 54 | camera_info_msg = camera_info; 55 | corrected_camera_info_msg = sensor_msgs::CameraInfo(); 56 | 57 | uint32_t image_width = camera_info_msg.width; 58 | uint32_t image_height = camera_info_msg.height; 59 | 60 | cv::Size image_size = cv::Size(image_width, image_height); 61 | 62 | cv::Mat modified_camera_matrix; 63 | 64 | if (camera_info_msg.distortion_model == "rational_polynomial") { 65 | camera_matrix = cv::Mat(3, 3, CV_64F, &camera_info_msg.K[0]); 66 | distance_coeffs = cv::Mat(4, 1, CV_64F, &camera_info_msg.D[0]); 67 | } 68 | else if (camera_info_msg.distortion_model == "equidistant") { 69 | camera_matrix = cv::Mat(3, 3, CV_64F, &camera_info_msg.K[0]); 70 | distance_coeffs = cv::Mat(4, 1, CV_64F, &camera_info_msg.D[0]); 71 | 72 | //cv::Mat scaled_camera_matrix = camera_matrix * 73 | camera_matrix.at(2, 2) = 1.; 74 | 75 | //cv::Mat output_image; 76 | cv::Mat identity_mat = cv::Mat::eye(3, 3, CV_64F); 77 | 78 | cv::fisheye::estimateNewCameraMatrixForUndistortRectify(camera_matrix, distance_coeffs, image_size, 79 | identity_mat, modified_camera_matrix); 80 | 81 | cv::fisheye::initUndistortRectifyMap(camera_matrix, 82 | distance_coeffs, 83 | identity_mat, 84 | modified_camera_matrix, 85 | image_size, 86 | CV_16SC2, 87 | map1, map2); 88 | 89 | //The rectified_cameramat_ is the new cameramat after rectification: 90 | //cv::fisheye::estimateNewCameraMatrixForUndistortRectify(cameramat_, distcoeff4, raw_image.size(), cv::Matx33d::eye(), rectified_cameramat_, 1, cv_ptr->image.size(), fov_scale_); 91 | //cv::fisheye::initUndistortRectifyMap(cameramat_, distcoeff4, cv::Matx33d::eye(), rectified_cameramat_, cv_ptr->image.size(), CV_32FC1, map1, map2); 92 | //Then publish the new camera info out: 93 | //sensor_msgs::CameraInfo rectified_camera_info; 94 | 95 | //corrected_camera_info_msg.header = image_msg->header; 96 | corrected_camera_info_msg.distortion_model = "rational_polynomial"; // change to rational_polynomial after rectification 97 | corrected_camera_info_msg.height = camera_info_msg.height; 98 | corrected_camera_info_msg.width = camera_info_msg.width; 99 | corrected_camera_info_msg.K[0] = modified_camera_matrix.at(0,0); 100 | corrected_camera_info_msg.K[2] = modified_camera_matrix.at(0,2); 101 | corrected_camera_info_msg.K[4] = modified_camera_matrix.at(1,1); 102 | corrected_camera_info_msg.K[5] = modified_camera_matrix.at(1,2); 103 | corrected_camera_info_msg.K[8] = modified_camera_matrix.at(2,2); 104 | 105 | for(int i = 0; i < 8; i++) 106 | corrected_camera_info_msg.D.push_back(0.0); 107 | 108 | //rectified_camera_info_pub_.publish(rectified_camera_info); (edited) 109 | } 110 | 111 | valid_camera_info = true; 112 | } 113 | 114 | 115 | 116 | bool Video::InitialiseVideo(std::string camera_name, std::string video_filename) { 117 | 118 | ros::NodeHandle private_nh("~"); 119 | ros::NodeHandle public_nh; 120 | 121 | image_transport::ImageTransport image_transport(public_nh); 122 | 123 | valid_camera_info = false; 124 | file_name = video_filename; 125 | video_device = cv::VideoCapture(file_name); 126 | frame_counter = 0; 127 | std::string topic_prefix = "/gmsl/"; 128 | topic_prefix += camera_name; 129 | 130 | uncorrected_publisher = image_transport.advertise(topic_prefix + "/image_color", 1); 131 | corrected_publisher = image_transport.advertise(topic_prefix + "/rect/image_color", 1); 132 | 133 | corrected_info_publisher = public_nh.advertise(topic_prefix + "/rect/camera_info", 1); 134 | uncorrected_info_publisher = public_nh.advertise(topic_prefix + "/camera_info", 1); 135 | 136 | if(!video_device.isOpened()) { // check if we succeeded 137 | //ROS_INFO_STREAM("could not open video file: " << file_name << " called " << camera_name); 138 | return false; 139 | } 140 | 141 | return true; 142 | } 143 | 144 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/test/bag_data.h.in: -------------------------------------------------------------------------------- 1 | #define TEST_DATA_LOCATION "@BAG_TEST_FOLDER@" 2 | -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/test/pipeline.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/catkin_ws/src/dataset_tools/h264_bag_playback/test/pipeline.bag -------------------------------------------------------------------------------- /catkin_ws/src/dataset_tools/h264_bag_playback/test/test_playback.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include "boost/date_time/posix_time/posix_time.hpp" 5 | 6 | #include 7 | 8 | #include "h264_bag_playback.hpp" 9 | #include "bag_data.h" 10 | 11 | 12 | class DirectPlayback : public dataset_toolkit::h264_bag_playback { 13 | 14 | public: 15 | 16 | DirectPlayback(std::string bag_file) 17 | : h264_bag_playback() { 18 | 19 | private_nh.setParam("bag_file", bag_file); 20 | init_playback(); 21 | } 22 | 23 | 24 | void ImagePublisher(image_transport::Publisher &publisher, const sensor_msgs::ImageConstPtr &message) {} 25 | 26 | 27 | void CameraInfoPublisher(ros::Publisher &publisher, const rosbag::MessageInstance &message, 28 | const sensor_msgs::CameraInfoConstPtr &scaled_info_msg){} 29 | 30 | 31 | void MessagePublisher(ros::Publisher &publisher, const rosbag::MessageInstance &message) { 32 | if (stats.find(message.getTopic()) == stats.end()) { 33 | stats[message.getTopic()] = 0; 34 | } 35 | 36 | stats[message.getTopic()]++; 37 | } 38 | std::map stats; 39 | }; 40 | 41 | 42 | 43 | class BagReaderTest : public testing::Test { 44 | protected: 45 | 46 | virtual void SetUp() { 47 | } 48 | 49 | }; 50 | 51 | 52 | TEST_F(BagReaderTest, test_full_bag_read) { 53 | 54 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 55 | DirectPlayback playback(bag_file_name); 56 | 57 | // clear params 58 | playback.private_nh.setParam("time_start", ""); 59 | playback.private_nh.setParam("time_end", ""); 60 | playback.private_nh.setParam("percentage_start", 0.); 61 | playback.private_nh.setParam("percentage_end", 100.); 62 | 63 | playback.init_playback(); 64 | 65 | 66 | playback.ReadFromBag(); 67 | 68 | for(auto elem : playback.stats){ 69 | std::cout << elem.first << " " << elem.second << std::endl; 70 | } 71 | 72 | // checked from test file using rosbag info 73 | EXPECT_EQ(playback.stats["/vn100/imu"], 266); 74 | EXPECT_EQ(playback.stats["/tf"], 114); 75 | EXPECT_EQ(playback.stats["/velodyne_points"], 26); 76 | } 77 | 78 | TEST_F(BagReaderTest, test_full_bag_read_with_params) { 79 | 80 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 81 | DirectPlayback playback(bag_file_name); 82 | playback.private_nh.setParam("time_start", "2018-02-18T22:42:40.30"); 83 | playback.private_nh.setParam("time_end", "2018-02-18T22:42:43.0"); 84 | playback.init_playback(); 85 | playback.ReadFromBag(); 86 | 87 | // checked from test file using rosbag info 88 | EXPECT_EQ(playback.stats["/vn100/imu"], 266); 89 | EXPECT_EQ(playback.stats["/tf"], 114); 90 | EXPECT_EQ(playback.stats["/velodyne_points"], 26); 91 | 92 | } 93 | 94 | TEST_F(BagReaderTest, test_late_start) { 95 | 96 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 97 | DirectPlayback playback(bag_file_name); 98 | playback.private_nh.setParam("time_start", "2018-02-18T22:42:41.30"); 99 | playback.private_nh.setParam("time_end", ""); 100 | playback.init_playback(); 101 | playback.ReadFromBag(); 102 | 103 | // checked from test file using rqt_bag 104 | EXPECT_LT(playback.stats["/vn100/imu"], 200); 105 | EXPECT_LT(playback.stats["/tf"], 100); 106 | EXPECT_EQ(playback.stats["/velodyne_points"], 17); 107 | 108 | } 109 | 110 | TEST_F(BagReaderTest, test_early_end) { 111 | 112 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 113 | DirectPlayback playback(bag_file_name); 114 | playback.private_nh.setParam("time_start", ""); 115 | playback.private_nh.setParam("time_end", "2018-02-18T22:42:41.30"); 116 | playback.init_playback(); 117 | playback.ReadFromBag(); 118 | 119 | // checked from test file using rqt_bag 120 | EXPECT_LT(playback.stats["/vn100/imu"], 200); 121 | EXPECT_LT(playback.stats["/tf"], 100); 122 | EXPECT_EQ(playback.stats["/velodyne_points"], 9); 123 | 124 | } 125 | 126 | TEST_F(BagReaderTest, test_percentage_start) { 127 | 128 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 129 | DirectPlayback playback(bag_file_name); 130 | playback.private_nh.setParam("time_start", ""); 131 | playback.private_nh.setParam("time_end", ""); 132 | playback.private_nh.setParam("percentage_start", 99.8); 133 | playback.private_nh.setParam("percentage_end", -0.1); 134 | playback.init_playback(); 135 | playback.ReadFromBag(); 136 | 137 | for(auto elem : playback.stats){ 138 | std::cout << elem.first << " " << elem.second << std::endl; 139 | } 140 | 141 | // checked from test file using rqt_bag 142 | EXPECT_LT(playback.stats["/vn100/imu"], 50); 143 | EXPECT_LT(playback.stats["/tf"], 50); 144 | EXPECT_LT(playback.stats["/velodyne_points"], 9); 145 | 146 | } 147 | 148 | TEST_F(BagReaderTest, test_percentage_end) { 149 | 150 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 151 | DirectPlayback playback(bag_file_name); 152 | playback.private_nh.setParam("time_start", ""); 153 | playback.private_nh.setParam("time_end", ""); 154 | playback.private_nh.setParam("percentage_start", NAN); 155 | playback.private_nh.setParam("percentage_end", 0.1); 156 | playback.init_playback(); 157 | playback.ReadFromBag(); 158 | 159 | for(auto elem : playback.stats){ 160 | std::cout << elem.first << " " << elem.second << std::endl; 161 | } 162 | 163 | // checked from test file using rqt_bag 164 | EXPECT_LT(playback.stats["/vn100/imu"], 50); 165 | EXPECT_LT(playback.stats["/tf"], 50); 166 | EXPECT_LT(playback.stats["/velodyne_points"], 9); 167 | 168 | } 169 | 170 | 171 | TEST_F(BagReaderTest, test_percentage_mid) { 172 | 173 | std::string bag_file_name = std::string(TEST_DATA_LOCATION) + std::string("/pipeline.bag"); 174 | DirectPlayback playback(bag_file_name); 175 | playback.private_nh.setParam("time_start", ""); 176 | playback.private_nh.setParam("time_end", ""); 177 | playback.private_nh.setParam("percentage_start", 0.1); 178 | playback.private_nh.setParam("percentage_end", 0.2); 179 | playback.init_playback(); 180 | playback.ReadFromBag(); 181 | 182 | for(auto elem : playback.stats){ 183 | std::cout << elem.first << " " << elem.second << std::endl; 184 | } 185 | 186 | // checked from test file using rqt_bag 187 | EXPECT_LT(playback.stats["/vn100/imu"], 50); 188 | EXPECT_LT(playback.stats["/tf"], 50); 189 | EXPECT_LT(playback.stats["/velodyne_points"], 9); 190 | 191 | } 192 | 193 | int main(int argc, char **argv){ 194 | testing::InitGoogleTest(&argc, argv); 195 | 196 | ros::init(argc, argv, "tester"); 197 | ros::NodeHandle nh; 198 | 199 | return RUN_ALL_TESTS(); 200 | } 201 | -------------------------------------------------------------------------------- /catkin_ws/src/extraction/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(extraction) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 6 | 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | std_msgs 11 | h264_bag_playback 12 | dataset_tools 13 | dataset_msgs 14 | sensor_msgs 15 | custom_point_types 16 | tf 17 | tf_conversions 18 | tf2_msgs 19 | ibeo_object_msg 20 | lanelet2_core 21 | lanelet2_io 22 | lanelet2_projection 23 | lanelet2_routing 24 | lanelet2_traffic_rules 25 | lanelet2_validation 26 | ) 27 | 28 | # package currently needs opencv 29 | find_package(OpenCV 3 REQUIRED) 30 | 31 | ################################### 32 | ## catkin specific configuration ## 33 | ################################### 34 | catkin_package( 35 | #INCLUDE_DIRS ${PROJECT_NAME} 36 | ) 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | include_directories( 42 | include/${PROJECT_NAME} 43 | ${catkin_INCLUDE_DIRS} 44 | ) 45 | 46 | ## Declare a C++ executable 47 | add_executable(${PROJECT_NAME} src/extraction.cpp) 48 | 49 | target_link_libraries(${PROJECT_NAME} 50 | ${catkin_LIBRARIES} 51 | ${OpenCV_LIBRARIES}) 52 | 53 | 54 | ############# 55 | ## Install ## 56 | ############# 57 | install(TARGETS ${PROJECT_NAME} 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 61 | 62 | install(DIRECTORY launch/ 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 64 | ) 65 | install(DIRECTORY config/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 67 | ) 68 | -------------------------------------------------------------------------------- /catkin_ws/src/extraction/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | extraction 4 | 0.0.0 5 | Project the lidar onto one or more images with motion correction 6 | 7 | 8 | 9 | 10 | acfr 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | catkin 31 | 32 | roscpp 33 | 34 | dataset_msgs 35 | sensor_msgs 36 | tf2_msgs 37 | std_msgs 38 | pcl_ros 39 | h264_bag_playback 40 | dataset_tools 41 | custom_point_types 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /catkin_ws/src/feature_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(feature_model) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES feature_model 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/feature_model.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/feature_model_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_feature_model.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /catkin_ws/src/feature_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | feature_model 4 | 0.0.0 5 | The feature_model package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /catkin_ws/src/feature_model/script/rss.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright (c) 2018-2019 Intel Corporation 4 | # 5 | # This work is licensed under the terms of the MIT license. 6 | # For a copy, see . 7 | # 8 | 9 | class RSS: 10 | 11 | # Below ones are default parameter from the RSS library: https://intel.github.io/ad-rss-lib/documentation/Main.html#Section::ParameterDiscussion 12 | # RESPONSE TIME: 13 | # common sense value for human drivers is about 2 seconds, but this can be lower for AV. 14 | # LONGITUDINAL ACCELERATION: 15 | # Finding acceleration values for RSS is more complicated. These values may vary from ODD to ODD. 16 | # The minimum deceleration values must not exceed normal human driving behaviour. 17 | # Also big difference between the minimum and maximum acceleration will lead to defencisve driving. That leads to not participating in the dense traffic. 18 | # maximum acceleration a low speeds a standard car is in the range of 3.4 m/s4 to 7 m/s2 19 | # Required safety distance for the car at 50 km/h is brake_min = 4 m/s2 and brake_max = 8 m/s2 20 | # Rule of thump in german driving schools are brake_min = 4 m/s2 and brake_max = 8 m/s2 21 | # We are only considering one way and longitudinal safe distance. 22 | 23 | def __init__(self): 24 | # V_ego in m/s 25 | # V_other in m/s 26 | 27 | self.res_ego = 1 #s 28 | self.res_other = 2 #s 29 | self.accel_max = 3.5 #m/s2 30 | self.brake_min = 4 #m/s2 31 | self.break_max = 8 #m/s2 32 | #lat_break_min = 0.8 #m/s2 33 | #lat_accel_max = 0.2 #m/s2 34 | #margin = 10 #cm 35 | 36 | self.vb_vel_margin = 0.25 #m/s 37 | 38 | 39 | def calculate_rss_safe_dist(self, V_ego, V_other): 40 | long_d_min = self.long_safe_dist(V_ego, V_other) 41 | 42 | return long_d_min 43 | 44 | def long_safe_dist(self, V_ego, V_other): 45 | first_term = V_ego*self.res_ego 46 | second_term = ((self.accel_max*self.res_ego**2)/2) 47 | third_term = (((V_ego+(self.res_ego*self.accel_max))**2)/(2*self.brake_min)) 48 | fourth_term = ((V_other**2)/(2*self.break_max)) 49 | d_min = first_term+second_term+third_term-fourth_term 50 | 51 | return d_min 52 | 53 | 54 | -------------------------------------------------------------------------------- /catkin_ws/src/gmsl_frame_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(gmsl_frame_msg) 3 | 4 | #set (CMAKE_CXX_STANDARD 11) 5 | 6 | # FindDriveworks.cmake, ArchConfiguration.cmake, and LibFindMacros.cmake were needed for my setup they are taken from driveworks/samples/cmake/ 7 | # ArchConfiguration.cmake was the only file that needed small changes, remove the fatal error on line 17 and add the following lines in its place 8 | # set(VIBRANTE TRUE) 9 | # add_definitions(-DVIBRANTE) 10 | # this is the path I placed the driveworks cmake files in 11 | #set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 12 | 13 | 14 | find_package(catkin REQUIRED COMPONENTS 15 | std_msgs 16 | message_generation 17 | ) 18 | 19 | add_message_files( 20 | FILES 21 | FrameInfo.msg 22 | ) 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | ) 28 | 29 | catkin_package( 30 | ) 31 | 32 | #link_directories( 33 | # ${Driveworks_LIBRARY}) 34 | 35 | #include_directories( 36 | # ${catkin_INCLUDE_DIRS} 37 | # ${Driveworks_INCLUDE_DIR} 38 | # ${CUDA_INCLUDE_DIRS} 39 | # ${CMAKE_CURRENT_SOURCE_DIR}/src 40 | # ${CMAKE_CURRENT_SOURCE_DIR}/src/driveworks-src 41 | # ${GLEW_INCLUDE_DIR} 42 | #) 43 | 44 | # TODO: add a FindNvmedia.cmake file for this? Why does it not exist? 45 | #include_directories(/usr/share/visionworks/sources/3rdparty/nvmedia/ 46 | #/usr/share/visionworks/VisionWorks-1.6-Samples/3rdparty/glfw3/include/ 47 | #) 48 | 49 | 50 | # ros node name template 51 | #set(NODE_NAME ${PROJECT_NAME}_node) 52 | 53 | #add_executable(${NODE_NAME} src/main.cpp 54 | # src/driveworks-src/ProgramArguments.cpp 55 | # src/driveworks-src/ConsoleColor.cpp 56 | # src/driveworks-src/WindowEGL.cpp 57 | # src/driveworks-src/WindowGLFW.cpp 58 | # src/driveworks-src/Grid.cpp 59 | # src/ROSInterface.cpp 60 | #) 61 | 62 | #target_link_libraries(${NODE_NAME} 63 | # ${catkin_LIBRARIES} 64 | # nvmedia 65 | # ${GLEW_LIBRARY} 66 | # ${Driveworks_LIBRARY} 67 | # #driveworks 68 | # GLESv2 69 | # EGL 70 | # drm 71 | # #glfw3 72 | # ${CUDA_LIBRARY} 73 | # ${OpenCV_LIBS} 74 | #) 75 | -------------------------------------------------------------------------------- /catkin_ws/src/gmsl_frame_msg/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/catkin_ws/src/gmsl_frame_msg/README.md -------------------------------------------------------------------------------- /catkin_ws/src/gmsl_frame_msg/msg/FrameInfo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string name 3 | uint64 camera_timestamp 4 | time ros_timestamp 5 | uint64 frame_counter 6 | uint64 global_counter 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/gmsl_frame_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gmsl_frame_msg 4 | 1.0.0 5 | 6 | Message definition for gmsl video stream 7 | 8 | 9 | catkin 10 | 11 | message_generation 12 | message_runtime 13 | 14 | wei zhou 15 | BSD 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/ibeo_object_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(ibeo_object_msg) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_runtime 7 | std_msgs 8 | nav_msgs 9 | geometry_msgs 10 | sensor_msgs 11 | shape_msgs 12 | message_generation 13 | ) 14 | 15 | add_message_files( 16 | FILES 17 | IbeoObject.msg 18 | ) 19 | 20 | 21 | generate_messages( 22 | DEPENDENCIES 23 | nav_msgs 24 | geometry_msgs 25 | sensor_msgs 26 | shape_msgs 27 | std_msgs 28 | ) 29 | 30 | 31 | catkin_package( 32 | #LIBRARIES ${PROJECT_NAME} 33 | CATKIN_DEPENDS message_generation message_runtime nav_msgs geometry_msgs sensor_msgs shape_msgs 34 | ) 35 | 36 | include_directories( 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | #add_dependencies(${PROJECT_NAME} 41 | # ${${PROJECT_NAME}_EXPORTED_TARGETS} 42 | # ${catkin_EXPORTED_TARGETS} 43 | #) 44 | 45 | -------------------------------------------------------------------------------- /catkin_ws/src/ibeo_object_msg/msg/IbeoObject.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 object_id 3 | uint8 object_class 4 | uint16 object_class_uncertainty 5 | float32 object_age 6 | float32 object_class_age 7 | bool mobile 8 | bool motion_model_validated 9 | geometry_msgs/PoseWithCovariance pose 10 | geometry_msgs/TwistWithCovariance twist 11 | shape_msgs/SolidPrimitive shape 12 | -------------------------------------------------------------------------------- /catkin_ws/src/ibeo_object_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ibeo_object_msg 4 | 0.0.0 5 | 6 | Message definition for ibeo object data 7 | 8 | 9 | catkin 10 | 11 | message_generation 12 | message_runtime 13 | nav_msgs 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | shape_msgs 18 | 19 | wei zhou 20 | BSD 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /catkin_ws/src/lane_points_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(lane_points_msg) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_runtime 7 | std_msgs 8 | message_generation 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | LanePoints.msg 14 | ) 15 | 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | std_msgs 20 | ) 21 | 22 | 23 | catkin_package( 24 | #LIBRARIES ${PROJECT_NAME} 25 | CATKIN_DEPENDS message_generation message_runtime 26 | ) 27 | 28 | include_directories( 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | #add_dependencies(${PROJECT_NAME} 33 | # ${${PROJECT_NAME}_EXPORTED_TARGETS} 34 | # ${catkin_EXPORTED_TARGETS} 35 | #) 36 | 37 | -------------------------------------------------------------------------------- /catkin_ws/src/lane_points_msg/msg/LanePoints.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 max_x 3 | int32 max_y 4 | std_msgs/Int32MultiArray x 5 | std_msgs/Int32MultiArray y 6 | std_msgs/Float32MultiArray i 7 | std_msgs/Int32MultiArray odom_x 8 | std_msgs/Int32MultiArray odom_y 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/lane_points_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lane_points_msg 4 | 0.0.0 5 | 6 | Message definition for lane points data 7 | 8 | 9 | catkin 10 | 11 | message_generation 12 | message_runtime 13 | std_msgs 14 | 15 | Dhanoop Karunakaran 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.1) 2 | project(laneletsmap_generator) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | h264_bag_playback 6 | roscpp 7 | dataset_tools 8 | std_msgs 9 | pcl_ros 10 | tf 11 | tf_conversions 12 | custom_point_types 13 | lane_points_msg 14 | lanelet2_core 15 | lanelet2_io 16 | lanelet2_projection 17 | lanelet2_routing 18 | lanelet2_traffic_rules 19 | lanelet2_validation) 20 | 21 | find_package(OpenCV REQUIRED) 22 | 23 | ## Setup include directories 24 | include_directories( 25 | ${catkin_INCLUDE_DIRS} 26 | include/${PROJECT_NAME} 27 | ) 28 | 29 | catkin_package( 30 | LIBRARIES ${PROJECT_NAME} 31 | INCLUDE_DIRS include 32 | CATKIN_DEPENDS roscpp std_msgs pcl_ros tf_conversions tf h264_bag_playback dataset_tools 33 | ) 34 | 35 | find_package(PCL REQUIRED) 36 | include_directories(${PCL_INCLUDE_DIRS}) 37 | link_directories(${PCL_LIBRARY_DIRS}) 38 | add_definitions(${PCL_DEFINITIONS}) 39 | 40 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 41 | 42 | add_executable(${PROJECT_NAME} 43 | src/laneletsmap_generator.cpp) 44 | 45 | target_link_libraries(${PROJECT_NAME} 46 | ${catkin_LIBRARIES} 47 | ${PCL_LIBRARIES} 48 | ${OpenCV_LIBS} 49 | ) 50 | if(catkin_EXPORTED_LIBRARIES) 51 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_LIBRARIES}) 52 | endif() 53 | 54 | ## Mark the nodelet library for installations 55 | install(TARGETS ${PROJECT_NAME} 56 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 59 | 60 | ## Mark other files for installation (e.g. launch and bag files, etc.) 61 | install(DIRECTORY launch/ 62 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 63 | ) 64 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/README.md: -------------------------------------------------------------------------------- 1 | # Package: laneletsmap_generator 2 | 3 | ## Description: 4 | Projects the ibeo lidar data from a bag file into an image and laneletmap in osm format. 5 | 6 | ### laneletsmap_generator 7 | 8 | **Description** : Projects the ibeo lidar information into a 2d grid which is output to an image file and laneletmap in osm format 9 | 10 | **Parameters** 11 | * bag_file: bag file for input data 12 | * output_image: output image file (can be any opencv type) 13 | * laneletsmap_file: location to save laneletsmap 14 | 15 | * min_intensity, max_intensity: range of lidar intensity for colourising the projection - different lidars have different ranges of reported intensity 16 | 17 | * parameters for the h264 bag playback - some useful parameters listed below, for more parameters see h264_bag_playback project 18 | * percentage_start: place in the bag file to start 19 | * percentage_end: place in the bag file to end 20 | * horizon_in_buffer: set value="true" to enable the correction of the moving platform using the IMU 21 | 22 | * use_rings: an array of numbers for the lidar ring numbers to use in the projection 23 | 24 | **Author** : Dhanoop karunakaran 25 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/include/laneletsmap_generator/Conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef CONVERSIONS_H 2 | #define CONVERSIONS_H 3 | 4 | 5 | typedef struct { 6 | double r; // a fraction between 0 and 1 7 | double g; // a fraction between 0 and 1 8 | double b; // a fraction between 0 and 1 9 | } rgb; 10 | 11 | typedef struct { 12 | double h; // angle in degrees 13 | double s; // a fraction between 0 and 1 14 | double v; // a fraction between 0 and 1 15 | } hsv; 16 | 17 | static hsv rgb2hsv(rgb in); 18 | static rgb hsv2rgb(hsv in); 19 | 20 | hsv rgb2hsv(rgb in) 21 | { 22 | hsv out; 23 | double min, max, delta; 24 | 25 | min = in.r < in.g ? in.r : in.g; 26 | min = min < in.b ? min : in.b; 27 | 28 | max = in.r > in.g ? in.r : in.g; 29 | max = max > in.b ? max : in.b; 30 | 31 | out.v = max; // v 32 | delta = max - min; 33 | if (delta < 0.00001) 34 | { 35 | out.s = 0; 36 | out.h = 0; // undefined, maybe nan? 37 | return out; 38 | } 39 | if( max > 0.0 ) { // NOTE: if Max is == 0, this divide would cause a crash 40 | out.s = (delta / max); // s 41 | } else { 42 | // if max is 0, then r = g = b = 0 43 | // s = 0, h is undefined 44 | out.s = 0.0; 45 | out.h = NAN; // its now undefined 46 | return out; 47 | } 48 | if( in.r >= max ) // > is bogus, just keeps compilor happy 49 | out.h = ( in.g - in.b ) / delta; // between yellow & magenta 50 | else 51 | if( in.g >= max ) 52 | out.h = 2.0 + ( in.b - in.r ) / delta; // between cyan & yellow 53 | else 54 | out.h = 4.0 + ( in.r - in.g ) / delta; // between magenta & cyan 55 | 56 | out.h *= 60.0; // degrees 57 | 58 | if( out.h < 0.0 ) 59 | out.h += 360.0; 60 | 61 | return out; 62 | } 63 | 64 | 65 | rgb hsv2rgb(hsv in) 66 | { 67 | double hh, p, q, t, ff; 68 | long i; 69 | rgb out; 70 | 71 | if(in.s <= 0.0) { // < is bogus, just shuts up warnings 72 | out.r = in.v; 73 | out.g = in.v; 74 | out.b = in.v; 75 | return out; 76 | } 77 | hh = in.h; 78 | if(hh >= 360.0) hh = 0.0; 79 | hh /= 60.0; 80 | i = (long)hh; 81 | ff = hh - i; 82 | p = in.v * (1.0 - in.s); 83 | q = in.v * (1.0 - (in.s * ff)); 84 | t = in.v * (1.0 - (in.s * (1.0 - ff))); 85 | 86 | switch(i) { 87 | case 0: 88 | out.r = in.v; 89 | out.g = t; 90 | out.b = p; 91 | break; 92 | case 1: 93 | out.r = q; 94 | out.g = in.v; 95 | out.b = p; 96 | break; 97 | case 2: 98 | out.r = p; 99 | out.g = in.v; 100 | out.b = t; 101 | break; 102 | 103 | case 3: 104 | out.r = p; 105 | out.g = q; 106 | out.b = in.v; 107 | break; 108 | case 4: 109 | out.r = t; 110 | out.g = p; 111 | out.b = in.v; 112 | break; 113 | case 5: 114 | default: 115 | out.r = in.v; 116 | out.g = p; 117 | out.b = q; 118 | break; 119 | } 120 | return out; 121 | } 122 | 123 | 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/include/laneletsmap_generator/draw_shapes.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by stew on 3/06/20. 3 | // 4 | 5 | #ifndef TOP_DOWN_PROJECTION_DRAW_SHAPES_HPP 6 | #define TOP_DOWN_PROJECTION_DRAW_SHAPES_HPP 7 | 8 | class DrawItem 9 | { 10 | public: 11 | DrawItem() : alpha(255), shape_size(1) {} 12 | DrawItem(uint8_t _alpha, uint8_t _shape_size) : alpha(_alpha), shape_size(_shape_size) {} 13 | 14 | virtual void drawItem(cv::Mat &image, cv::Point &point, cv::Vec4b &colour) { 15 | 16 | } 17 | 18 | unsigned short alpha; 19 | unsigned short shape_size; 20 | std::string topic_name; 21 | std::string field_name = "intensity"; 22 | }; 23 | 24 | 25 | class CircleItem : public DrawItem 26 | { 27 | public: 28 | 29 | CircleItem() : DrawItem(255,3) {} 30 | CircleItem(uint8_t _alpha, uint8_t _shape_size) : DrawItem(_alpha, _shape_size) {} 31 | 32 | void drawItem(cv::Mat &image, cv::Point &point, cv::Vec4b &colour) { 33 | // Set the corresponding pixel to the RBG colour 34 | colour[3] = alpha; 35 | 36 | // std::cout << "here " << alpha << ", " << shape_size << ", " << topic_name << std::endl; 37 | // Set the alpha value for the blurred layer 38 | cv::circle(image, point, shape_size, colour, CV_FILLED); 39 | } 40 | }; 41 | 42 | class PointItem : public DrawItem 43 | { 44 | public: 45 | 46 | PointItem() : DrawItem(255,3) {} 47 | PointItem(uint8_t _alpha, uint8_t _shape_size) : DrawItem(_alpha, _shape_size) {} 48 | 49 | void drawItem(cv::Mat &image, cv::Point &point, cv::Vec4b &colour) { 50 | // Set the corresponding pixel to the RBG colour 51 | colour[3] = alpha; 52 | 53 | // Set the alpha value for the blurred layer 54 | image.at(point) = colour; 55 | } 56 | }; 57 | 58 | 59 | 60 | 61 | #endif //TOP_DOWN_PROJECTION_DRAW_SHAPES_HPP 62 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/launch/project_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | ["ibeo/lidar/dynamic,point,150,3", 6 | "ibeo/lidar/static,point,150,3", 7 | "/velodyne/front/pole_extractor/observations,1,10", 8 | "/velodyne/front/points,circle,10,6", 9 | "/velodyne/front/points,point,255,1", 10 | "/velodyne/front/pole_extractor/points/all,circle,200,16", 11 | "/velodyne/front/corner_extractor/points/all,circle,200,16", 12 | "/pointcloud,point,255,1"] 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | [90, 91, 92, 93, 94] 32 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/launch/semantic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ["/velodyne/front/pointsB0B1A2A1A0_label_filtered,circle,100,6","/velodyne/front/pointsB0B1A2A1A0_label_filtered,point,255,1"] 5 | 6 | 7 | 8 | 9 | 10 | [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,80, 81, 90, 91] 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/launch/test_parameters.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | ["ibeo/lidar/static,1,3", 8 | "/velodyne/front/pole_extractor/observations,1,10", 9 | "/velodyne/front/points,circle,10,6", 10 | "/velodyne/front/points,point,255,1", 11 | "/velodyne/front/pole_extractor/points/all,circle,200,16", 12 | "/velodyne/front/corner_extractor/points/all,circle,200,16", 13 | "/pointcloud,point,255,1"] 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,80, 81, 90, 91] 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | laneletsmap_generator 3 | 1.0.0 4 | Convert lidar sequence to a png. 5 | Dhanoop Karunakaran 6 | Dhanoop Karunakaran 7 | BSD 8 | http://acfr 9 | 10 | catkin 11 | libpcl-all 12 | pcl_ros 13 | h264_bag_playback 14 | roscpp 15 | dataset_tools 16 | std_msgs 17 | tf 18 | opencv 19 | custom_point_types 20 | tf_conversions 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/laneletsmap_generator/scripts/test_yaw_correction.sh: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/catkin_ws/src/laneletsmap_generator/scripts/test_yaw_correction.sh -------------------------------------------------------------------------------- /helper.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -z "$1" ] 4 | then 5 | echo "**************************************************************" 6 | echo "No first argument supplied, Select one of the below" 7 | echo "----------------------------------------------------" 8 | echo "" 9 | echo "build_first_image : Build the first docker image" 10 | echo "" 11 | echo "build_first_imgage : Build the second docker image" 12 | echo "" 13 | echo "create_first_container : Build the first docker container" 14 | echo "" 15 | echo "create_second_container : Build the second docker container" 16 | echo "" 17 | echo "start_first_container : Start second docker container" 18 | echo "" 19 | echo "start_second_container : Start second docker container" 20 | echo "" 21 | echo "remove_untagged : Remove the untagged images" 22 | echo "" 23 | echo "**************************************************************" 24 | echo "" 25 | echo "And pass second argument as well: 'full path of local folder of the 26 | project and expected path is: "&& pwd 27 | exit 1 28 | fi 29 | 30 | if [ -z "$2" ] 31 | then 32 | echo "No location path is supplied and expected path is: "&& pwd 33 | exit 1 34 | fi 35 | 36 | first_docker_i=validation_first_i 37 | first_docker_c=validation_first_c 38 | second_docker_i=validation_second_i 39 | second_docker_c=validation_second_c 40 | home_volume="$2":/validation 41 | #data_volume=/home/beastan/Documents/phd/dataset:/data 42 | echo "Running '$1' and '$2' options" 43 | 44 | # Start the first docker container 45 | if [[ $1 == "start_first_container" ]]; then 46 | docker start $first_docker_c 47 | docker exec -it $first_docker_c /bin/bash 48 | fi 49 | 50 | # Start the second docker container 51 | if [[ $1 == "start_second_container" ]]; then 52 | docker start $second_docker_c 53 | docker exec -it $second_docker_c /bin/bash 54 | fi 55 | 56 | # Remove untagged docker images 57 | if [[ $1 == "remove_untagged" ]]; then 58 | docker rmi -f $(docker images --filter "dangling=true" -q --no-trunc) 59 | fi 60 | 61 | # Build first docker image 62 | if [[ $1 == "build_first_image" ]]; then 63 | docker build -t $first_docker_i -f Dockerfile.first . 64 | fi 65 | 66 | # Build second docker image 67 | if [[ $1 == "build_second_image" ]]; then 68 | docker build -t $second_docker_i -f Dockerfile.second . 69 | fi 70 | 71 | # create first docker container 72 | if [[ $1 == "create_first_container" ]]; then 73 | docker run --net=host -d -v $home_volume --name $first_docker_c $first_docker_i 74 | fi 75 | 76 | # create second docker container 77 | if [[ $1 == "create_second_container" ]]; then 78 | docker run --net=host -d -v $home_volume --name $second_docker_c $second_docker_i 79 | fi 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /lz4_streamdecode_error_fix.bash: -------------------------------------------------------------------------------- 1 | sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak 2 | sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak 3 | sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h 4 | sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h 5 | -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/EnvironmentSimulator/Applications/odrplot/xodr.py: -------------------------------------------------------------------------------- 1 | 2 | import sys 3 | import csv 4 | import math 5 | import matplotlib.pyplot as plt 6 | 7 | H_SCALE = 10 8 | text_x_offset = 0 9 | text_y_offset = 0.7 10 | text_size = 7 11 | 12 | with open(sys.argv[1]) as f: 13 | reader = csv.reader(f, skipinitialspace=True) 14 | positions = list(reader) 15 | 16 | ref_x = [] 17 | ref_y = [] 18 | ref_z = [] 19 | ref_h = [] 20 | 21 | lane_x = [] 22 | lane_y = [] 23 | lane_z = [] 24 | lane_h = [] 25 | 26 | border_x = [] 27 | border_y = [] 28 | border_z = [] 29 | border_h = [] 30 | 31 | road_id = [] 32 | road_id_x = [] 33 | road_id_y = [] 34 | 35 | road_start_dots_x = [] 36 | road_start_dots_y = [] 37 | 38 | road_end_dots_x = [] 39 | road_end_dots_y = [] 40 | 41 | lane_section_dots_x = [] 42 | lane_section_dots_y = [] 43 | 44 | arrow_dx = [] 45 | arrow_dy = [] 46 | 47 | current_road_id = None 48 | current_lane_id = None 49 | current_lane_section = None 50 | new_lane_section = False 51 | 52 | for i in range(len(positions) + 1): 53 | 54 | if i < len(positions): 55 | pos = positions[i] 56 | 57 | # plot road id before going to next road 58 | if i == len(positions) or (pos[0] == 'lane' and i > 0 and current_lane_id == '0'): 59 | 60 | if current_lane_section == '0': 61 | road_id.append(int(current_road_id)) 62 | index = int(len(ref_x[-1])/3.0) 63 | h = ref_h[-1][index] 64 | road_id_x.append(ref_x[-1][index] + (text_x_offset * math.cos(h) - text_y_offset * math.sin(h))) 65 | road_id_y.append(ref_y[-1][index] + (text_x_offset * math.sin(h) + text_y_offset * math.cos(h))) 66 | road_start_dots_x.append(ref_x[-1][0]) 67 | road_start_dots_y.append(ref_y[-1][0]) 68 | if len(ref_x) > 0: 69 | arrow_dx.append(ref_x[-1][1]-ref_x[-1][0]) 70 | arrow_dy.append(ref_y[-1][1]-ref_y[-1][0]) 71 | else: 72 | arrow_dx.append(0) 73 | arrow_dy.append(0) 74 | 75 | lane_section_dots_x.append(ref_x[-1][-1]) 76 | lane_section_dots_y.append(ref_y[-1][-1]) 77 | 78 | if i == len(positions): 79 | break 80 | 81 | if pos[0] == 'lane': 82 | current_road_id = pos[1] 83 | current_lane_section = pos[2] 84 | current_lane_id = pos[3] 85 | if pos[3] == '0': 86 | ltype = 'ref' 87 | ref_x.append([]) 88 | ref_y.append([]) 89 | ref_z.append([]) 90 | ref_h.append([]) 91 | 92 | elif pos[4] == 'no-driving': 93 | ltype = 'border' 94 | border_x.append([]) 95 | border_y.append([]) 96 | border_z.append([]) 97 | border_h.append([]) 98 | else: 99 | ltype = 'lane' 100 | lane_x.append([]) 101 | lane_y.append([]) 102 | lane_z.append([]) 103 | lane_h.append([]) 104 | else: 105 | if ltype == 'ref': 106 | ref_x[-1].append(float(pos[0])) 107 | ref_y[-1].append(float(pos[1])) 108 | ref_z[-1].append(float(pos[2])) 109 | ref_h[-1].append(float(pos[3])) 110 | 111 | elif ltype == 'border': 112 | border_x[-1].append(float(pos[0])) 113 | border_y[-1].append(float(pos[1])) 114 | border_z[-1].append(float(pos[2])) 115 | border_h[-1].append(float(pos[3])) 116 | else: 117 | lane_x[-1].append(float(pos[0])) 118 | lane_y[-1].append(float(pos[1])) 119 | lane_z[-1].append(float(pos[2])) 120 | lane_h[-1].append(float(pos[3])) 121 | 122 | p1 = plt.figure(1) 123 | 124 | # plot road ref line segments 125 | for i in range(len(ref_x)): 126 | plt.plot(ref_x[i], ref_y[i], linewidth=2.0, color='#BB5555') 127 | 128 | # plot driving lanes in blue 129 | for i in range(len(lane_x)): 130 | plt.plot(lane_x[i], lane_y[i], linewidth=1.0, color='#3333BB') 131 | 132 | # plot border lanes in gray 133 | for i in range(len(border_x)): 134 | plt.plot(border_x[i], border_y[i], linewidth=1.0, color='#AAAAAA') 135 | 136 | # plot red dots indicating lane dections 137 | for i in range(len(lane_section_dots_x)): 138 | plt.plot(lane_section_dots_x[i], lane_section_dots_y[i], 'o', ms=4.0, color='#BB5555') 139 | 140 | for i in range(len(road_start_dots_x)): 141 | # plot a yellow dot at start of each road 142 | plt.plot(road_start_dots_x[i], road_start_dots_y[i], 'o', ms=5.0, color='#BBBB33') 143 | # and an arrow indicating road direction 144 | plt.arrow(road_start_dots_x[i], road_start_dots_y[i], arrow_dx[i], arrow_dy[i], width=0.1, head_width=1.0, color='#BB5555') 145 | 146 | # plot road id numbers 147 | for i in range(len(road_id)): 148 | plt.text(road_id_x[i], road_id_y[i], road_id[i], size=text_size, ha='center', va='center', color='#222222') 149 | 150 | 151 | plt.gca().set_aspect('equal', 'datalim') 152 | 153 | #p2 = plt.figure(2) 154 | #for i in range(len(z)): 155 | # ivec = [j for j in range(len(z[i]))] 156 | # plt.plot(z[i]) 157 | 158 | plt.show() 159 | 160 | 161 | -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/dat2csv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/dat2csv -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/esmini: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/esmini -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/esmini-dyn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/esmini-dyn -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/odrplot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/odrplot -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/odrviewer: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/odrviewer -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/osireceiver: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/osireceiver -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/bin/replayer: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/esmini-bin_ubuntu/bin/replayer -------------------------------------------------------------------------------- /parameters/esmini-bin_ubuntu/version.txt: -------------------------------------------------------------------------------- 1 | ESMINI_GIT_REV="39f31f9" 2 | ESMINI_GIT_TAG="v2.20.5" 3 | ESMINI_GIT_BRANCH="tags/v2.20.5^0" 4 | ESMINI_BUILD_VERSION="2.20.5.build.2190" 5 | -------------------------------------------------------------------------------- /parameters/plot.py: -------------------------------------------------------------------------------- 1 | import os 2 | import matplotlib 3 | 4 | # For the error: Exception ignored in: > Traceback (most recent call last): 5 | matplotlib.use('Agg') 6 | 7 | import matplotlib.pyplot as plt 8 | import json 9 | 10 | ''' 11 | loc = '/model/parameters/' 12 | _dir = os.listdir(loc) 13 | evaluation = { 14 | 'cut-in': { 15 | 'param_ego_speed_init': [], 16 | 'param_adv_speed_init': [], 17 | 'param_start_diff': [], 18 | 'param_cutin_start_dist': [], 19 | 'param_cutin_time': [], 20 | 'param_ego_to_cutin_dist': [], 21 | 'param_ego_to_cutin_time': [], 22 | 'param_adv_to_cutin_time': [], 23 | 'param_adv_to_cutin_dist': [] 24 | }, 25 | 'cut-out':{ 26 | 'param_ego_speed_init': [], 27 | 'param_adv_speed_init': [], 28 | 'param_start_diff': [], 29 | 'param_cutout_start_dist': [], 30 | 'param_cutout_time': [], 31 | 'param_ego_to_cutout_dist': [], 32 | 'param_ego_to_cutout_time': [], 33 | 'param_adv_to_cutout_time': [], 34 | 'param_adv_to_cutout_dist': [] 35 | 36 | } 37 | } 38 | for _file in _dir: 39 | base = os.path.basename(_file) 40 | fileDetails = os.path.splitext(base) 41 | if fileDetails[1] == '.cutin' or fileDetails[1] == '.cutout': 42 | print("Processing the file: {}".format(_file)) 43 | with open(_file) as f: 44 | data = json.loads(f.read()) 45 | _type = data['type'] 46 | if _type == 'cutin': 47 | for param in data['parameter']: 48 | evaluation['cut-in']['param_ego_speed_init'].append(param['param_ego_speed_init']) 49 | evaluation['cut-in']['param_adv_speed_init'].append(param['param_adv_speed_init']) 50 | evaluation['cut-in']['param_start_diff'].append(param['param_start_diff']) 51 | evaluation['cut-in']['param_cutin_start_dist'].append(param['param_cutin_start_dist']) 52 | evaluation['cut-in']['param_cutin_time'].append(param['param_cutin_time']) 53 | evaluation['cut-in']['param_ego_to_cutin_dist'].append(param['param_ego_to_cutin_dist']) 54 | evaluation['cut-in']['param_adv_to_cutin_time'].append(param['param_adv_to_cutin_time']) 55 | evaluation['cut-in']['param_adv_to_cutin_dist'].append(param['param_adv_to_cutin_dist']) 56 | 57 | if _type == 'cutout': 58 | for param in data['parameter']: 59 | evaluation['cut-out']['param_ego_speed_init'].append(param['param_ego_speed_init']) 60 | evaluation['cut-out']['param_adv_speed_init'].append(param['param_adv_speed_init']) 61 | evaluation['cut-out']['param_start_diff'].append(param['param_start_diff']) 62 | evaluation['cut-out']['param_cutout_start_dist'].append(param['param_cutout_start_dist']) 63 | evaluation['cut-out']['param_cutout_time'].append(param['param_cutout_time']) 64 | evaluation['cut-out']['param_ego_to_cutout_dist'].append(param['param_ego_to_cutout_dist']) 65 | evaluation['cut-out']['param_adv_to_cutout_time'].append(param['param_adv_to_cutout_time']) 66 | evaluation['cut-out']['param_adv_to_cutout_dist'].append(param['param_adv_to_cutout_dist']) 67 | 68 | 69 | 70 | print(evaluation) 71 | 72 | for key in evaluation['cut-in'].keys(): 73 | path_to_save_dir = '/model/plots/param_plots/' 74 | name = path_to_save_dir+key 75 | plt.figure() 76 | hist = plt.hist(evaluation['cut-in'][key], bins='auto', color='#B45743', rwidth=0.25, normed=False) 77 | plt.ylabel("Frequency") 78 | plt.xlabel(key) 79 | plt.savefig(name) 80 | plt.close() 81 | ''' 82 | #-----------------------------------esmini data------------------------------ 83 | path_to_save_dir = 'plots/' 84 | _file = 'esmini_real_plot_data.json' 85 | with open(_file) as f: 86 | data = json.loads(f.read()) 87 | 88 | print("total number of scenarios: {}".format(len(data['data']))) 89 | 90 | ''' 91 | no = 7 92 | key = 'speed' 93 | adversary_esmini1 = data['data'][no]['adversary_esmini'] 94 | adversary_real1 = data['data'][no]['adversary_real'] 95 | ego_esmini1 = data['data'][no]['ego_esmini'] 96 | ego_real1 = data['data'][no]['ego_real'] 97 | esmini1 = data['data'][no]['esmini'] 98 | real1 = data['data'][no]['real'] 99 | ''' 100 | ''' 101 | no = 0 102 | key = 'speed' 103 | adversary_esmini2 = data['data'][no]['adversary_esmini'] 104 | adversary_real2 = data['data'][no]['adversary_real'] 105 | ego_esmini2 = data['data'][no]['ego_esmini'] 106 | ego_real2 = data['data'][no]['ego_real'] 107 | esmini2 = data['data'][no]['esmini'] 108 | real2 = data['data'][no]['real'] 109 | ''' 110 | 111 | no = 0 112 | key = 'speed' 113 | adversary_esmini3 = data['data'][no]['adversary_esmini'] 114 | adversary_real3 = data['data'][no]['adversary_real'] 115 | ego_esmini3 = data['data'][no]['ego_esmini'] 116 | ego_real3 = data['data'][no]['ego_real'] 117 | 118 | adversary_esmini_sec3 = data['data'][no]['adversary_esmini_sec'] 119 | adversary_real_sec3 = data['data'][no]['adversary_real_sec'] 120 | ego_esmini_sec3 = data['data'][no]['ego_esmini_sec'] 121 | ego_real_sec3 = data['data'][no]['ego_real_sec'] 122 | 123 | name = path_to_save_dir+"test" 124 | plt.figure() 125 | plt.xlabel("second") 126 | plt.ylabel(key+" (km/hr)") 127 | plt.xlim([0, 11]) 128 | #plt.plot(adversary_real1['sec'], adversary_real1[key]) 129 | #plt.plot(adversary_esmini1['sec'], adversary_esmini1[key], '--') 130 | #plt.plot(adversary_real2['sec'], adversary_real2[key]) 131 | #plt.plot(adversary_esmini2['sec'], adversary_esmini2[key], '--') 132 | plt.plot(adversary_real_sec3['sec'], adversary_real_sec3[key]) 133 | plt.plot(adversary_esmini_sec3['sec'], adversary_esmini_sec3[key], '--') 134 | plt.legend(['Real-world', 'OpenSCENARIO']) 135 | #plt.legend(loc="upper right", prop={'size': 8}, labelspacing=0.1, bbox_to_anchor=(1.125,1)) 136 | plt.savefig(name) 137 | plt.close() 138 | 139 | name = path_to_save_dir+"test_ego" 140 | plt.figure() 141 | plt.xlabel("second") 142 | plt.ylabel(key+' (km/hr)') 143 | #plt.ylim([0, 20]) 144 | plt.plot(ego_real_sec3['sec'], ego_real_sec3[key], label='real') 145 | plt.plot(ego_esmini_sec3['sec'], ego_esmini_sec3[key], label='generated') 146 | plt.legend(loc="upper right", prop={'size': 8}, labelspacing=0.1, bbox_to_anchor=(1.125,1)) 147 | plt.savefig(name) 148 | plt.close() 149 | 150 | 151 | adversary_esmini_milli = data['data'][no]['adversary_esmini_milli'] 152 | adversary_real_milli = data['data'][no]['adversary_real_milli'] 153 | 154 | sec = [] 155 | speed = [] 156 | for index in range(len(adversary_esmini_sec3['speed'])): 157 | sec.append(index*10) 158 | speed.append(adversary_esmini_sec3['speed'][index]) 159 | 160 | sec_real = [] 161 | speed_real = [] 162 | for index in range(len(adversary_real_milli['speed'])): 163 | sec_real.append(index) 164 | speed_real.append(adversary_real_milli['speed'][index]) 165 | 166 | name = path_to_save_dir+"test_other_milli" 167 | plt.figure() 168 | plt.xlabel("second") 169 | plt.ylabel(key+' (km/hr)') 170 | #plt.ylim([0, 20]) 171 | #plt.plot(adversary_esmini_milli['speed'], label='generated') 172 | plt.plot(adversary_esmini_milli['sec'], adversary_esmini_milli['speed'], label='real') 173 | #plt.legend(loc="upper right", prop={'size': 8}, labelspacing=0.1, bbox_to_anchor=(1.125,1)) 174 | plt.legend(['Real-world', 'OpenSCENARIO']) 175 | plt.savefig(name) 176 | 177 | key = 's' 178 | name = path_to_save_dir+"test_s_t" 179 | plt.figure() 180 | plt.xlabel("t, lateral displacement") 181 | plt.ylabel('s, longitudinal displacement') 182 | plt.xlim([-1, -5]) 183 | #plt.ylim([100, 130]) 184 | #plt.plot(adversary_real1['sec'], adversary_real1[key]) 185 | #plt.plot(adversary_esmini1['sec'], adversary_esmini1[key], '--') 186 | #plt.plot(adversary_real2['sec'], adversary_real2[key]) 187 | #plt.plot(adversary_esmini2['sec'], adversary_esmini2[key], '--') 188 | plt.plot(adversary_real3['t'], adversary_real3[key]) 189 | plt.plot(adversary_esmini3['t'], adversary_esmini3[key], '--') 190 | plt.legend(['Real-world', 'OpenSCENARIO']) 191 | #plt.legend(loc="upper right", prop={'size': 8}, labelspacing=0.1, bbox_to_anchor=(1.125,1)) 192 | plt.savefig(name) 193 | plt.close() 194 | 195 | 196 | 197 | ''' 198 | name = path_to_save_dir+"test_rss" 199 | key = 'rss' 200 | plt.figure() 201 | plt.xlabel("second") 202 | plt.ylabel(key) 203 | plt.ylim([10, 40]) 204 | #plt.plot(esmini1['sec'], esmini1[key], '--') 205 | #plt.plot(real1['sec'], real1[key]) 206 | #plt.plot(esmini2['sec'], esmini2[key], '--') 207 | #plt.plot(real2['sec'], real2[key]) 208 | plt.plot(esmini3['sec'], esmini3[key], '--') 209 | plt.plot(real3['sec'], real3[key]) 210 | plt.legend(['RSS computed in OpenSCENARIO', 'RSS computed in real data']) 211 | #plt.legend(loc="upper right", prop={'size': 8}, labelspacing=0.1, bbox_to_anchor=(1.125,1)) 212 | plt.savefig(name) 213 | plt.close() 214 | ''' 215 | 216 | 217 | 218 | -------------------------------------------------------------------------------- /parameters/plots/comparison.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/comparison.psd -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210608_054523.0/test_ego_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210608_054523.0/test_ego_full.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210608_054523.0/test_ego_half.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210608_054523.0/test_ego_half.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210608_054523.0/test_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210608_054523.0/test_full.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210608_054523.0/test_half.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210608_054523.0/test_half.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210608_054523.0/test_s_t_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210608_054523.0/test_s_t_full.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210608_054523.0/test_s_t_half.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210608_054523.0/test_s_t_half.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_2.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_all.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_all.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_ego_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_ego_2.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_ego_all.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_ego_all.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_ego_half.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_ego_half.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_half.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_half.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_s_t_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_s_t_2.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_s_t_all.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_s_t_all.png -------------------------------------------------------------------------------- /parameters/plots/gate_south-end_north-end_20210615_043348.0/test_s_t_half.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/gate_south-end_north-end_20210615_043348.0/test_s_t_half.png -------------------------------------------------------------------------------- /parameters/plots/rss_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/rss_comparison.png -------------------------------------------------------------------------------- /parameters/plots/speed_comaprison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/speed_comaprison.png -------------------------------------------------------------------------------- /parameters/plots/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/test.png -------------------------------------------------------------------------------- /parameters/plots/test_ego.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/test_ego.png -------------------------------------------------------------------------------- /parameters/plots/test_other_milli.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/test_other_milli.png -------------------------------------------------------------------------------- /parameters/plots/test_rss.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/test_rss.png -------------------------------------------------------------------------------- /parameters/plots/test_s_t.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/test_s_t.png -------------------------------------------------------------------------------- /parameters/plots/trajectory_2_sec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/trajectory_2_sec.png -------------------------------------------------------------------------------- /parameters/plots/trajectory_5_sec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/trajectory_5_sec.png -------------------------------------------------------------------------------- /parameters/plots/trajectory_all_sec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dkarunakaran/scenario_extraction_framework/9099608029d5cd2048046f28f5d52df34b8222ef/parameters/plots/trajectory_all_sec.png -------------------------------------------------------------------------------- /parameters/rss.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Copyright (c) 2018-2019 Intel Corporation 4 | # 5 | # This work is licensed under the terms of the MIT license. 6 | # For a copy, see . 7 | # 8 | 9 | class RSS: 10 | 11 | # Below ones are default parameter from the RSS library: https://intel.github.io/ad-rss-lib/documentation/Main.html#Section::ParameterDiscussion 12 | # RESPONSE TIME: 13 | # common sense value for human drivers is about 2 seconds, but this can be lower for AV. 14 | # LONGITUDINAL ACCELERATION: 15 | # Finding acceleration values for RSS is more complicated. These values may vary from ODD to ODD. 16 | # The minimum deceleration values must not exceed normal human driving behaviour. 17 | # Also big difference between the minimum and maximum acceleration will lead to defencisve driving. That leads to not participating in the dense traffic. 18 | # maximum acceleration a low speeds a standard car is in the range of 3.4 m/s4 to 7 m/s2 19 | # Required safety distance for the car at 50 km/h is brake_min = 4 m/s2 and brake_max = 8 m/s2 20 | # Rule of thump in german driving schools are brake_min = 4 m/s2 and brake_max = 8 m/s2 21 | # We are only considering one way and longitudinal safe distance. 22 | 23 | def __init__(self): 24 | # V_ego in m/s 25 | # V_other in m/s 26 | 27 | self.res_ego = 1 #s 28 | self.res_other = 2 #s 29 | self.accel_max = 3.5 #m/s2 30 | self.brake_min = 4 #m/s2 31 | self.break_max = 8 #m/s2 32 | #lat_break_min = 0.8 #m/s2 33 | #lat_accel_max = 0.2 #m/s2 34 | #margin = 10 #cm 35 | 36 | self.vb_vel_margin = 0.25 #m/s 37 | 38 | 39 | def calculate_rss_safe_dist(self, V_ego, V_other): 40 | long_d_min = self.long_safe_dist(V_ego, V_other) 41 | 42 | return long_d_min 43 | 44 | def long_safe_dist(self, V_ego, V_other): 45 | first_term = V_ego*self.res_ego 46 | second_term = ((self.accel_max*self.res_ego**2)/2) 47 | third_term = (((V_ego+(self.res_ego*self.accel_max))**2)/(2*self.brake_min)) 48 | fourth_term = ((V_other**2)/(2*self.break_max)) 49 | d_min = first_term+second_term+third_term-fourth_term 50 | 51 | return d_min 52 | 53 | 54 | -------------------------------------------------------------------------------- /parameters/scenario_data/gate_south-end_north-end_20210615_043348.0.laneletToOD: -------------------------------------------------------------------------------- 1 | { 2 | "data": [ 3 | { 4 | "laneWidth": 3.116338267278657, 5 | "length": 25.05336197320479, 6 | "mDiff": 0.0, 7 | "noLanes": 3, 8 | "sStart": 0.0 9 | }, 10 | { 11 | "laneWidth": 3.094918922841961, 12 | "length": 25.055182516702143, 13 | "mDiff": -6.340947731347745e-05, 14 | "noLanes": 3, 15 | "sStart": 25.05336197320479 16 | }, 17 | { 18 | "laneWidth": 3.06526833943475, 19 | "length": 25.33511902105067, 20 | "mDiff": -0.00010233610034203147, 21 | "noLanes": 3, 22 | "sStart": 50.10854448990693 23 | }, 24 | { 25 | "laneWidth": 3.0891135366011344, 26 | "length": 25.068557347216988, 27 | "mDiff": 1.1683600817504252e-05, 28 | "noLanes": 3, 29 | "sStart": 75.4436635109576 30 | }, 31 | { 32 | "laneWidth": 3.0977987315676043, 33 | "length": 24.977435046468813, 34 | "mDiff": 0.00039031779930076934, 35 | "noLanes": 3, 36 | "sStart": 100.53203522835193 37 | }, 38 | { 39 | "laneWidth": 3.085780106419556, 40 | "length": 25.216451371421705, 41 | "mDiff": 7.393386943334171e-05, 42 | "noLanes": 3, 43 | "sStart": 125.51456204371397 44 | }, 45 | { 46 | "laneWidth": 3.104745205260117, 47 | "length": 25.264801799062788, 48 | "mDiff": 1.9871264410005907e-05, 49 | "noLanes": 3, 50 | "sStart": 150.73101341513566 51 | }, 52 | { 53 | "laneWidth": 3.0754229495615863, 54 | "length": 25.098562035254623, 55 | "mDiff": -0.00017059337277950667, 56 | "noLanes": 3, 57 | "sStart": 175.99581521419844 58 | }, 59 | { 60 | "laneWidth": 3.1855594665389493, 61 | "length": 25.15114114585224, 62 | "mDiff": 0.00018229882746700722, 63 | "noLanes": 3, 64 | "sStart": 201.09437724945306 65 | }, 66 | { 67 | "laneWidth": 3.1679737900346168, 68 | "length": 25.05164804971863, 69 | "mDiff": -8.475934558279005e-06, 70 | "noLanes": 3, 71 | "sStart": 226.24540154701415 72 | }, 73 | { 74 | "laneWidth": 3.185720541387242, 75 | "length": 25.093785248563673, 76 | "mDiff": -0.0001257202444822087, 77 | "noLanes": 3, 78 | "sStart": 251.2970495967328 79 | }, 80 | { 81 | "laneWidth": 3.184253088290679, 82 | "length": 25.013008198121202, 83 | "mDiff": -0.00015593041790005295, 84 | "noLanes": 3, 85 | "sStart": 276.39083484529647 86 | }, 87 | { 88 | "laneWidth": 3.1725334727677055, 89 | "length": 25.041612625994787, 90 | "mDiff": 0.00012514718097336613, 91 | "noLanes": 3, 92 | "sStart": 301.4038430434177 93 | }, 94 | { 95 | "laneWidth": 3.1999058033616223, 96 | "length": 25.10881084605801, 97 | "mDiff": 0.00031154953663435, 98 | "noLanes": 3, 99 | "sStart": 326.44545566941247 100 | }, 101 | { 102 | "laneWidth": 3.193744252459163, 103 | "length": 25.09949772425824, 104 | "mDiff": 0.00032899506693203343, 105 | "noLanes": 3, 106 | "sStart": 351.5542665154705 107 | }, 108 | { 109 | "laneWidth": 3.2134838999988644, 110 | "length": 25.161259622194926, 111 | "mDiff": 0.00016336769988309233, 112 | "noLanes": 3, 113 | "sStart": 376.64036834354715 114 | }, 115 | { 116 | "laneWidth": 3.1576038989339477, 117 | "length": 25.07045385874037, 118 | "mDiff": -3.659887320696421e-05, 119 | "noLanes": 3, 120 | "sStart": 401.7827058279112 121 | }, 122 | { 123 | "laneWidth": 3.0876464235656216, 124 | "length": 25.141591303194364, 125 | "mDiff": 0.00036894979648577597, 126 | "noLanes": 3, 127 | "sStart": 426.8333271017789 128 | }, 129 | { 130 | "laneWidth": 3.077298398855662, 131 | "length": 25.060049547098373, 132 | "mDiff": 7.112352793518414e-05, 133 | "noLanes": 3, 134 | "sStart": 451.9712276241678 135 | }, 136 | { 137 | "laneWidth": 3.2719701453332632, 138 | "length": 25.07936387351428, 139 | "mDiff": 0.00010815395637629038, 140 | "noLanes": 3, 141 | "sStart": 477.0288176228415 142 | }, 143 | { 144 | "laneWidth": 3.2149604722210285, 145 | "length": 25.10651369961354, 146 | "mDiff": 6.815375962343183e-05, 147 | "noLanes": 3, 148 | "sStart": 502.1015033834529 149 | }, 150 | { 151 | "laneWidth": 3.19784953837859, 152 | "length": 25.03421544173407, 153 | "mDiff": 3.158442086045703e-05, 154 | "noLanes": 3, 155 | "sStart": 527.2067319280446 156 | }, 157 | { 158 | "laneWidth": 3.2427877948264983, 159 | "length": 25.093721249941172, 160 | "mDiff": 2.1846726702225844e-05, 161 | "noLanes": 3, 162 | "sStart": 552.2346710510915 163 | }, 164 | { 165 | "laneWidth": 3.203098947191883, 166 | "length": 24.9677000063009, 167 | "mDiff": 4.551191001822761e-05, 168 | "noLanes": 3, 169 | "sStart": 577.3538936282152 170 | }, 171 | { 172 | "laneWidth": 3.1896873501973317, 173 | "length": 25.058132986460684, 174 | "mDiff": 0.0002863141892044837, 175 | "noLanes": 3, 176 | "sStart": 602.3526998907091 177 | }, 178 | { 179 | "laneWidth": 3.1433683088551323, 180 | "length": 25.052024248433337, 181 | "mDiff": -0.000200735327585342, 182 | "noLanes": 3, 183 | "sStart": 627.4082350964895 184 | }, 185 | { 186 | "laneWidth": 3.084297230977119, 187 | "length": 25.29098932021847, 188 | "mDiff": 4.716243919498098e-05, 189 | "noLanes": 3, 190 | "sStart": 652.4595789452416 191 | }, 192 | { 193 | "laneWidth": 3.045419396173098, 194 | "length": 25.24958539544884, 195 | "mDiff": -5.01180576003301e-05, 196 | "noLanes": 3, 197 | "sStart": 677.7550790769071 198 | } 199 | ], 200 | "file": "/data/ibeo/others/gate_south-end_north-end_20210615_043348.0.bag", 201 | "noOfLanesStart": 3 202 | } 203 | -------------------------------------------------------------------------------- /process_bags.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -z "$1" ] 4 | then 5 | echo "**************************************************************" 6 | echo "No folder location is given" 7 | exit 1 8 | fi 9 | 10 | source 'catkin_ws/devel/setup.bash' 11 | 12 | folder_loc="$1/*" 13 | for file in $folder_loc 14 | do 15 | echo "____________________________________________________" 16 | echo "processing:$file" 17 | roslaunch /model/catkin_ws/launch/validation_dataset.launch bag_file:=$file output_image:=/model/images/new_image.png laneletsmap_file:=/model/map.osm 18 | 19 | done 20 | --------------------------------------------------------------------------------